diff --git a/ThriftyTest/autogen.sh b/ThriftyTest/autogen.sh new file mode 100755 index 00000000..bfe038b3 --- /dev/null +++ b/ThriftyTest/autogen.sh @@ -0,0 +1,14 @@ +#!/bin/bash +for fullpath in autons/*; do + if [[ -d "$fullpath" ]]; then + echo "[ INFO ] Skipping directory $fullpath" + continue + fi + filename=`basename $fullpath` + if [[ $filename = "auto-builder.py" ]]; then + continue + fi + name="${filename%.*}" # bash magic from stack overflow + auto_gen $fullpath > src/main/deploy/pathplanner/autos/$name.auto + echo "[ CREATE ] $name.auto" +done diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt index 56c8911b..610dbaeb 100644 --- a/ThriftyTest/autons/center.txt +++ b/ThriftyTest/autons/center.txt @@ -1,9 +1,10 @@ Align H L4 -Align GH & Upper Algae +Upper Algae ? Align GH Align Barge Center -Net -Upper Algae & Align IJ +Net Prep + Net +Upper Algae ? Align IJ Align Barge Left -Net -Align LIntake +Net Prep + Net +Stow +Align LIntake \ No newline at end of file diff --git a/ThriftyTest/autons/center2.txt b/ThriftyTest/autons/center2.txt deleted file mode 100644 index b8dc1060..00000000 --- a/ThriftyTest/autons/center2.txt +++ /dev/null @@ -1,12 +0,0 @@ -Align H -L4 -Align Inside Lollipop & High Ground Algae -Beeline Barge Center -Net -Align Outside Lollipop & High Ground Algae -Beeline Barge Left -Net -Align IJ & High Ground Algae -Align Barge Left -Net -Beeline LIntake diff --git a/ThriftyTest/autons/left4.txt b/ThriftyTest/autons/left.txt similarity index 100% rename from ThriftyTest/autons/left4.txt rename to ThriftyTest/autons/left.txt diff --git a/ThriftyTest/autons/lollipop.txt b/ThriftyTest/autons/lollipop.txt new file mode 100644 index 00000000..ea71e7dd --- /dev/null +++ b/ThriftyTest/autons/lollipop.txt @@ -0,0 +1,8 @@ +Align G + L4 +Prep Lollipop Inside + (Lollipop Intake ? Lollipop Inside) +Beeline Barge Center +Net Prep + Net +Prep Lollipop Outside + (Lollipop Intake ? Lollipop Outside) +Beeline Barge Left +Net Prep + Net +Beeline LIntake diff --git a/ThriftyTest/autons/right4.txt b/ThriftyTest/autons/right.txt similarity index 100% rename from ThriftyTest/autons/right4.txt rename to ThriftyTest/autons/right.txt diff --git a/ThriftyTest/autons/steal-and-shoot.txt b/ThriftyTest/autons/steal-and-shoot.txt new file mode 100644 index 00000000..7092b0bb --- /dev/null +++ b/ThriftyTest/autons/steal-and-shoot.txt @@ -0,0 +1,6 @@ +Align G + L4 +Prep Steal Inside + (Lollipop Intake ? Steal Inside) +Shoot Outside + Eject Algae +Prep Steal Outside + (Lollipop Intake ? Steal Outside) +Shoot Inside + Eject Algae +Beeline RIntake diff --git a/ThriftyTest/autons/steal.txt b/ThriftyTest/autons/steal.txt new file mode 100644 index 00000000..34b81f60 --- /dev/null +++ b/ThriftyTest/autons/steal.txt @@ -0,0 +1,8 @@ +Align G + L4 +Prep Steal Inside + (Lollipop Intake ? Steal Inside) +Beeline Barge Center +Net Prep + Net +Prep Steal Outside + (Lollipop Intake ? Steal Outside) +Beeline Barge Left +Net Prep + Net +Beeline LIntake diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index c7dcf2a3..3c8c55b5 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -25,6 +25,7 @@ "/SmartDashboard/Elevator/Subsystem": "Subsystem", "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/PhotonAlerts": "Alerts", + "/SmartDashboard/Prep/Set Angle": "Command", "/SmartDashboard/Prep/Set Center": "Command", "/SmartDashboard/Prep/Set Left": "Command", "/SmartDashboard/Prep/Set Right": "Command", @@ -34,6 +35,7 @@ "/SmartDashboard/Test/Drive To Center": "Command", "/SmartDashboard/Test/Elevator Up": "Command", "/SmartDashboard/Test/Enter test state": "Command", + "/SmartDashboard/Test/FIRE!!!": "Command", "/SmartDashboard/Test/Follow Algae": "Command", "/SmartDashboard/Test/L4": "Command", "/SmartDashboard/Test/LowReefAlgaeIntake": "Command", @@ -54,12 +56,7 @@ "visible": true } }, - "/SmartDashboard/Prep/Set Left": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Prep/Set Right": { + "/SmartDashboard/Command Scheduler": { "window": { "visible": true } @@ -68,6 +65,7 @@ "Algae": { "image": "../resources/algae.jpg", "length": 0.800000011920929, + "selectable": false, "width": 0.800000011920929 }, "FF": { @@ -96,12 +94,12 @@ 255.0 ], "length": 0.9399999976158142, + "selectable": false, "style": "Box/Image", "width": 0.9399999976158142 }, "Tracked Object": { "arrows": false, - "image": "/Users/rekrab/hackbots/2025_Reefscape/resources/algae.jpg", "length": 0.800000011920929, "width": 0.800000011920929 }, @@ -223,9 +221,6 @@ "open": true } }, - "Vision": { - "open": true - }, "open": true }, "photonvision": { diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto deleted file mode 100644 index bf2049f9..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto +++ /dev/null @@ -1,202 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align J" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Align LIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align K" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline LIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align L" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline LIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align A" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "named", - "data": { - "name": "Recalibrate" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto deleted file mode 100644 index 62534586..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto +++ /dev/null @@ -1,202 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align E" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Align RIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align D" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline RIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align C" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline RIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align B" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "named", - "data": { - "name": "Recalibrate" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 273f4bd7..aadd0ce8 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -17,19 +17,19 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { "type": "named", "data": { - "name": "Align GH" + "name": "Upper Algae" } }, { "type": "named", "data": { - "name": "Lower Algae" + "name": "Align GH" } } ] @@ -42,13 +42,26 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "Net" + "commands": [ + { + "type": "named", + "data": { + "name": "Net" + } + }, + { + "type": "named", + "data": { + "name": "Net Prep" + } + } + ] } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { @@ -72,10 +85,29 @@ "name": "Align Barge Left" } }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Net Prep" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + }, { "type": "named", "data": { - "name": "Net" + "name": "Stow" } }, { diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto deleted file mode 100644 index 7cd04d27..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto +++ /dev/null @@ -1,111 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align H" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align GH" - } - }, - { - "type": "named", - "data": { - "name": "Lower Algae" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "HBarge" - } - }, - { - "type": "named", - "data": { - "name": "Beeline Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - }, - { - "type": "path", - "data": { - "pathName": "BargeIJ" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Upper Algae" - } - }, - { - "type": "named", - "data": { - "name": "Align IJ" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "IJBarge2" - } - }, - { - "type": "named", - "data": { - "name": "Beeline Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - }, - { - "type": "path", - "data": { - "pathName": "Barge2Backup" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto new file mode 100644 index 00000000..bfd53739 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Prep Lollipop Inside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Lollipop Inside"}}]}},{"type":"named","data":{"name":"Beeline Barge Center"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Prep Lollipop Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Lollipop Outside"}}]}},{"type":"named","data":{"name":"Beeline Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto new file mode 100644 index 00000000..61fcaf2b --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Prep Steal Inside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Inside"}}]}},{"type":"named","data":{"name":"Beeline Barge Center"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Prep Steal Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Outside"}}]}},{"type":"named","data":{"name":"Beeline Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/left.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/left.auto new file mode 100644 index 00000000..d8f477a3 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/left.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align J"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Align LIntake"}}]}},{"type":"named","data":{"name":"Align K"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},{"type":"named","data":{"name":"Align L"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},{"type":"named","data":{"name":"Align A"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Recalibrate"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/right.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/right.auto new file mode 100644 index 00000000..17ec859f --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/right.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align E"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Align RIntake"}}]}},{"type":"named","data":{"name":"Align D"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline RIntake"}}]}},{"type":"named","data":{"name":"Align C"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline RIntake"}}]}},{"type":"named","data":{"name":"Align B"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Recalibrate"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto new file mode 100644 index 00000000..424baac8 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Prep Steal Inside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Inside"}}]}},{"type":"named","data":{"name":"Shoot Outside"}},{"type":"named","data":{"name":"Eject Algae"}},{"type":"named","data":{"name":"Prep Steal Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Outside"}}]}},{"type":"named","data":{"name":"Shoot Inside"}},{"type":"named","data":{"name":"Eject Algae"}},{"type":"named","data":{"name":"Beeline RIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index b147a5f5..807e453d 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -40,14 +40,27 @@ public static class FieldConstants { public static final Pose2d kIJ = new Pose2d(5.155, 5.194, Rotation2d.fromDegrees(-120)); public static final Pose2d kBargeFromCenter = - new Pose2d(7.459, 4.717, Rotation2d.fromDegrees(21.0)); - public static final Pose2d kBargeFromLeft = new Pose2d(7.5, 6.6, Rotation2d.fromDegrees(21)); + new Pose2d(7.4, 4.717, Rotation2d.fromDegrees(21.0)); + public static final Pose2d kBargeFromLeft = new Pose2d(7.4, 6.6, Rotation2d.fromDegrees(21)); public static final Pose2d kStartCenter = new Pose2d(7.076, 3.991, Rotation2d.kPi); public static final Pose2d kStartRight = new Pose2d(7.076, 1.2, Rotation2d.kPi); public static final Pose2d kStartLeft = new Pose2d(7.076, 6.6, Rotation2d.kPi); public static final Rotation2d kStartHeading = Rotation2d.kPi; // facing our alliance + + public static final Pose2d kInsideStealPrep = new Pose2d(7.642, 1.226, Rotation2d.kZero); + public static final Pose2d kOutsideStealPrep = new Pose2d(7.642, 2.4, Rotation2d.kZero); + public static final Pose2d kInsideSteal = new Pose2d(8, 1.226, Rotation2d.kZero); + public static final Pose2d kOutsideSteal = new Pose2d(8, 2.4, Rotation2d.kZero); + + public static final Pose2d kOutsideLollipopPrep = new Pose2d(7.642, 6.7, Rotation2d.kZero); + public static final Pose2d kInsideLollipopPrep = new Pose2d(7.642, 5.5, Rotation2d.kZero); + public static final Pose2d kOutsideLollipop = new Pose2d(8, 6.7, Rotation2d.kZero); + public static final Pose2d kInsideLollipop = new Pose2d(8, 5.5, Rotation2d.kZero); + + public static final Pose2d kInsideShoot = new Pose2d(6, 1.2, Rotation2d.kPi); + public static final Pose2d kOutsideShoot = new Pose2d(6, 2.4, Rotation2d.kPi); } public static final class StateSpaceConstants { diff --git a/ThriftyTest/src/main/java/frc/robot/Robot.java b/ThriftyTest/src/main/java/frc/robot/Robot.java index 506b267d..997603ad 100644 --- a/ThriftyTest/src/main/java/frc/robot/Robot.java +++ b/ThriftyTest/src/main/java/frc/robot/Robot.java @@ -35,6 +35,7 @@ public Robot() { m_loopTimer = new LoopTimer("Robot"); m_ologger = new OnboardLogger("Robot"); m_ologger.registerDouble("Battery Voltage", RobotController::getBatteryVoltage); + m_ologger.registerBoolean("FMS Connected", DriverStation::isFMSAttached); } @Override diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index fbf8a519..21160d96 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -21,6 +21,7 @@ public static class Driver { public static final int kLeftAlign = 4; public static final double deadband = 0.01; + public static final boolean kEnableSmartButton = true; } public static class Operator { @@ -29,9 +30,9 @@ public static class Operator { public static final int kL3 = 90; public static final int kL4 = 0; - public static final int kSecondaryL1 = 14; + public static final int kStowClimb = 14; - public static final int kEjectCoral = Button.kL2.value; + public static final int kEjectCoral = Button.kL1.value; public static final int kLeftAlign = Button.kSquare.value; public static final int kRightAlign = Button.kCircle.value; @@ -41,7 +42,7 @@ public static class Operator { public static final int kGroundAlgaeIntake = 180; public static final int kProcessor = 90; public static final int kHighGroundAlgaeIntake = 270; - public static final int kNet = 0; + public static final int kNetPrep = 0; public static final int kAlgae = Button.kR2.value; public static final int kRaiseClimb = Button.kCreate.value; @@ -49,12 +50,12 @@ public static class Operator { public static final int kStow = Button.kPS.value; - public static final int kCoralIntake = Button.kL1.value; - public static final int kCalibrateElevator = 15; public static final int kRightFunnel = 11; public static final int kLeftFunnel = 12; + public static final int kScoreNet = Button.kL2.value; + public static final int kCoralScore = Button.kR1.value; } } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java index bed9c7aa..ff8ef73b 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java @@ -6,6 +6,7 @@ import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.Superstructure.Subsystems; +import frc.robot.superstructure.states.ManualAlgaeEject; import frc.robot.superstructure.states.SeedAngle; import frc.robot.superstructure.states.SeedPose; import frc.robot.superstructure.states.Stow; @@ -24,6 +25,7 @@ public Command build(Subsystems subsystems) { return subsystems.elevator().go(ElevatorState.L4); } })); + SmartDashboard.putData("Test/FIRE!!!", superstructure.enter(new ManualAlgaeEject())); SmartDashboard.putData("Test/Follow Algae", superstructure.enter(new TrackAlgae())); } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java index 1799ff09..8d0edb05 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java @@ -1,12 +1,13 @@ package frc.robot.binding; import java.util.function.DoubleSupplier; -import edu.wpi.first.math.geometry.Rotation2d; +import com.therekrab.autopilot.APTarget; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.FieldConstants; import frc.robot.binding.BindingConstants.Driver; -import com.therekrab.autopilot.APTarget; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.Align; import frc.robot.superstructure.states.DeferredAlign; @@ -35,10 +36,18 @@ public void bind(Superstructure superstructure) { superstructure.setDrive(superstructure.enterWithoutProxy(new TeleopDrive(m_x, m_y, m_rot))); m_resetHeading.onTrue(superstructure.enter(new HeadingReset())); - // m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align( - // new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2)))); - m_smartAlign.and(superstructure.holdingAlgae().negate()).whileTrue(superstructure.enter( - new DeferredAlign(AlignLocation.Center))); + + Command smartWithAlgae; + if (Driver.kEnableSmartButton) { + smartWithAlgae = superstructure.enter(new Align(new APTarget(FieldConstants.k_processor))); + } else { + smartWithAlgae = Commands.none(); + } + m_smartAlign.whileTrue(Commands.either( + smartWithAlgae, + superstructure.enter(new DeferredAlign(AlignLocation.Center)), + superstructure.holdingAlgae())); + m_leftAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); m_rightAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Right))); } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index ce786668..5b70f024 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -2,21 +2,26 @@ import static edu.wpi.first.units.Units.Meters; import com.pathplanner.lib.auto.NamedCommands; +import com.therekrab.autopilot.APTarget; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.Constants; import frc.robot.Constants.CoralLevel; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ScoringLocations; -import com.therekrab.autopilot.APTarget; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.AlgaeStow; import frc.robot.superstructure.states.Align; import frc.robot.superstructure.states.CoralScore; import frc.robot.superstructure.states.CoralWait; import frc.robot.superstructure.states.ElevatorZero; +import frc.robot.superstructure.states.HighGroundAlgaeIntake; import frc.robot.superstructure.states.IntakeComplete; import frc.robot.superstructure.states.LowerReefAlgaeIntake; -import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.ManualAlgaeEject; +import frc.robot.superstructure.states.UncheckedNet; +import frc.robot.superstructure.states.NetPrep; +import frc.robot.superstructure.states.StowTemp; +import frc.robot.superstructure.states.TrackAlgae; import frc.robot.superstructure.states.UpperReefAlgaeIntake; public class NamedCommandBindings implements Binder { @@ -36,8 +41,15 @@ public void bind(Superstructure superstructure) { /* algae */ NamedCommands.registerCommand("Lower Algae", superstructure.enter(new LowerReefAlgaeIntake())); NamedCommands.registerCommand("Upper Algae", superstructure.enter(new UpperReefAlgaeIntake())); - NamedCommands.registerCommand("Net", superstructure.enter(new Net())); + NamedCommands.registerCommand("Net", superstructure.enter(new UncheckedNet())); + NamedCommands.registerCommand("Net Prep", superstructure.enter(new NetPrep())); NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); + NamedCommands.registerCommand("Lollipop Intake", superstructure.enter(new HighGroundAlgaeIntake())); + NamedCommands.registerCommand("Eject Algae", superstructure.enter(new ManualAlgaeEject())); + NamedCommands.registerCommand("Stow", superstructure.enter(new StowTemp())); + + /* steal hehe */ + NamedCommands.registerCommand("Track", superstructure.enter(new TrackAlgae())); /* align */ for (ScoringLocations location : Constants.ScoringLocations.values()) { @@ -46,6 +58,8 @@ public void bind(Superstructure superstructure) { new Align(new APTarget(location.value).withEntryAngle(location.value.getRotation())) .allianceRelative())); } + + // HP stations APTarget lIntake = new APTarget(FieldConstants.kLeftIntake) .withRotationRadius(Meters.of(2.0)); APTarget rIntake = new APTarget(FieldConstants.kRightIntake) @@ -66,12 +80,17 @@ public void bind(Superstructure superstructure) { new Align(rIntake) .allianceRelative() .fast())); + + // Halfway points NamedCommands.registerCommand("Align IJ", superstructure.enter( - new Align(new APTarget(FieldConstants.kIJ).withEntryAngle(FieldConstants.kIJ.getRotation())) - .allianceRelative())); + new Align(new APTarget(FieldConstants.kIJ) + .withEntryAngle(FieldConstants.kIJ.getRotation())) + .allianceRelative())); NamedCommands.registerCommand("Align GH", superstructure.enter( - new Align(new APTarget(FieldConstants.kGH).withEntryAngle(FieldConstants.kGH.getRotation())) - .allianceRelative())); + new Align(new APTarget(FieldConstants.kGH) + .withEntryAngle(FieldConstants.kGH.getRotation())) + .allianceRelative())); + // Barge APTarget bargeFromCenter = new APTarget(FieldConstants.kBargeFromCenter) .withEntryAngle(Rotation2d.fromDegrees(55.0)) .withRotationRadius(Meters.of(1.5)); @@ -79,12 +98,60 @@ public void bind(Superstructure superstructure) { .withEntryAngle(Rotation2d.fromDegrees(35.0)) .withRotationRadius(Meters.of(1.5)); NamedCommands.registerCommand("Align Barge Center", superstructure.enter( - new Align(bargeFromCenter).allianceRelative().slow())); + new Align(bargeFromCenter).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); NamedCommands.registerCommand("Align Barge Left", superstructure.enter( - new Align(bargeFromLeft).allianceRelative().slow())); + new Align(bargeFromLeft).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); NamedCommands.registerCommand("Beeline Barge Center", superstructure.enter( - new Align(bargeFromCenter.withoutEntryAngle()).allianceRelative().slow())); + new Align(bargeFromCenter.withoutEntryAngle()).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); NamedCommands.registerCommand("Beeline Barge Left", superstructure.enter( - new Align(bargeFromLeft.withoutEntryAngle()).allianceRelative().slow())); + new Align(bargeFromLeft.withoutEntryAngle()).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); + + // Steal! + APTarget insideStealPrep = new APTarget(FieldConstants.kInsideStealPrep) + .withRotationRadius(Meters.of(1)); + APTarget outsideStealPrep = new APTarget(FieldConstants.kOutsideStealPrep) + .withRotationRadius(Meters.of(1)); + APTarget outsideSteal = new APTarget(FieldConstants.kOutsideSteal); + APTarget insideSteal = new APTarget(FieldConstants.kInsideSteal); + APTarget insideLollipopPrep = new APTarget(FieldConstants.kInsideLollipop) + .withRotationRadius(Meters.of(1)); + APTarget outsideLollipopPrep = new APTarget(FieldConstants.kOutsideLollipopPrep) + .withRotationRadius(Meters.of(1)); + APTarget insideLollipop = new APTarget(FieldConstants.kInsideLollipop); + APTarget outsideLollipop = new APTarget(FieldConstants.kOutsideLollipop); + NamedCommands.registerCommand("Prep Steal Inside", + superstructure.enter(new Align(insideStealPrep).allianceRelative())); + NamedCommands.registerCommand("Prep Steal Outside", + superstructure.enter(new Align(outsideStealPrep).allianceRelative())); + NamedCommands.registerCommand("Steal Inside", + superstructure.enter(new Align(insideSteal).allianceRelative())); + NamedCommands.registerCommand("Steal Outside", + superstructure.enter(new Align(outsideSteal).allianceRelative())); + + NamedCommands.registerCommand("Prep Lollipop Inside", + superstructure.enter(new Align(insideLollipopPrep).allianceRelative())); + NamedCommands.registerCommand("Prep Lollipop Outside", + superstructure.enter(new Align(outsideLollipopPrep).allianceRelative())); + NamedCommands.registerCommand("Lollipop Inside", + superstructure.enter(new Align(insideLollipop).allianceRelative())); + NamedCommands.registerCommand("Lollipop Outside", + superstructure.enter(new Align(outsideLollipop).allianceRelative())); + + // Shoot! + APTarget outsideShoot = new APTarget(FieldConstants.kOutsideShoot) + .withRotationRadius(Meters.of(0.5)); + APTarget insideShoot = new APTarget(FieldConstants.kInsideShoot) + .withRotationRadius(Meters.of(0.5)); + NamedCommands.registerCommand("Shoot Outside", + superstructure.enter(new Align(outsideShoot).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); + NamedCommands.registerCommand("Shoot Inside", + superstructure.enter(new Align(insideShoot).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); } + } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 3632871e..93154e84 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -6,11 +6,10 @@ import frc.robot.binding.BindingConstants.Operator; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.Climb; +import frc.robot.superstructure.states.ClimbStowed; import frc.robot.superstructure.states.RaiseClimb; -import frc.robot.superstructure.states.CompleteCoralIntake; import frc.robot.superstructure.states.CompleteCoralScore; import frc.robot.superstructure.states.CoralEject; -import frc.robot.superstructure.states.CoralIntake; import frc.robot.superstructure.states.CoralScore; import frc.robot.superstructure.states.CoralScorePrep; import frc.robot.superstructure.states.ElevatorZero; @@ -20,41 +19,44 @@ import frc.robot.superstructure.states.UpperReefAlgaeIntake; import frc.robot.superstructure.states.DeferredAlign.AlignLocation; import frc.robot.superstructure.states.LowerReefAlgaeIntake; -import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.UncheckedNet; import frc.robot.superstructure.states.NetPrep; import frc.robot.superstructure.states.Processor; import frc.robot.superstructure.states.ProcessorPrep; +import frc.robot.superstructure.states.AutoSelectCoralScore; +import frc.robot.superstructure.states.CheckedNet; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.Stow; +import frc.robot.superstructure.states.StowTemp; public class OperatorBindings implements Binder { private final CommandPS5Controller m_controller = new CommandPS5Controller(BindingConstants.kOperatorPort); private final Trigger m_l1 = m_controller.pov(Operator.kL1); - private final Trigger m_secondaryL1 = m_controller.pov(Operator.kSecondaryL1); private final Trigger m_l2 = m_controller.pov(Operator.kL2); private final Trigger m_l3 = m_controller.pov(Operator.kL3); private final Trigger m_l4 = m_controller.pov(Operator.kL4); - + private final Trigger m_left = m_controller.button(Operator.kLeftAlign); private final Trigger m_right = m_controller.button(Operator.kRightAlign); - - private final Trigger m_coralIntake = m_controller.button(Operator.kCoralIntake); + private final Trigger m_ejectCoral = m_controller.button(Operator.kEjectCoral); - + private final Trigger m_algae = m_controller.button(Operator.kAlgae); - + private final Trigger m_algaeGround = m_controller.pov(Operator.kGroundAlgaeIntake); private final Trigger m_algaeHighGround = m_controller.pov(Operator.kHighGroundAlgaeIntake); private final Trigger m_algaeLowReef = m_controller.button(Operator.kLowerAlgae); private final Trigger m_algaeHighReef = m_controller.button(Operator.kUpperAlgae); - + private final Trigger m_processor = m_controller.pov(Operator.kProcessor); - private final Trigger m_net = m_controller.pov(Operator.kNet); - + private final Trigger m_netPrep = m_controller.pov(Operator.kNetPrep); + private final Trigger m_netScore = m_controller.button(Operator.kScoreNet); + private final Trigger m_raiseClimb = m_controller.button(Operator.kRaiseClimb); private final Trigger m_climb = m_controller.button(Operator.kClimb); + private final Trigger m_stowClimb = m_controller.button(Operator.kStowClimb); private final Trigger m_stow = m_controller.button(Operator.kStow); @@ -62,9 +64,13 @@ public class OperatorBindings implements Binder { private final Trigger m_funnelRight = m_controller.button(Operator.kRightFunnel); private final Trigger m_funnel = m_funnelLeft.and(m_funnelRight); + private final Trigger m_coralScore = m_controller.button(Operator.kCoralScore); + private final Trigger m_zeroElevator = m_controller.button(Operator.kCalibrateElevator); public void bind(Superstructure superstructure) { + /* algae tracking */ + /* algae intake */ m_algae.and(m_algaeGround).whileTrue(superstructure.enter(new GroundAlgaeIntake())); m_algae.and(m_algaeHighGround).whileTrue(superstructure.enter(new HighGroundAlgaeIntake())); @@ -74,18 +80,18 @@ public void bind(Superstructure superstructure) { /* algae score */ m_algae.and(m_processor).whileTrue(superstructure.enter(new ProcessorPrep())); m_algae.and(m_processor).onFalse(superstructure.enter(new Processor())); - m_algae.and(m_net).whileTrue(superstructure.enter(new NetPrep())); - m_algae.and(m_net).onFalse(superstructure.enter(new Net())); + m_algae.and(m_netPrep) + .whileTrue(superstructure.enter(new NetPrep())) + .onFalse(superstructure.enter(new StowTemp())); + m_netScore.onTrue(superstructure.enter(new CheckedNet())); /* coral intake & score */ - m_coralIntake.whileTrue(superstructure.enter(new CoralIntake())); - m_coralIntake.onFalse(superstructure.enter(new CompleteCoralIntake())); m_ejectCoral.whileTrue(superstructure.enter(new CoralEject())); - bindCoral(m_l1.and(m_secondaryL1.negate()), CoralLevel.L1, superstructure); - bindCoral(m_l1.and(m_secondaryL1), CoralLevel.SecondaryL1, superstructure); + bindCoral(m_l1, CoralLevel.L1, superstructure); bindCoral(m_l2, CoralLevel.L2, superstructure); bindCoral(m_l3, CoralLevel.L3, superstructure); bindCoral(m_l4, CoralLevel.L4, superstructure); + m_coralScore.onTrue(superstructure.enter(new AutoSelectCoralScore())); /* align */ m_left.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); @@ -94,6 +100,7 @@ public void bind(Superstructure superstructure) { /* climb */ m_climb.whileTrue(superstructure.enter(new Climb())); m_raiseClimb.whileTrue(superstructure.enter(new RaiseClimb())); + m_stowClimb.whileTrue(superstructure.enter(new ClimbStowed())); /* misc */ m_zeroElevator.whileTrue(superstructure.enter(new ElevatorZero())); @@ -102,10 +109,12 @@ public void bind(Superstructure superstructure) { } private void bindCoral(Trigger trigger, CoralLevel level, Superstructure superstructure) { - trigger.and(m_algae.negate()).whileTrue(superstructure.enter(new CoralScorePrep(level))); - trigger.and(m_algae.negate()).and(superstructure.aligned()) - .onTrue(superstructure.enter(new CoralScore(level))); - trigger.and(m_algae.negate()).onFalse(superstructure.enter(new CompleteCoralScore(level))); + trigger.and(m_algae.negate()) + .whileTrue(superstructure.enter(new CoralScorePrep(level))) + .onFalse(superstructure.enter(new StowTemp())); + // trigger.and(m_algae.negate()).and(superstructure.aligned()) + // .onTrue(superstructure.enter(new CoralScore(level))); + // trigger.and(m_algae.negate()).onFalse(superstructure.enter(new CompleteCoralScore(level))); } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java index 70cb2eb0..70450c75 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java @@ -31,7 +31,8 @@ public class Algae extends PassiveSubsystem { private AlgaeIOInputsLogger m_inputsLogger; private boolean m_hasAlgae; - private SuperDebouncer m_debouncer = new SuperDebouncer(AlgaeConstants.kAlgaeDebounceTime.in(Seconds), DebounceType.kFalling); + private SuperDebouncer m_debouncer = + new SuperDebouncer(AlgaeConstants.kAlgaeDebounceTime.in(Seconds), DebounceType.kFalling); private MedianFilter m_filter = new MedianFilter(10); @@ -42,12 +43,12 @@ public Algae() { } else { m_io = new AlgaeIOSim(); } - resetDebouncer(); + resetDebouncer(false); m_inputs = new AlgaeIOInputs(); m_inputsLogger = new AlgaeIOInputsLogger(m_inputs); m_ologger = new OnboardLogger("Algae"); m_ologger.registerBoolean("Holding", holdingAlgae()); - + RobotObserver.setAlgaePieceHeldSupplier(this.holdingAlgae()); m_timer = new LoopTimer("Algae"); } @@ -86,7 +87,8 @@ public void periodic() { m_timer.reset(); m_io.updateInputs(m_inputs); m_inputsLogger.log(); - m_hasAlgae = m_debouncer.calculate(getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold); + m_hasAlgae = + m_debouncer.calculate(getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold); m_ologger.log(); m_timer.log(); } @@ -107,7 +109,13 @@ public Command intake() { .unless(holdingAlgae()); } - private void resetDebouncer() { + /** + * Resets the debouncer to be willing to accept any new inputs as accurate + */ + private void resetDebouncer(boolean ignore) { + if (ignore) { + return; + } m_debouncer.overrideTimer(); } @@ -133,6 +141,19 @@ public Command processorScore() { Commands.waitSeconds(AlgaeConstants.kProcessorScoreTime)) .finallyDo(this::keep) + .finallyDo(this::resetDebouncer) .onlyIf(holdingAlgae()); } + + /** Ejects an algae fast */ + public Command eject() { + return Commands.sequence( + runOnce(() -> setVoltage(AlgaeConstants.kManualEjectVoltage)), + Commands.waitSeconds(AlgaeConstants.kManualEjectTime)) + + .finallyDo(this::keep) + .finallyDo(this::resetDebouncer) + .onlyIf(holdingAlgae()); + } + } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java index 7189b9f7..86a49666 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java @@ -17,6 +17,7 @@ public final class AlgaeConstants { protected static final double kNetEjectVoltage = -3.0; protected static final double kProcessorEjectVoltage = -3.2; protected static final double kHoldVoltage = 6.0; + protected static final double kManualEjectVoltage = -6.0; protected static final double kTorqueCurrentThreshold = 75; // We should consider 40-55 range as well. protected static final Time kAlgaeDebounceTime = Seconds.of(0.8); @@ -26,6 +27,7 @@ public final class AlgaeConstants { protected static final double kProcessorScoreTime = 2.0; protected static final double kNetScoreTime = 0.4; + protected static final double kManualEjectTime = 0.3; protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() .withMotorOutput(new MotorOutputConfigs() diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java index ad4fa0f6..9526e280 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -90,14 +90,14 @@ public Trigger lowered() { } protected void passive() { - if (DriverStation.isFMSAttached() && DriverStation.isTeleop() - && DriverStation.getMatchTime() < 40) { - if (raised().getAsBoolean()) { - setUp(); - } else { - stop(); - } - } + // if (DriverStation.isFMSAttached() && DriverStation.isTeleop() + // && DriverStation.getMatchTime() < 40) { + // if (!raised().getAsBoolean()) { + // setUp(); + // } else { + // stop(); + // } + // } } /** diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 3eda403f..47718c7b 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.drivetrain; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; import java.io.IOException; @@ -21,16 +20,14 @@ import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; -import com.ctre.phoenix6.swerve.SwerveRequest.RobotCentric; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.FlippingUtil; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -38,9 +35,9 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -55,12 +52,14 @@ import frc.robot.RobotObserver; import com.therekrab.autopilot.APTarget; import com.therekrab.autopilot.Autopilot; +import com.therekrab.autopilot.Autopilot.APResult; import frc.robot.driveassist.ForceField; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.utils.FieldUtils; import frc.robot.utils.LoopTimer; import frc.robot.utils.OnboardLogger; import frc.robot.vision.localization.TimestampedPoseEstimate; +import frc.robot.vision.tracking.SimplePoseFilter; import frc.robot.vision.tracking.AlgaeTracker.ObjectTrackingStatus; /** @@ -80,6 +79,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private boolean m_aligned; private Trigger m_tippyTrigger = new Trigger(() -> false); + private Trigger m_slowTrigger = new Trigger(() -> false); private boolean m_hasReceivedVisionUpdate; @@ -87,9 +87,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) .withDriveRequestType(DriveRequestType.Velocity); - private RobotCentric m_trackingRequest = new RobotCentric() - .withDriveRequestType(DriveRequestType.Velocity); - private FieldCentricFacingAngle m_veloRequest = new FieldCentricFacingAngle() .withHeadingPID(DriveConstants.HeadingPID.kP, 0, 0) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) @@ -105,7 +102,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private boolean m_hasAppliedOperatorPerspective = false; /* last known object tracking input */ - private Optional m_objectStatus = Optional.empty(); + private Optional m_algae = Optional.empty(); + private final SimplePoseFilter m_algaeSmoother = new SimplePoseFilter(); + private Pose3d m_lastAlgae = Pose3d.kZero; private Pose2d m_estimatedPose = new Pose2d(); @@ -128,6 +127,8 @@ public CommandSwerveDrivetrain( m_ologger.registerDouble("Velocity", this::getVelocity); m_ologger.registerPose("Estimated Pose", this::getPose); m_ologger.registerBoolean("Received Vision Update", () -> m_hasReceivedVisionUpdate); + m_ologger.registerBoolean("Valid Object Estimate", seesAlgae()); + m_ologger.registerPose3d("Last Algae", () -> m_lastAlgae); RobotObserver.setVelocitySupplier(this::getVelocity); RobotObserver.setNoElevatorZoneSupplier(dangerZone()); @@ -221,7 +222,8 @@ private void setPose(Pose2d pose) { } public Command setLocalHeading(Rotation2d heading) { - return Commands.runOnce(() -> resetRotation(FieldUtils.getLocalRotation(heading))).ignoringDisable(true); + return Commands.runOnce(() -> resetRotation(FieldUtils.getLocalRotation(heading))) + .ignoringDisable(true); } private ChassisSpeeds getRobotRelativeSpeeds() { @@ -258,12 +260,16 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + // Update algae state + m_algae = m_algaeSmoother.calculate(); + if (m_algae.isPresent()) { + m_lastAlgae = m_algae.get(); + RobotObserver.getField().getObject("Algae").setPose(m_lastAlgae.toPose2d()); + } else { + RobotObserver.getField().getObject("Algae").setPoses(); + } m_ologger.log(); m_hasReceivedVisionUpdate = false; - // Expire old algae tracking data - if (m_objectStatus.isPresent() && m_objectStatus.get().isExpired()) { - m_objectStatus = Optional.empty(); - } m_timer.log(); } @@ -348,7 +354,7 @@ public void addPoseEstimate(TimestampedPoseEstimate estimate) { } public void addObjectTrackingData(ObjectTrackingStatus status) { - m_objectStatus = Optional.of(status); + m_algaeSmoother.add(status); } public Command sysIdQuasistaticTranslation(SysIdRoutine.Direction direction) { @@ -402,6 +408,11 @@ public Trigger inReefZone() { }); } + public Trigger seesAlgae() { + // TODO: ensure distance is below threshold + return new Trigger(() -> m_algae.isPresent()); + } + /** * Drives the robot from given x, y, and rotatational velocity suppliers. */ @@ -433,6 +444,10 @@ public void setTippyTrigger(Trigger tippyTrigger) { m_tippyTrigger = tippyTrigger; } + public void setSlowTrigger(Trigger slowTrigger) { + m_slowTrigger = slowTrigger; + } + private AngularVelocity getMaxRotationalRate() { if (m_tippyTrigger.getAsBoolean()) { return DriveConstants.kMaxTippyAngularSpeed; @@ -441,9 +456,27 @@ private AngularVelocity getMaxRotationalRate() { } } - /** - * Drives to a certain point on the field - */ + private LinearVelocity getMaxSpeed() { + if (m_slowTrigger.getAsBoolean()) { + return DriveConstants.kMaxTippySpeed; + } + return DriveConstants.kMaxLinearSpeed; + } + + private void setVelocity(APResult goal) { + double norm = Math.hypot(goal.vx().in(MetersPerSecond), goal.vy().in(MetersPerSecond)); + double max = getMaxSpeed().in(MetersPerSecond); + if (norm > max) { + goal = new APResult(goal.vx().times(max / norm), goal.vy().times(max / norm), + goal.targetAngle()); + } + setControl(m_veloRequest + .withVelocityX(goal.vx()) + .withVelocityY(goal.vy()) + .withTargetDirection(goal.targetAngle()) + .withMaxAbsRotationalRate(getMaxRotationalRate())); + } + public Command align(Autopilot autopilot, APTarget target) { return Commands.sequence( runOnce(() -> { @@ -451,19 +484,16 @@ public Command align(Autopilot autopilot, APTarget target) { setAligned(false); }), run(() -> { - Translation2d velocities = getVelocityComponents(); - Transform2d output = autopilot.calculate(m_estimatedPose, velocities, target); - setControl(m_veloRequest - .withVelocityX(output.getX()) - .withVelocityY(output.getY()) - .withTargetDirection(output.getRotation()) - .withMaxAbsRotationalRate(getMaxRotationalRate())); + ChassisSpeeds robotRelatiSpeeds = getRobotRelativeSpeeds(); + APResult output = autopilot.calculate(m_estimatedPose, robotRelatiSpeeds, target); + setVelocity(output); + + setAligned(autopilot.atTarget(m_estimatedPose, target)); })) .until(() -> { return autopilot.atTarget(m_estimatedPose, target); }) .finallyDo(this::stop) - .finallyDo(interrupted -> setAligned(!interrupted)) .finallyDo(() -> { RobotObserver.getField().getObject("reference").setPoses(); }); @@ -475,36 +505,25 @@ public Command seedLocal(Pose2d pose) { } public Command followObject() { - return run(new Runnable() { - private final PIDController thetaController = new PIDController(8, 0, 0); - private final PIDController driveController = new PIDController(1, 0, 0); - - @Override - public void run() { - m_objectStatus.ifPresentOrElse(status -> { - double output = -this.thetaController.calculate(status.yaw().getRadians(), 0); - double speed = DriveConstants.kObjectTrackSpeed.in(MetersPerSecond); - if (status.pose().isPresent()) { - double distance = m_estimatedPose - .relativeTo(status.pose().get()) - .getTranslation() - .getNorm(); - speed = Math.min( - DriveConstants.kMaxObjectTrackingSpeed.in(MetersPerSecond), - -this.driveController.calculate(distance, 0)); - } - /* - * we invert the calculation because it outputs a negative number, because measurement > - * ref, so err < 0 - */ - double vx = speed * status.yaw().getCos(); - double vy = speed * status.yaw().getSin(); - setControl(m_trackingRequest - .withRotationalRate(output) - .withVelocityX(vx) - .withVelocityY(vy)); - }, () -> stop()); + return run(() -> { + if (m_algae.isEmpty()) { + stop(); + return; } - }); + // If we have a pose estimate, for algae, use Autopilot to go there. + // APTarget target = new APTarget(algae.transformBy(DriveConstants.kAlgaeOffset)); + Rotation2d angle = m_lastAlgae.toPose2d().getTranslation() + .minus(m_estimatedPose.getTranslation()) + .getAngle(); + APTarget target = + new APTarget(m_lastAlgae.toPose2d() + .transformBy(new Transform2d(Translation2d.kZero, angle)) + .transformBy(DriveConstants.kAlgaeOffset)); + APResult output = DriveConstants.kTightAutopilot.calculate( + m_estimatedPose, + getRobotRelativeSpeeds(), + target); + setVelocity(output); + }).onlyWhile(seesAlgae()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index f8e8baa6..85e92ef9 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.Centimeters; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; @@ -14,8 +13,12 @@ import com.therekrab.autopilot.APConstraints; import com.therekrab.autopilot.APProfile; import com.therekrab.autopilot.Autopilot; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.generated.TunerConstants; @@ -51,18 +54,6 @@ public static class HeadingPID { public static final Autopilot kFastAutopilot = new Autopilot(kFastProfile); - private static final APConstraints kSlowAPConstraints = new APConstraints() - .withAcceleration(3) - .withJerk(1.5) - .withVelocity(3); - - private static final APProfile kSlowProfile = new APProfile(kSlowAPConstraints) - .withErrorXY(Inches.of(2)) - .withErrorTheta(Degrees.of(3)) - .withBeelineRadius(Centimeters.of(20)); - - public static final Autopilot kSlowAutopilot = new Autopilot(kSlowProfile); - protected static final PPHolonomicDriveController kPathplannerHolonomicDriveController = new PPHolonomicDriveController(kTranslationPID, kRotationPID); @@ -74,6 +65,7 @@ public static class HeadingPID { protected static final LinearVelocity kMaxLinearSpeed = MetersPerSecond.of(4); protected static final LinearAcceleration kMaxLinearAcceleration = MetersPerSecondPerSecond.of(3); + public static final LinearVelocity kMaxTippySpeed = MetersPerSecond.of(3); protected static final AngularVelocity kMaxAngularSpeed = RotationsPerSecond.of(2); protected static final AngularVelocity kMaxTippyAngularSpeed = RotationsPerSecond.of(0.5); protected static final AngularAcceleration kMaxAngularAcceleration = @@ -85,7 +77,10 @@ public static class HeadingPID { protected static final double k_closedLoopOverrideToleranceTranslation = 0.05; protected static final double k_closedLoopOverrideToleranceRotation = 0.05; + public static final Distance kObjectDistanceLimit = Meters.of(3); protected static final LinearVelocity kObjectTrackSpeed = MetersPerSecond.of(2); protected static final LinearVelocity kMaxObjectTrackingSpeed = MetersPerSecond.of(4); + protected static final Transform2d kAlgaeOffset = new Transform2d( + new Translation2d(-0.5, 0), Rotation2d.kZero); } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 9d983261..480dd487 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -54,6 +54,7 @@ public Elevator() { m_inputsLogger = new ElevatorIOInputsLogger(m_inputs); m_ologger = new OnboardLogger("Elevator"); m_ologger.registerString("State", () -> m_reference.toString()); + SmartDashboard.putData("Elevator/Force Net", unsafeNet()); SmartDashboard.putData("Elevator/Lazy Zero", runOnce(m_io::calibrateZero).ignoringDisable(true).withName("Lazy Zero")); m_timer = new LoopTimer("Elevator"); @@ -155,6 +156,12 @@ public Command autoZero() { .withName("Autozero"); } + private Command unsafeNet() { + return Commands.sequence( + runOnce(() -> m_io.setPosition(ElevatorState.Net.position())), + Commands.idle()); + } + public void setPrefireRequirement(Trigger trigger) { m_prefireReq = trigger; } @@ -162,4 +169,8 @@ public void setPrefireRequirement(Trigger trigger) { public void setStayRequirement(Trigger trigger) { m_stay = trigger; } + + public ElevatorState getElevatorState() { + return m_reference; + } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index db640942..129efc8e 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -28,7 +28,7 @@ public enum ElevatorState { /** Height to intake algae from lower reef */ LowerReef(2.0), /** Height to intake algae from upper reef */ - UpperReef(4.5 - 1.5 * ElevatorConstants.kInch); + UpperReef(4.5 - 3 * ElevatorConstants.kInch); private final double m_position; diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java b/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java index 84f3ceca..22965010 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java @@ -39,6 +39,7 @@ public Superstructure( leds); drivetrain.setTippyTrigger(tippy()); + drivetrain.setSlowTrigger(elevator.unsafe()); RobotObserver.setFFEnabledSupplier(elevator.unsafe().and(() -> !DriverStation.isAutonomous())); } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java index 3c028779..716dc799 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java @@ -53,11 +53,6 @@ public Align fast() { return this; } - public Align slow() { - m_autopilot = DriveConstants.kSlowAutopilot; - return this; - } - private APTarget target() { if (m_flip) { return FieldUtils.flipTargetConditionally(m_target); diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java new file mode 100644 index 00000000..1666eabf --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java @@ -0,0 +1,29 @@ +package frc.robot.superstructure.states; + +import java.util.Map; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.CoralLevel; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class AutoSelectCoralScore implements EnterableState { + + + public Command build(Subsystems subsystems) { + final Map scoreCommands = Map.ofEntries( + Map.entry(ElevatorState.L1, + new CoralScore(CoralLevel.L1).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L1))), + Map.entry(ElevatorState.L2, + new CoralScore(CoralLevel.L2).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L2))), + Map.entry(ElevatorState.L3, + new CoralScore(CoralLevel.L3).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L3))), + Map.entry(ElevatorState.L4, new CoralScore(CoralLevel.L4).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L4)))); + return Commands.select(scoreCommands, subsystems.elevator()::getElevatorState); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java new file mode 100644 index 00000000..95ef4d3b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java @@ -0,0 +1,20 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CheckedNet implements EnterableState { + /** + * A state that scores an algae in the net + */ + public CheckedNet() {} + + public Command build(Subsystems subsystems) { + return new UncheckedNet().build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.Net)); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java index 94299bf5..77d2634f 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java @@ -1,9 +1,10 @@ package frc.robot.superstructure.states; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure.Subsystems; -public class ClimbStowed { +public class ClimbStowed implements EnterableState { /** * A state that represents the climber at its "stow" position */ diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java index aae780b3..98a145ac 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java @@ -8,11 +8,14 @@ public class CoralWait implements EnterableState { /** * A state that puts the robot into a "waiting" state until a coral can be acquired. - * The state is NOT a coral itake state, however, and does not actually change any mechanisms + * The state is NOT a coral itake state, however, and does not actually change any mechanisms. + * This also will wait 0.1s first before anything else happens to avoid any mistakes (coral leaving scoring mechanism). */ public CoralWait() {} public Command build(Subsystems subsystems) { - return Commands.waitUntil(subsystems.coral().present()); + return Commands.sequence( + Commands.waitSeconds(0.1), + Commands.waitUntil(subsystems.coral().present())); } } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java index b3adfd06..8e483c75 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java @@ -54,9 +54,7 @@ public Command build(Subsystems subsystems) { Pose2d closest = subsystems.drivetrain().getPose().nearest(locations); APTarget target = new APTarget(closest) .withEntryAngle(closest.getRotation()); - return Commands.sequence( - subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target), - Commands.idle(subsystems.drivetrain())); + return subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target); }, Set.of(subsystems.drivetrain())); } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java index 5d4e9e41..d8143624 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java @@ -16,10 +16,9 @@ public LowerReefAlgaeIntake() {} public Command build(Subsystems subsystems) { return Commands.sequence( subsystems.elevator().go(ElevatorState.LowerReef), - Commands.parallel( - subsystems.pivot().go(PivotState.ReefIntake), - subsystems.algae().intake()), - subsystems.pivot().go(PivotState.ReefExtract)) + Commands.deadline( + subsystems.algae().intake(), + subsystems.pivot().go(PivotState.ReefIntake))) .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.algae()::release) diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java new file mode 100644 index 00000000..d6352c2f --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java @@ -0,0 +1,11 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class ManualAlgaeEject implements EnterableState { + public Command build(Subsystems subsystems) { + return subsystems.algae().eject(); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java new file mode 100644 index 00000000..502c2cc4 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java @@ -0,0 +1,25 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class StowTemp implements EnterableState { + /** + * Stows all the stowables on the robot + */ + public StowTemp() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Stow), + subsystems.pivot().go(PivotState.Stow)) + + .finallyDo(subsystems.elevator()::release) // This allows the elevator to stow itself automatically. + .finallyDo(subsystems.pivot()::release); + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java index 0e8a9c66..bbada704 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java @@ -6,6 +6,7 @@ public class TrackAlgae implements EnterableState { public Command build(Subsystems subsystems) { - return subsystems.drivetrain().followObject(); + return subsystems.drivetrain().followObject() + .until(subsystems.algae().holdingAlgae()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UncheckedNet.java similarity index 91% rename from ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java rename to ThriftyTest/src/main/java/frc/robot/superstructure/states/UncheckedNet.java index b0130924..bd4e985a 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UncheckedNet.java @@ -7,11 +7,11 @@ import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure.Subsystems; -public class Net implements EnterableState { +public class UncheckedNet implements EnterableState { /** * A state that scores an algae in the net */ - public Net() {} + public UncheckedNet() {} public Command build(Subsystems subsystems) { return Commands.sequence( diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java index c3e9edae..8c3c196e 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java @@ -16,9 +16,9 @@ public UpperReefAlgaeIntake() {} public Command build(Subsystems subsystems) { return Commands.sequence( subsystems.elevator().go(ElevatorState.UpperReef), - subsystems.pivot().go(PivotState.ReefIntake), - subsystems.algae().intake(), - subsystems.pivot().go(PivotState.ReefExtract)) + Commands.deadline( + subsystems.algae().intake(), + subsystems.pivot().go(PivotState.ReefIntake))) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.elevator()::release) diff --git a/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java b/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java deleted file mode 100644 index 4ce161dd..00000000 --- a/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.utils; - -import java.util.function.BooleanSupplier; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -public class MonitoredSupplier implements Supplier { - private T m_prev; - - private final Supplier m_supplier; - - public MonitoredSupplier(Supplier supplier) { - m_supplier = supplier; - } - - public void ifChanged(Consumer action) { - T curr = m_supplier.get(); - if (!curr.equals(m_prev)) { - m_prev = curr; - action.accept(curr); - } - } - - public static MonitoredSupplier fromDoubleSuplier(DoubleSupplier supplier) { - return new MonitoredSupplier<>(supplier::getAsDouble); - } - - public static MonitoredSupplier fromBooleanSupplier(BooleanSupplier supplier) { - return new MonitoredSupplier(supplier::getAsBoolean); - } - - public T get() { - return m_supplier.get(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java index 5f2add24..9d400585 100644 --- a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java +++ b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java @@ -7,6 +7,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.util.datalog.BooleanLogEntry; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; @@ -24,63 +25,84 @@ public class OnboardLogger { private final String m_name; - private final List, DoubleLogEntry>> m_doubleEntries; - private final List, BooleanLogEntry>> m_booleanEntries; - private final List, StringLogEntry>> m_stringEntries; - private final List, StructLogEntry>> m_poseEntries; - private final List, StructArrayLogEntry>> m_poseListEntries; + private final List, DoubleLogEntry>> m_doubleEntries; + private final List, BooleanLogEntry>> m_booleanEntries; + private final List, StringLogEntry>> m_stringEntries; + private final List, StructLogEntry>> m_pose2dEntries; + private final List, StructArrayLogEntry>> m_pose2dArrayEntries; + private final List, StructLogEntry>> m_pose3dEntries; + private final List, StructArrayLogEntry>> m_pose3dArrayEntries; public OnboardLogger(String name) { m_name = name; m_doubleEntries = new ArrayList<>(); m_booleanEntries = new ArrayList<>(); m_stringEntries = new ArrayList<>(); - m_poseEntries = new ArrayList<>(); - m_poseListEntries = new ArrayList<>(); + m_pose2dEntries = new ArrayList<>(); + m_pose2dArrayEntries = new ArrayList<>(); + m_pose3dEntries = new ArrayList<>(); + m_pose3dArrayEntries = new ArrayList<>(); } public void registerDouble(String name, DoubleSupplier supplier) { DoubleLogEntry entry = new DoubleLogEntry(dataLog, m_name + "/" + name); - m_doubleEntries.add(new Pair<>(MonitoredSupplier.fromDoubleSuplier(supplier), entry)); + m_doubleEntries.add(new Pair<>(supplier::getAsDouble, entry)); } public void registerBoolean(String name, BooleanSupplier supplier) { BooleanLogEntry entry = new BooleanLogEntry(dataLog, m_name + "/" + name); - m_booleanEntries.add(new Pair<>(MonitoredSupplier.fromBooleanSupplier(supplier), entry)); + m_booleanEntries.add(new Pair<>(supplier::getAsBoolean, entry)); } public void registerString(String name, Supplier supplier) { StringLogEntry entry = new StringLogEntry(dataLog, m_name + "/" + name); - m_stringEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + m_stringEntries.add(new Pair<>(supplier, entry)); } public void registerPose(String name, Supplier supplier) { StructLogEntry entry = StructLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); - m_poseEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + m_pose2dEntries.add(new Pair<>(supplier, entry)); + } + + public void registerPose3d(String name, Supplier supplier) { + StructLogEntry entry = StructLogEntry.create(dataLog, m_name + "/" + name, Pose3d.struct); + m_pose3dEntries.add(new Pair<>(supplier, entry)); } public void registerPoses(String name, Supplier supplier) { StructArrayLogEntry entry = StructArrayLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); - m_poseListEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + m_pose2dArrayEntries.add(new Pair<>(supplier, entry)); + } + + public void registerPoses3d(String name, Supplier supplier) { + StructArrayLogEntry entry = + StructArrayLogEntry.create(dataLog, m_name + "/" + name, Pose3d.struct); + m_pose3dArrayEntries.add(new Pair<>(supplier, entry)); } public void log() { - for (Pair, DoubleLogEntry> pair : m_doubleEntries) { - pair.getFirst().ifChanged(d -> pair.getSecond().append(d)); + for (Pair, DoubleLogEntry> pair : m_doubleEntries) { + pair.getSecond().update(pair.getFirst().get()); + } + for (Pair, BooleanLogEntry> pair : m_booleanEntries) { + pair.getSecond().update(pair.getFirst().get()); + } + for (Pair, StringLogEntry> pair : m_stringEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, BooleanLogEntry> pair : m_booleanEntries) { - pair.getFirst().ifChanged(b -> pair.getSecond().append(b)); + for (Pair, StructLogEntry> pair : m_pose2dEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StringLogEntry> pair : m_stringEntries) { - pair.getFirst().ifChanged(s -> pair.getSecond().append(s)); + for (Pair, StructArrayLogEntry> pair : m_pose2dArrayEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StructLogEntry> pair : m_poseEntries) { - pair.getFirst().ifChanged(p -> pair.getSecond().append(p)); + for (Pair, StructLogEntry> pair : m_pose3dEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StructArrayLogEntry> pair : m_poseListEntries) { - pair.getFirst().ifChanged(ps -> pair.getSecond().append(ps)); + for (Pair, StructArrayLogEntry> pair : m_pose3dArrayEntries) { + pair.getSecond().update(pair.getFirst().get()); } } } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java index f38a2dda..8d49df50 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -47,7 +47,7 @@ public class LocalizationConstants { Map.entry("test", new Transform3d(0, 0, 0, new Rotation3d()))); private static Map kRealCameras = Map.ofEntries( - Map.entry("cam1", new Transform3d( // left tight + Map.entry("cam5", new Transform3d( // left tight new Translation3d(0.256, 0.289, kCameraHeight), new Rotation3d(0, kTightPitch, -kTightYaw + Units.degreesToRadians(0.9)))), Map.entry("cam2", new Transform3d( // left wide @@ -98,11 +98,12 @@ public class LocalizationConstants { protected static final double kAvgErr = 0.08; protected static final double kErrStdDevs = 0.02; - public static final boolean kEnableReefFilter = true; + public static final boolean kEnableTagFilter = true; - protected static final Set kReefIds = Set.of( - 6, 7, 8, 9, 10, 11, // red tags - 17, 18, 19, 20, 21, 22 // blue tags + protected static final Set kApprovedTagIds = Set.of( + 6, 7, 8, 9, 10, 11, // red reef tags + 17, 18, 19, 20, 21, 22, // blue reef tags + 3, 16 // processor tags are okay as well. ); } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java index ca8f08d3..d96b71f3 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java @@ -163,8 +163,8 @@ private boolean precheckValidity(PhotonPipelineResult result) { } // Ensure we only accept reef-focused estimates return result.hasTargets() - && (!LocalizationConstants.kEnableReefFilter - || LocalizationConstants.kReefIds.contains(result.getBestTarget().getFiducialId())); + && (!LocalizationConstants.kEnableTagFilter + || LocalizationConstants.kApprovedTagIds.contains(result.getBestTarget().getFiducialId())); } private Optional process(PhotonPipelineResult result, Pose3d pose) { diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index c987005b..5abd6129 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -14,8 +14,10 @@ import org.slf4j.LoggerFactory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; @@ -27,13 +29,18 @@ import frc.robot.vision.CameraIOInputsLogger; public class AlgaeTracker implements Runnable { - public static final boolean enabled = TrackingConstants.kEnabled; - public record ObjectTrackingStatus(Rotation2d yaw, double time, Optional pose) { + public record ObjectTrackingStatus( + Rotation2d yaw, + double time, + Optional pose) { public boolean isExpired() { return Timer.getTimestamp() - time() >= TrackingConstants.kExpirationTime.in(Seconds); } + public boolean isOkay() { + return !isExpired(); + } } @SuppressWarnings("unused") @@ -61,37 +68,55 @@ public AlgaeTracker(Supplier robotPose, Consumer a m_inputsLogger = new CameraIOInputsLogger(m_inputs, TrackingConstants.kCameraName); } + private void addResult(PhotonPipelineResult result) { + if (!result.hasTargets()) { + return; + } + PhotonTrackedTarget target = result.getBestTarget(); + Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().yaw); + + Optional estimatedDistance = estimateDistance(target); + + if (estimatedDistance.isEmpty()) { + m_action.accept(new ObjectTrackingStatus( + yaw, + Timer.getTimestamp(), + Optional.empty())); + return; + } + + double dist = estimatedDistance.get(); + // Determine camera-relative position of algae + Transform3d cameraOffset = new Transform3d( + yaw.getCos(), + yaw.getSin(), + getAlgaeHeight(target).minus(TrackingConstants.kRobotToCamera.getMeasureZ()).in(Meters), + Rotation3d.kZero).times(dist); + // Determine robot-relative position of algae + Pose2d robot = m_robotPose.get(); + Pose3d camera = new Pose3d(robot).transformBy(TrackingConstants.kRobotToCamera); + Pose3d algae = camera.transformBy(cameraOffset); + Translation2d relative = algae.toPose2d().relativeTo(robot).getTranslation(); + m_action.accept(new ObjectTrackingStatus( + relative.getAngle(), + Timer.getTimestamp(), + Optional.of(algae))); + } + public void run() { m_io.updateInputs(m_inputs); m_inputsLogger.log(); List results = m_inputs.unreadResults; - results.forEach(result -> { - if (!result.hasTargets()) { - return; - } - PhotonTrackedTarget target = result.getBestTarget(); - Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().yaw); - - Optional estimatedDistance = estimateDistance(target); - - estimatedDistance.ifPresentOrElse(dist -> { - Transform2d robotOffset = - new Transform2d(new Translation2d(yaw.getCos(), yaw.getSin()).times(dist), Rotation2d.kZero); - Pose2d algae = m_robotPose.get().transformBy(robotOffset); - m_action.accept(new ObjectTrackingStatus( - yaw, - Timer.getTimestamp(), - Optional.of(algae))); - }, () -> { - m_action.accept(new ObjectTrackingStatus( - yaw, - Timer.getTimestamp(), - Optional.empty())); - }); - - }); + results.forEach(this::addResult); + } + + private Distance getAlgaeHeight(PhotonTrackedTarget target) { + if (target.pitch >= 0) { + return TrackingConstants.kLollipopAlgaeHeight; + } + return TrackingConstants.kGroundAlgaeHeight; } private Optional estimateDistance(PhotonTrackedTarget target) { diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java index 117eb9cf..953159e4 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Seconds; import java.util.function.Supplier; + import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; @@ -23,14 +24,14 @@ public class CameraIOTrackingSim implements CameraIO { private final PhotonCamera m_camera; private final Supplier m_robotPose; - + private final static Transform3d offset = new Transform3d(0, 0, TrackingConstants.kGroundAlgaeHeight.in(Meters), Rotation3d.kZero); // simulation stuff only happens once private static final VisionSystemSim visionSim = new VisionSystemSim("tracking"); private static final TargetModel targetModel = new TargetModel(0.5); - private static final Pose3d initialTargetPose = new Pose3d(2, 2, 0, Rotation3d.kZero); + private static final Pose3d initialTargetPose = new Pose3d(2, 2, TrackingConstants.kLollipopAlgaeHeight.in(Meters), Rotation3d.kZero); private static final VisionTargetSim targetSim = new VisionTargetSim(initialTargetPose, targetModel); private static final SimCameraProperties simProps = new SimCameraProperties() diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java new file mode 100644 index 00000000..1b30f411 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java @@ -0,0 +1,59 @@ +package frc.robot.vision.tracking; + +import java.util.ArrayDeque; +import java.util.Optional; +import java.util.Queue; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.vision.tracking.AlgaeTracker.ObjectTrackingStatus; + +public class SimplePoseFilter { + private record PoseData(Translation3d pos, double trust) { + }; + + private final Queue m_data; + private static final int SIZE_LIMIT = 100; + + public SimplePoseFilter() { + m_data = new ArrayDeque<>(SIZE_LIMIT); + } + + public void add(ObjectTrackingStatus status) { + // Add status to list + m_data.add(status); + while (m_data.size() > SIZE_LIMIT) { + m_data.remove(); + } + } + + public Optional calculate() { + // Ensure that list is below size limit. + Optional result = m_data.stream() + .filter(ObjectTrackingStatus::isOkay) + .filter(s -> s.pose().isPresent()) + .map(s -> { + double trust = calculateTrust(s); + return new PoseData( + s.pose().get().getTranslation().times(trust), + trust); + }) + .reduce((a, b) -> new PoseData(a.pos().plus(b.pos()), a.trust() + b.trust())); + // If we have nothing, say so. + if (result.isEmpty()) { + return Optional.empty(); + } + PoseData data = result.get(); + Pose3d combined = new Pose3d(data.pos().div(data.trust()), Rotation3d.kZero); + return Optional.of(combined); + } + + private double calculateTrust(ObjectTrackingStatus status) { + double dt = Timer.getTimestamp() - status.time(); + if (dt == 0) { + return 1; + } + return 1 / dt; + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java index deda9daf..fe6a8c89 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java @@ -5,20 +5,22 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; class TrackingConstants { public static final boolean kEnabled = false; - protected static final String kCameraName = "camera"; + protected static final String kCameraName = "algaeman"; /* it is very important that the robot's position be ON THE GROUND (z = 0) */ - protected static final Transform3d kRobotToCamera = new Transform3d(0, 0, 0.3, Rotation3d.kZero); + // protected static final Transform3d kRobotToCamera = new Transform3d(Units.inchesToMeters(9), Units.inchesToMeters(10), Units.inchesToMeters(11), Rotation3d.kZero); + protected static final Transform3d kRobotToCamera = new Transform3d(Units.inchesToMeters(8.304 + 0.75), Units.inchesToMeters(9.75), Units.inchesToMeters(11), Rotation3d.kZero); /* simulation */ - protected static final double kFPS = 14; - protected static final int kResWidth = 640; - protected static final int kResHeight = 480; + protected static final double kFPS = 30; + protected static final int kResWidth = 320; + protected static final int kResHeight = 320; protected static final Rotation2d kFOVDiag = Rotation2d.fromDegrees(100); protected static final double kCalibError = 0.5; @@ -33,7 +35,7 @@ class TrackingConstants { protected static final Distance kGroundAlgaeHeight = Inches.of(8.125); protected static final Distance kLollipopAlgaeHeight = Inches.of(14.0625); - protected static final boolean kDistanceEstimationEnabled = false; + protected static final boolean kDistanceEstimationEnabled = true; protected static final Time kExpirationTime = Milliseconds.of(300); } diff --git a/ThriftyTest/vendordeps/Autopilot.json b/ThriftyTest/vendordeps/Autopilot.json index 115430ef..89514027 100644 --- a/ThriftyTest/vendordeps/Autopilot.json +++ b/ThriftyTest/vendordeps/Autopilot.json @@ -2,7 +2,7 @@ "name": "Autopilot", "uuid": "4e922d47-9954-4db5-929f-c708cc62e152", "fileName": "Autopilot.json", - "version": "1.2.1", + "version": "1.4.0", "frcYear": "2025", "jsonUrl": "https://therekrab.github.io/autopilot/vendordep.json", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.github.therekrab", "artifactId": "autopilot", - "version": "1.2.1" + "version": "1.4.0" } ], "cppDependencies": [],