From 6196da8a232a2cde858d286d1354070ad1b9d0d4 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Sun, 15 Mar 2026 08:42:33 +0800 Subject: [PATCH] Have Climber --- build.gradle | 2 +- .../autos/8 Balls Middle Start Blue.auto | 25 ++ ...art.auto => 8 Balls Middle Start Red.auto} | 0 .../pathplanner/autos/Depot Auto Blue.auto | 88 +++++++ .../{Depot Auto.auto => Depot Auto Red.auto} | 0 .../deploy/pathplanner/autos/New Auto.auto | 31 +++ .../autos/Shoot and Climb Blue.auto | 67 ++++++ ...nd Climb.auto => Shoot and Climb Red.auto} | 39 +-- .../autos/Shoot and Depot Blue.auto | 158 +++++++++++++ .../autos/Shoot and Depot Red.auto | 139 +++++++++++ ...=> Shoot and Depot Without Climb Red.auto} | 59 +++-- .../autos/Shoot and Depot and Climb Blue.auto | 151 ++++++++++++ .../autos/Shoot and Depot and Climb Red.auto | 157 ++++++++++++ .../Test of individual command Blue.auto | 31 +++ .../autos/Test of individual command.auto | 25 ++ .../pathplanner/paths/Climb Step2 Red.path | 8 +- .../pathplanner/paths/Climb Step3 Blue.path | 12 +- .../pathplanner/paths/Climb Step3 Red.path | 8 +- .../paths/Copy of blue depot go 1.path | 63 +++++ .../pathplanner/paths/Depot Intake Blue.path | 14 +- .../pathplanner/paths/Depot Intake Red.path | 4 +- .../paths/Depot To Climb Blue.path | 54 +++++ .../pathplanner/paths/Depot To Climb Red.path | 54 +++++ .../pathplanner/paths/Depot to Shoot Red.path | 14 +- .../deploy/pathplanner/paths/New Path.path | 54 +++++ .../paths/Shoot to Climb Step1 Red.path | 18 +- .../paths/Shoot to Depot Blue.path | 6 +- .../pathplanner/paths/Shoot to Depot Red.path | 14 +- .../pathplanner/paths/To Depot Blue.path | 54 +++++ .../pathplanner/paths/blue depot go 1.path | 95 ++++++++ src/main/deploy/pathplanner/settings.json | 3 +- src/main/java/frc/robot/Constants.java | 5 +- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 25 +- .../java/frc/robot/controls/Autocommand.java | 87 ++++--- .../frc/robot/controls/ClimberControls.java | 2 +- .../frc/robot/controls/DriveControls.java | 101 +++++--- .../frc/robot/controls/IntakerControls.java | 7 +- .../frc/robot/controls/ShooterControls.java | 42 +++- .../robot/subsystems/ClimberSubsystem.java | 12 - .../subsystems/CommandSwerveDrivetrain.java | 223 +++++++++--------- .../robot/subsystems/IntakerSubsystem.java | 19 +- .../robot/subsystems/LimelightSupplier.java | 108 --------- .../robot/subsystems/ShooterSubsystem.java | 1 + 44 files changed, 1644 insertions(+), 439 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto rename src/main/deploy/pathplanner/autos/{8 Balls Middle Start.auto => 8 Balls Middle Start Red.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Depot Auto Blue.auto rename src/main/deploy/pathplanner/autos/{Depot Auto.auto => Depot Auto Red.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto rename src/main/deploy/pathplanner/autos/{Shoot and Climb.auto => Shoot and Climb Red.auto} (71%) create mode 100644 src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto create mode 100644 src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto rename src/main/deploy/pathplanner/autos/{Shoot and Depot.auto => Shoot and Depot Without Climb Red.auto} (76%) create mode 100644 src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto create mode 100644 src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto create mode 100644 src/main/deploy/pathplanner/autos/Test of individual command Blue.auto create mode 100644 src/main/deploy/pathplanner/autos/Test of individual command.auto create mode 100644 src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path create mode 100644 src/main/deploy/pathplanner/paths/Depot To Climb Blue.path create mode 100644 src/main/deploy/pathplanner/paths/Depot To Climb Red.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/To Depot Blue.path create mode 100644 src/main/deploy/pathplanner/paths/blue depot go 1.path delete mode 100644 src/main/java/frc/robot/subsystems/LimelightSupplier.java diff --git a/build.gradle b/build.gradle index 40494f8..42337d8 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto new file mode 100644 index 0000000..f434864 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Blue" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/8 Balls Middle Start.auto b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/8 Balls Middle Start.auto rename to src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto diff --git a/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto b/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto new file mode 100644 index 0000000..306d117 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intaker_down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "To Depot Blue" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot Intake Blue" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intaker_up" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot to Shoot Blue" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Depot Auto Red.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Depot Auto.auto rename to src/main/deploy/pathplanner/autos/Depot Auto Red.auto diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..2bfb51c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New Path" + } + }, + { + "type": "named", + "data": { + "name": "AutoAim" + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto b/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto new file mode 100644 index 0000000..e6f9ac9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Blue" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to Climb Step1 Blue" + } + }, + { + "type": "named", + "data": { + "name": "climb_up" + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step2 Blue" + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step3 Blue" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + }, + { + "type": "named", + "data": { + "name": "climb_down" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Climb.auto b/src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto similarity index 71% rename from src/main/deploy/pathplanner/autos/Shoot and Climb.auto rename to src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto index e259fe9..79862b3 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Climb.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto @@ -5,9 +5,22 @@ "data": { "commands": [ { - "type": "path", + "type": "parallel", "data": { - "pathName": "Back To Shoot Red" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Red" + } + }, + { + "type": "named", + "data": { + "name": "climber_init" + } + } + ] } }, { @@ -17,15 +30,15 @@ } }, { - "type": "path", + "type": "wait", "data": { - "pathName": "Shoot to Climb Step1 Red" + "waitTime": 0.25 } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 1.0 + "pathName": "Shoot to Climb Step1 Red" } }, { @@ -34,24 +47,12 @@ "name": "climb_up" } }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, { "type": "path", "data": { "pathName": "Climb Step2 Red" } }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, { "type": "path", "data": { @@ -61,7 +62,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 0.25 } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto new file mode 100644 index 0000000..ca97962 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto @@ -0,0 +1,158 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Blue" + } + }, + { + "type": "named", + "data": { + "name": "intaker_down" + } + }, + { + "type": "named", + "data": { + "name": "climber_init" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intaker_down" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "start_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot to Depot Blue" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Depot Intake Blue" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoAim" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "stop_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "Shake" + } + }, + { + "type": "named", + "data": { + "name": "intaker_up" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "blue depot go 1" + } + }, + { + "type": "named", + "data": { + "name": "start_intake" + } + }, + { + "type": "named", + "data": { + "name": "reset_backboard0" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto new file mode 100644 index 0000000..12c4570 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto @@ -0,0 +1,139 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Red" + } + }, + { + "type": "named", + "data": { + "name": "intaker_down" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot to Depot Red" + } + }, + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "start_intake_timelimit" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot Intake Red" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intaker_up" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot to Shoot Red" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoAim" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "Shake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "Shake" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto similarity index 76% rename from src/main/deploy/pathplanner/autos/Shoot and Depot.auto rename to src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto index 8e8d0d1..f8fafd7 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Depot.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto @@ -19,6 +19,12 @@ "data": { "name": "intaker_down" } + }, + { + "type": "named", + "data": { + "name": "climber_init" + } } ] } @@ -29,6 +35,12 @@ "name": "shoot" } }, + { + "type": "named", + "data": { + "name": "start_intake_and_conveyor" + } + }, { "type": "parallel", "data": { @@ -38,12 +50,6 @@ "data": { "pathName": "Shoot to Depot Red" } - }, - { - "type": "named", - "data": { - "name": "start_intake_timelimit" - } } ] } @@ -52,12 +58,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "start_intake_timelimit" - } - }, { "type": "path", "data": { @@ -74,13 +74,7 @@ { "type": "named", "data": { - "name": "intaker_up" - } - }, - { - "type": "path", - "data": { - "pathName": "Depot to Shoot Red" + "name": "AutoAim" } } ] @@ -89,7 +83,32 @@ { "type": "named", "data": { - "name": "shoot" + "name": "stop_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "Shake" + } + }, + { + "type": "named", + "data": { + "name": "intaker_up" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto new file mode 100644 index 0000000..a8f54e1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto @@ -0,0 +1,151 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Blue" + } + }, + { + "type": "named", + "data": { + "name": "intaker_down" + } + }, + { + "type": "named", + "data": { + "name": "climber_init" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "start_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot to Depot Blue" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Depot Intake Blue" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoAim" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "stop_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "Shake" + } + }, + { + "type": "named", + "data": { + "name": "intaker_up" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Depot To Climb Blue" + } + }, + { + "type": "named", + "data": { + "name": "climb_up" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step2 Blue" + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step3 Blue" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto new file mode 100644 index 0000000..1297783 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto @@ -0,0 +1,157 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Back To Shoot Red" + } + }, + { + "type": "named", + "data": { + "name": "intaker_down" + } + }, + { + "type": "named", + "data": { + "name": "climber_init" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "start_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot to Depot Red" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Depot Intake Red" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoAim" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "stop_intake_and_conveyor" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoShoot" + } + }, + { + "type": "named", + "data": { + "name": "Shake" + } + }, + { + "type": "named", + "data": { + "name": "intaker_up" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Depot To Climb Red" + } + }, + { + "type": "named", + "data": { + "name": "climb_up" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step2 Red" + } + }, + { + "type": "path", + "data": { + "pathName": "Climb Step3 Red" + } + }, + { + "type": "named", + "data": { + "name": "climb_down" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test of individual command Blue.auto b/src/main/deploy/pathplanner/autos/Test of individual command Blue.auto new file mode 100644 index 0000000..e465381 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test of individual command Blue.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "start_intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "stop_intake" + } + } + ] + } + }, + "resetOdom": false, + "folder": "Blue Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test of individual command.auto b/src/main/deploy/pathplanner/autos/Test of individual command.auto new file mode 100644 index 0000000..bf6b6c0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test of individual command.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "start_intake_and_conveyor" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": false, + "folder": "Red Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Climb Step2 Red.path b/src/main/deploy/pathplanner/paths/Climb Step2 Red.path index 393510c..b74cdb3 100644 --- a/src/main/deploy/pathplanner/paths/Climb Step2 Red.path +++ b/src/main/deploy/pathplanner/paths/Climb Step2 Red.path @@ -4,12 +4,12 @@ { "anchor": { "x": 15.9, - "y": 3.365 + "y": 3.465 }, "prevControl": null, "nextControl": { "x": 15.522355156858092, - "y": 3.365 + "y": 3.465 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 15.517, - "y": 3.365 + "y": 3.465 }, "prevControl": { "x": 15.77785583456356, - "y": 3.365 + "y": 3.465 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Climb Step3 Blue.path b/src/main/deploy/pathplanner/paths/Climb Step3 Blue.path index c7e7f89..59939b1 100644 --- a/src/main/deploy/pathplanner/paths/Climb Step3 Blue.path +++ b/src/main/deploy/pathplanner/paths/Climb Step3 Blue.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.3229961597714217, - "y": 4.702614323435373 + "x": 1.323, + "y": 4.704 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.604 }, "prevControl": { - "x": 0.7939837109919805, - "y": 4.592976954720596 + "x": 0.7925211774538647, + "y": 4.604 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Blue Middle", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Climb Step3 Red.path b/src/main/deploy/pathplanner/paths/Climb Step3 Red.path index b84853e..d7faf95 100644 --- a/src/main/deploy/pathplanner/paths/Climb Step3 Red.path +++ b/src/main/deploy/pathplanner/paths/Climb Step3 Red.path @@ -4,12 +4,12 @@ { "anchor": { "x": 15.517, - "y": 3.365 + "y": 3.465 }, "prevControl": null, "nextControl": { "x": 15.767, - "y": 3.365 + "y": 3.465 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 15.517, - "y": 3.465 + "y": 3.565 }, "prevControl": { "x": 15.239842716853756, - "y": 3.465 + "y": 3.565 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path b/src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path new file mode 100644 index 0000000..818f060 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.64, + "y": 5.969 + }, + "prevControl": null, + "nextControl": { + "x": 0.9200056277857709, + "y": 5.902393157337589 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.485927152317881, + "y": 5.276448675496688 + }, + "prevControl": { + "x": 5.220827814571248, + "y": 5.6757450331143255 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 0.68, + "rotationDegrees": 135.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot Intake Blue.path b/src/main/deploy/pathplanner/paths/Depot Intake Blue.path index 19363a7..32a4aa5 100644 --- a/src/main/deploy/pathplanner/paths/Depot Intake Blue.path +++ b/src/main/deploy/pathplanner/paths/Depot Intake Blue.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.5191395924055082, - "y": 5.975819656893259 + "x": 1.39, + "y": 5.969 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.54, + "x": 0.64, "y": 5.969 }, "prevControl": { - "x": 0.2904111750287428, - "y": 5.954667573496888 + "x": 0.39, + "y": 5.969 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 3.0, + "maxVelocity": 0.25, + "maxAcceleration": 0.5, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Depot Intake Red.path b/src/main/deploy/pathplanner/paths/Depot Intake Red.path index 477aaa1..f6f7748 100644 --- a/src/main/deploy/pathplanner/paths/Depot Intake Red.path +++ b/src/main/deploy/pathplanner/paths/Depot Intake Red.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 16.0, + "x": 15.9, "y": 2.1 }, "prevControl": { - "x": 15.75, + "x": 15.65, "y": 2.1 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Depot To Climb Blue.path b/src/main/deploy/pathplanner/paths/Depot To Climb Blue.path new file mode 100644 index 0000000..2546f7b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot To Climb Blue.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.64, + "y": 5.97 + }, + "prevControl": null, + "nextControl": { + "x": 0.6399999999999998, + "y": 4.9700000000000015 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.64, + "y": 4.705 + }, + "prevControl": { + "x": 0.6400000000000002, + "y": 5.705 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Blue Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Climb Red.path b/src/main/deploy/pathplanner/paths/Depot To Climb Red.path new file mode 100644 index 0000000..18cfaf7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot To Climb Red.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 15.9, + "y": 2.1 + }, + "prevControl": null, + "nextControl": { + "x": 15.9, + "y": 3.0999999999999983 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.9, + "y": 3.465 + }, + "prevControl": { + "x": 15.9, + "y": 2.465000000000005 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot to Shoot Red.path b/src/main/deploy/pathplanner/paths/Depot to Shoot Red.path index 3550f71..8f261f1 100644 --- a/src/main/deploy/pathplanner/paths/Depot to Shoot Red.path +++ b/src/main/deploy/pathplanner/paths/Depot to Shoot Red.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 16.024211489072027, - "y": 1.797481861769637 + "x": 16.18921636103803, + "y": 1.647541846848796 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.0 }, "prevControl": { - "x": 13.313026105373341, - "y": 4.165954098256533 + "x": 13.247149348869588, + "y": 4.238251681778358 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..9afa2c7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 2.5 + }, + "prevControl": null, + "nextControl": { + "x": 3.8524186062054477, + "y": 2.382931901404091 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 3.0 + }, + "prevControl": { + "x": 1.7628130762195064, + "y": 3.0790086272982027 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Blue Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path b/src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path index 109f8ca..23d29ae 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 13.951314803780512, - "y": 3.7446544288446453 + "x": 14.25, + "y": 4.0 }, "isLocked": true, "linkedName": null @@ -17,15 +17,15 @@ { "anchor": { "x": 14.567, - "y": 3.3650694444444444 + "y": 3.4 }, "prevControl": { - "x": 14.32971291116739, - "y": 3.4437767342805516 + "x": 14.088847622018482, + "y": 3.5461858523613685 }, "nextControl": { - "x": 14.804287088832611, - "y": 3.2863621546083372 + "x": 14.806076188990758, + "y": 3.326907073819316 }, "isLocked": true, "linkedName": null @@ -36,8 +36,8 @@ "y": 3.365 }, "prevControl": { - "x": 15.593954129696199, - "y": 3.387182687738287 + "x": 15.572533463420765, + "y": 3.399470424413628 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path b/src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path index 7bd955e..76efba6 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Depot Red.path b/src/main/deploy/pathplanner/paths/Shoot to Depot Red.path index f821231..44b7e83 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Depot Red.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Depot Red.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 13.72461981440462, - "y": 3.8902460070118927 + "x": 13.699377937127288, + "y": 3.7771427571164287 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.1 }, "prevControl": { - "x": 15.201612527520673, - "y": 2.25153433233069 + "x": 15.073650526430217, + "y": 2.4474296685616617 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/To Depot Blue.path b/src/main/deploy/pathplanner/paths/To Depot Blue.path new file mode 100644 index 0000000..02f6a66 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/To Depot Blue.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.54, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.539999999999999, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.14, + "y": 5.97 + }, + "prevControl": { + "x": 2.1399999999999992, + "y": 5.97 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Blue Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue depot go 1.path b/src/main/deploy/pathplanner/paths/blue depot go 1.path new file mode 100644 index 0000000..aa3ea3b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/blue depot go 1.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.64, + "y": 5.969 + }, + "prevControl": null, + "nextControl": { + "x": 1.678433584781481, + "y": 5.9229373186851895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6943708609271524, + "y": 7.381829470198674 + }, + "prevControl": { + "x": 2.178364289572292, + "y": 7.226083943133918 + }, + "nextControl": { + "x": 3.0531960414062747, + "y": 7.490133162361142 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 7.2 + }, + "prevControl": { + "x": 5.804119228777829, + "y": 7.355340669064503 + }, + "nextControl": { + "x": 6.195880771222171, + "y": 7.044659330935497 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.195529801324503, + "y": 5.969 + }, + "prevControl": { + "x": 7.354483593458411, + "y": 5.7760396621938845 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.7, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 135.0 + }, + "reversed": false, + "folder": "Blue Middle", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "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 42b52ce..9a4cd76 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -7,7 +7,8 @@ "Red Middle" ], "autoFolders": [ - "Red Middle" + "Red Middle", + "Blue Middle" ], "defaultMaxVel": 1.5, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a873160..b2962fe 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,7 +17,8 @@ public final class Constants { CrossingTrench } public class Shooter { - public static final double conveyorSpeed =25; + public static final double conveyorSpeed = 25; + public static final double conveyorSpeedForAuto = 10; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) .withKV(0.099).withKA(0.0).withKS(0.0); @@ -135,7 +136,7 @@ public final class Constants { .withKV(0.00).withKA(0.00).withKS(0.00); //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI) - public static final double IntakeGetterSpeedforAuto = 0.4; + public static final double IntakeGetterSpeedforAuto = 0.5; // 单位RPS public static final double positionToleranceRotations = 0.00; //rotater容差 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3510e8b..d7b6c33 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,7 +50,9 @@ public class Robot extends TimedRobot { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + m_robotContainer.driveControls.pushDistanceData(); + } @Override public void autonomousExit() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ff63cd9..7467f63 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,7 +58,7 @@ public class RobotContainer { /* 控制封装/日志 */ /* ====================== */ - private final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager ); + public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager ); private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager); private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper); private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper); @@ -82,7 +82,6 @@ public class RobotContainer { configueClimber(); bingdingLED(); configueIntaker(); - configureBindings(); Autocommand.preNameCommands( climberSubsystem, IntakerSubsystem, @@ -134,6 +133,12 @@ public class RobotContainer { // Start：强行使用metaTag2全场定位 controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2)); + controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter); + System.out.println("Try using shooter for metatag2"); + })); + controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker); + System.out.println("Try using intaker for metatag2"); + })); //LM 按下时autoaimming，松开时alltelop // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); @@ -169,8 +174,7 @@ public class RobotContainer { controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets())); controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand())); - controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem)); - controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem)); + } /* ====================== */ /* LED 绑定 */ @@ -202,19 +206,6 @@ public class RobotContainer { controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem)); } - private void configureBindings() { - teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand()); - } - - private Command handleClimberAtTeleopStartCommand() { - return new InstantCommand(() -> { - if (climberSubsystem.wasAutoClimbUsedInAuto()) { - climberSubsystem.climbUp(); - climberSubsystem.clearAutoClimbUsedInAutoFlag(); - } - }, climberSubsystem); - } - public ClimberSubsystem getClimberSubsystem() { return climberSubsystem; } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index d4eaccb..48c795b 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.RobotStatusManager; import frc.robot.subsystems.IntakerSubsystem; -import frc.robot.subsystems.LimelightSupplier; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.Constants; @@ -34,16 +33,19 @@ import frc.robot.Constants.RobotStatus; NamedCommands.registerCommand("climb_up", climb_up(climber)); NamedCommands.registerCommand("climb_down", climb_down(climber)); NamedCommands.registerCommand("start_intake", start_intake(intaker)); + NamedCommands.registerCommand("start_intake_and_conveyor", start_intake_and_conveyor(intaker,shooter)); + NamedCommands.registerCommand("stop_intake_and_conveyor", stop_intake_and_conveyor(intaker,shooter)); NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker)); NamedCommands.registerCommand("stop_intake", stop_intake(intaker)); NamedCommands.registerCommand("intaker_down", intaker_down(intaker)); NamedCommands.registerCommand("intaker_up", intaker_up(intaker)); - NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain)); + //NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain)); NamedCommands.registerCommand("shoot", shoot(shooter)); NamedCommands.registerCommand("Shake", shake(driveControls)); NamedCommands.registerCommand("AutoAim", autoAim(driveControls)); NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls)); NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber)); + NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls)); } @@ -58,12 +60,9 @@ import frc.robot.Constants.RobotStatus; }, climber)); } public static Command climb_down(ClimberSubsystem climber) { - return new InstantCommand(() -> { - climber.markAutoClimbUsedInAuto(); // 标记自动攀爬已使用 - }, climber) - .andThen(new RunCommand(() -> { + return new RunCommand(() -> { climber.climbDown(); // 控制爬升器下降 - }, climber).withTimeout(2.0)) + }, climber).withTimeout(2.0) .andThen(new InstantCommand(() -> { climber.captureHoldPosition(); // 捕获当前爬升器位置以保持 climber.holdPosition(); // 停止并锁定当前爬升器位置 @@ -71,51 +70,70 @@ import frc.robot.Constants.RobotStatus; } public static Command start_intake(IntakerSubsystem intaker) { - return new RunCommand(() -> { + return new InstantCommand(() -> { intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球 }, intaker); } + public static Command start_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) { + return new InstantCommand(() -> { + shooter.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeedForAuto); + intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球 + + }, intaker, shooter); + } + public static Command stop_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) { + return new InstantCommand(() -> { + intaker.setIntakerGetterSpeed(0); // 启动吸球 + shooter.setConveyorSpeedByRPS(0); + }, intaker, shooter); + } public static Command start_intake_timelimit(IntakerSubsystem intaker) { return new RunCommand(() -> { intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 吸球机构下降 - }, intaker).withTimeout(3).andThen(new InstantCommand(() -> { + }, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> { intaker.setIntakerGetterSpeed(0); // 停止旋转 }, intaker)); } public static Command stop_intake(IntakerSubsystem intaker) { - return new InstantCommand(() -> { + return new RunCommand(() -> { intaker.setIntakerGetterSpeed(0); // 停止吸球 }, intaker); } public static Command intaker_down(IntakerSubsystem intaker) { return new RunCommand(() -> { - intaker.IntakerDownNonStop(); // 吸球机构下降 - }, intaker).withTimeout(1.0).andThen(new InstantCommand(() -> { - intaker.stopIntakerotater(); // 停止旋转 + intaker.IntakerDownNonStop(); // 吸球机构下降 + }, intaker).withTimeout(0.6) + + .andThen(new RunCommand(() -> { + intaker.IntakerUpNonStop(); // 吸球机构上升 + }, intaker).withTimeout(0.1)) + + .andThen(new InstantCommand(() -> { + intaker.stopIntakerotater(); // 停止 }, intaker)); } public static Command intaker_up(IntakerSubsystem intaker) { - return new InstantCommand(() -> { + return new RunCommand(() -> { intaker.IntakerUpNonStop(); // 吸球机构上升 - }, intaker).withTimeout(0.8).andThen(new InstantCommand(() -> { - intaker.stopIntakerotater();; // 停止旋转 + }, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> { + intaker.stopIntakerotater(); // 停止旋转 }, intaker)); } - public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) { - return Commands.sequence( - new InstantCommand(() -> { - LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); - }), - Commands.runEnd( - () -> { - drivetrain.driveToAprilTag(); - }, - () -> { - LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); - }, - drivetrain - ).until(() -> drivetrain.isAlignedToAprilTag())); - } + // public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) { + // return Commands.sequence( + // new InstantCommand(() -> { + // LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); + // }), + // Commands.runEnd( + // () -> { + // drivetrain.driveToAprilTag(); + // }, + // () -> { + // LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); + // }, + // drivetrain + // ).until(() -> drivetrain.isAlignedToAprilTag())); + // } public static Command shoot(ShooterSubsystem shooter) { return new RunCommand(() -> { @@ -126,7 +144,7 @@ import frc.robot.Constants.RobotStatus; }else{ shooter.outputBackboard(); } - }, shooter).withTimeout(1.0).andThen(new RunCommand(() -> { + }, shooter).withTimeout(0.25).andThen(new RunCommand(() -> { shooter.setFlywheelSpeedByRPS(64); shooter.setBackboardPosition(175); if(shooter.isBackboardAtTarget()){ @@ -135,7 +153,7 @@ import frc.robot.Constants.RobotStatus; shooter.outputBackboard(); } shooter.setConveyorSpeedByRPS(25); - }, shooter).withTimeout(3.0).andThen(new InstantCommand(() -> { + }, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> { shooter.setFlywheelSpeedByRPS(0); // 停止射击 //shooter.setBackboardPosition(0); // 恢复背板位置 shooter.setConveyorSpeedByRPS(0); // 停止输送 @@ -148,6 +166,9 @@ import frc.robot.Constants.RobotStatus; public static Command autoShoot(ShooterControls shooterControl) { return shooterControl.autoShootToHubCommand(); } + public static Command resetBackboard0Command(ShooterControls shooterControl) { + return shooterControl.resetBackboard0Command(); + } public static Command shake(DriveControls driveControls) { return driveControls.shakeCommand(); diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java index d52bc87..358e6da 100644 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ b/src/main/java/frc/robot/controls/ClimberControls.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.subsystems.ClimberSubsystem; -import frc.robot.subsystems.LimelightSupplier; + public class ClimberControls { diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index a73a46e..85eb28d 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -20,7 +20,6 @@ import frc.robot.Constants.DriveMode; import frc.robot.Constants.RobotStatus; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.LimelightSupplier; import java.util.Optional; import edu.wpi.first.wpilibj.DriverStation; @@ -138,12 +137,12 @@ public class DriveControls { vomega = 0; break; case PassingBall: - LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); + //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); break; case AutoAimming: - LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); + //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); vx = -driver.getLeftY() * maxSpeed * slowDriveScale; @@ -153,7 +152,7 @@ public class DriveControls { autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); break; case Climbing: - alignToClimb(); + // alignToClimb(); break; case CrossingBump: double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); @@ -406,16 +405,16 @@ public class DriveControls { public Command getAllianceColorCommand() { return Commands.runOnce(this::setTeamColors); } - public void alignToClimb(){ - LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); - if(!drivetrain.isAlignedToAprilTag()){ - drivetrain.driveToAprilTag(); - } - else{ - LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); - drivetrain.stop(); - } - } + // public void alignToClimb(){ + // LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); + // if(!drivetrain.isAlignedToAprilTag()){ + // drivetrain.driveToAprilTag(); + // } + // else{ + // LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); + // drivetrain.stop(); + // } + // } public Command autoAimCommand() { return drivetrain.applyRequest(() -> { double[] distanceAndRotation = calculateDistanceAndRotationToHub(); @@ -432,28 +431,60 @@ public class DriveControls { }, drivetrain)); } public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) { - Timer timer = new Timer(); + Timer timer = new Timer(); - return Commands.sequence( - new InstantCommand(timer::restart), - drivetrain.applyRequest(() -> { - double t = timer.get(); - boolean positive = ((int) (t / switchPeriod)) % 2 == 0; - double vy = positive ? amplitude : -amplitude; - return robotCentric - .withVelocityX(0.0) - .withVelocityY(vy) - .withRotationalRate(0.0); - }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime), - new InstantCommand(() -> { - timer.stop(); - autoControlNetworkTable.getEntry("ShakeVy").setDouble(0.0); - drivetrain.stop(); - }, drivetrain) - ); - } - public Command shakeCommand() { - return shakeCommand(0.25, 0.2, 3.0); + return Commands.sequence( + new InstantCommand(timer::restart), + drivetrain.applyRequest(() -> { + double t = timer.get(); + int phase = ((int) (t / switchPeriod)) % 4; + + double vx = 0.0; + double vy = 0.0; + double[] distanceAndRotation = calculateDistanceAndRotationToHub(); + double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); + + + switch (phase) { + case 0: + vx = amplitude; // 前 + vy = 0.0; + break; + case 1: + vx = 0.0; + vy = amplitude; // 右 + break; + case 2: + vx = -amplitude; // 后 + vy = 0.0; + break; + case 3: + vx = 0.0; + vy = -amplitude; // 左 + break; + } + + return robotCentric + .withVelocityX(vx) + .withVelocityY(vy) + .withRotationalRate(vomega); + }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime), + new InstantCommand(() -> { + timer.stop(); + drivetrain.stop(); + }, drivetrain) + ); +} + + public Command shakeCommand() { + return shakeCommand(0.3, 0.15, 5.0); + } + + public void pushDistanceData(){ + double[] distanceAndRotation = calculateDistanceAndRotationToHub(); + // 将距离值写入 NetworkTable + autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); + autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); } } diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java index aabc875..0937c78 100644 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ b/src/main/java/frc/robot/controls/IntakerControls.java @@ -73,12 +73,10 @@ public class IntakerControls { public Command defaultIntakerCommand() { return Commands.run(() -> { double getterspeed = 0; - double conveyoyspeedforintake = 0; double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent(); if (controller.getLeftTriggerAxis() > 0.1) { - getterspeed = controller.getLeftTriggerAxis() * 0.3 + 0.3; - ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake); + getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3; } IntakerSubsystem.setIntakerGetterSpeed(getterspeed); @@ -87,9 +85,6 @@ public class IntakerControls { IntakerSubsystem.IntakerUpNonStop(); } else if(controller.getLeftY() > 0.5){ IntakerSubsystem.IntakerDownNonStop(); - // double rotaterspeed = -controller.getLeftY() * 0.3 + 0.2; - // IntakerSubsystem.setIntakerRotater(rotaterspeed); - } else { IntakerSubsystem.stopIntakerotater(); } diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index a67a501..ac98a70 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -47,14 +47,16 @@ public class ShooterControls { }else{ shooterSubsystem.outputBackboard(); } + shooterSubsystem.setFlywheelSpeedByRPS(0); + if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);} + else{shooterSubsystem.setConveyorSpeedByRPS(0);} break; case PassingBall: var xP = shooterSubsystem.getDistanceToPassball(); - xP = Math.max(1.2, Math.min(4.5, xP)); - flywheelSpeed = -1.4964026859575532*xP*xP*xP*xP*xP*xP*xP+29.82445640241506*xP*xP*xP*xP*xP*xP-247.71603153159896*xP*xP*xP*xP*xP+1108.8727302184625*xP*xP*xP*xP-2881.517500617481*xP*xP*xP+4332.05363145275*xP*xP-3467.2508337871454*xP+1192.1508062663759; + xP = Math.max(2.5, Math.min(10.0, xP)); + flywheelSpeed = +0.004960317456971805*xP*xP*xP*xP*xP*xP*xP-0.22361111097424535*xP*xP*xP*xP*xP*xP+4.234722219890569*xP*xP*xP*xP*xP-43.63194442298942*xP*xP*xP*xP+264.05555544032126*xP*xP*xP-938.6444440829168*xP*xP+1818.2047612908214*xP-1415.9999995643104; flywheelSpeed += flywheelSpeedOffset; - backboardPosition = +34.5286998307899*xP*xP*xP*xP*xP*xP*xP*xP-788.5788623683884*xP*xP*xP*xP*xP*xP*xP+7658.073303693148*xP*xP*xP*xP*xP*xP-41191.69338972519*xP*xP*xP*xP*xP+133870.54588688645*xP*xP*xP*xP-268559.6987062158*xP*xP*xP+324254.6530324262*xP*xP-215015.48893131423*xP+59977.46545382827; - backboardPosition += backboardPositionOffset; + backboardPosition = +0.10912698402591796*xP*xP*xP*xP*xP*xP*xP-5.277777773586415*xP*xP*xP*xP*xP*xP+105.55555548303796*xP*xP*xP*xP*xP-1125.6944437656143*xP*xP*xP*xP+6865.763885174154*xP*xP*xP-23769.027765888906*xP*xP+43328.57140795847*xP-31599.99998506075; conveyorSpeed = Constants.Shooter.conveyorSpeed; if (controller.getRightTriggerAxis() < 0.1) { flywheelSpeed = 0; @@ -63,6 +65,7 @@ public class ShooterControls { if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()= flywheelTargetSpeed * 0.9 ) - .withTimeout(1.0) + .withTimeout(0.3) .andThen(new RunCommand(() -> { shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed); shooterSubsystem.setBackboardPosition(backboardTargetPosition); @@ -191,12 +195,8 @@ public class ShooterControls { } else { shooterSubsystem.outputBackboard(); } - if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) { - shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed); - } else { - shooterSubsystem.setConveyorSpeedByRPS(0.0); - } - }, shooterSubsystem).withTimeout(3.0)) + shooterSubsystem.setConveyorSpeedByRPS(25); + }, shooterSubsystem).withTimeout(5.0)) .andThen(new InstantCommand(() -> { shooterSubsystem.setFlywheelSpeedByRPS(0.0); shooterSubsystem.setConveyorSpeedByRPS(0.0); @@ -204,4 +204,22 @@ public class ShooterControls { }, shooterSubsystem)); }, Set.of(shooterSubsystem)); } -} \ No newline at end of file + + public Command resetBackboard0Command() { + return new RunCommand(() -> { + shooterSubsystem.setBackboardPosition(0); + + if (shooterSubsystem.isBackboardAtTarget()) { + shooterSubsystem.backboardMotor.set(0); + } else { + shooterSubsystem.outputBackboard(); + } + + }, shooterSubsystem) + .until(() -> shooterSubsystem.isBackboardAtTarget()) + .andThen(new InstantCommand(() -> { + shooterSubsystem.backboardMotor.set(0); + })); + } +} + diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index f65ab12..d2c3284 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -114,18 +114,6 @@ public class ClimberSubsystem extends SubsystemBase { stopMotor(); } - public void markAutoClimbUsedInAuto() { - autoClimbUsedInAuto = true; - } - - public boolean wasAutoClimbUsedInAuto() { - return autoClimbUsedInAuto; - } - - public void clearAutoClimbUsedInAutoFlag() { - autoClimbUsedInAuto = false; - } - public void stopAll() { climberMotor.set(0); } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index f835e94..585e787 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -173,6 +173,17 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs); } + public void forceUsingLimelightmt2WithllName(String llString) { + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString); + + if (mt2 == null || mt2.tagCount == 0) { + return; + } + + var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999); + addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs); + } private void fuseLimelightmt2() { Pose2d current = getState().Pose; @@ -276,10 +287,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略） //Pose2d current = getState().Pose; - if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) { - //System.out.println("NoUsingMetaTag2ForTooChangingPose"); - return false; - } + // if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) { + // //System.out.println("NoUsingMetaTag2ForTooChangingPose"); + // return false; + // } return true; } @@ -287,128 +298,128 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return Math.max(min, Math.min(max, value)); } - public String driveToAprilTag() { - System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); - System.out.println("TV = " + LimelightSupplier.isTargetVisible()); +// public String driveToAprilTag() { +// System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); +// System.out.println("TV = " + LimelightSupplier.isTargetVisible()); - if (!LimelightSupplier.isTargetVisible()) { - isAlignedToAprilTag = false; - //stop(); - return "No Target"; - } +// if (!LimelightSupplier.isTargetVisible()) { +// isAlignedToAprilTag = false; +// //stop(); +// return "No Target"; +// } - RawFiducial[] fiducials = - LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); +// RawFiducial[] fiducials = +// LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); - boolean foundDesiredTag = false; - for (RawFiducial fiducial : fiducials) { - if (fiducial.id == 16 || fiducial.id == 32) { - foundDesiredTag = true; - break; - } - } +// boolean foundDesiredTag = false; +// for (RawFiducial fiducial : fiducials) { +// if (fiducial.id == 16 || fiducial.id == 32) { +// foundDesiredTag = true; +// break; +// } +// } - if (!foundDesiredTag) { - isAlignedToAprilTag = false; - stop(); - return "No Desired Tag"; - } +// if (!foundDesiredTag) { +// isAlignedToAprilTag = false; +// stop(); +// return "No Desired Tag"; +// } - // ===== 读取原始误差 ===== - double tx = LimelightSupplier.getTX(); // deg - double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号 - double ry = LimelightSupplier.getTargetRotationY(); // deg +// // ===== 读取原始误差 ===== +// double tx = LimelightSupplier.getTX(); // deg +// double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号 +// double ry = LimelightSupplier.getTargetRotationY(); // deg - // ===== 用原始误差判断是否已对齐 ===== - isAlignedToAprilTag = - Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX && - Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ && - Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY; +// // ===== 用原始误差判断是否已对齐 ===== +// isAlignedToAprilTag = +// Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX && +// Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ && +// Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY; - if (isAlignedToAprilTag) { - stop(); - return "Aligned"; - } +// if (isAlignedToAprilTag) { +// stop(); +// return "Aligned"; +// } - // ===== 计算输出 ===== - double vx = 0.0; - double vy = 0.0; - double omega = 0.0; +// // ===== 计算输出 ===== +// double vx = 0.0; +// double vy = 0.0; +// double omega = 0.0; - // 1) 前后控制：根据 tz 前进/后退 - if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) { - vx = tz * Constants.Limelight.AutoClimbKpTZ; +// // 1) 前后控制：根据 tz 前进/后退 +// if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) { +// vx = tz * Constants.Limelight.AutoClimbKpTZ; - // 最小输出，避免快到目标时不动 - if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) { - vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx); - } +// // 最小输出，避免快到目标时不动 +// if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) { +// vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx); +// } - // 最大输出限幅 - vx = clamp(vx, - -Constants.Limelight.AutoClimbMaxVX, - Constants.Limelight.AutoClimbMaxVX); - } +// // 最大输出限幅 +// vx = clamp(vx, +// -Constants.Limelight.AutoClimbMaxVX, +// Constants.Limelight.AutoClimbMaxVX); +// } - // 2) 旋转控制：tx 为主，ry 为辅 - // tx 负责“镜头中心对准 tag” - // ry 负责“机器人姿态微调” - // double omegaFromTx = 0.0; - // double omegaFromRy = 0.0; +// // 2) 旋转控制：tx 为主，ry 为辅 +// // tx 负责“镜头中心对准 tag” +// // ry 负责“机器人姿态微调” +// // double omegaFromTx = 0.0; +// // double omegaFromRy = 0.0; - // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) { - // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX; - // } +// // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) { +// // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX; +// // } - // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) { - // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; - // } +// // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) { +// // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; +// // } - // omega = omegaFromTx + omegaFromRy; +// // omega = omegaFromTx + omegaFromRy; - // // 大偏角时直接给较明显的转速，先把朝向拉回来 - // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) { - // omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega); - // } +// // // 大偏角时直接给较明显的转速，先把朝向拉回来 +// // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) { +// // omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega); +// // } - // // 最小输出，避免转不动 - // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) { - // omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega); - // } +// // // 最小输出，避免转不动 +// // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) { +// // omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega); +// // } - // // 最大输出限幅 - // omega = clamp(omega, - // -Constants.Limelight.AutoClimbMaxOmega, - // Constants.Limelight.AutoClimbMaxOmega); +// // // 最大输出限幅 +// // omega = clamp(omega, +// // -Constants.Limelight.AutoClimbMaxOmega, +// // Constants.Limelight.AutoClimbMaxOmega); - Rotation2d ClimbcurrentAngle = getState().Pose.getRotation(); - double climbCurrentRad = ClimbcurrentAngle.getRadians(); - double TargetAngleforClimbing = Math.toRadians(0); - double Climberror = TargetAngleforClimbing - climbCurrentRad; - if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) { - omega = 0.0; - } else { - omega = Climberror * Constants.AutoPositioning.TurningkP; - } +// Rotation2d ClimbcurrentAngle = getState().Pose.getRotation(); +// double climbCurrentRad = ClimbcurrentAngle.getRadians(); +// double TargetAngleforClimbing = Math.toRadians(0); +// double Climberror = TargetAngleforClimbing - climbCurrentRad; +// if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) { +// omega = 0.0; +// } else { +// omega = Climberror * Constants.AutoPositioning.TurningkP; +// } - // ===== 调试输出 ===== - driveNetworkTable.getEntry("ClimbTX").setDouble(tx); - driveNetworkTable.getEntry("ClimbTZ").setDouble(tz); - driveNetworkTable.getEntry("ClimbRY").setDouble(ry); - driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx); - driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega); - driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag); +// // ===== 调试输出 ===== +// driveNetworkTable.getEntry("ClimbTX").setDouble(tx); +// driveNetworkTable.getEntry("ClimbTZ").setDouble(tz); +// driveNetworkTable.getEntry("ClimbRY").setDouble(ry); +// driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx); +// driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega); +// driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag); - // ===== 下发到底盘 ===== - driveRobotRelative(new ChassisSpeeds(vx, vy, omega)); +// // ===== 下发到底盘 ===== +// driveRobotRelative(new ChassisSpeeds(vx, vy, omega)); - return "Driving to AprilTag"; -} +// return "Driving to AprilTag"; +// } - public boolean isAlignedToAprilTag(){ - return isAlignedToAprilTag; - } +// public boolean isAlignedToAprilTag(){ +// return isAlignedToAprilTag; +// } @Override public void periodic() { @@ -420,10 +431,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees()); driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name()); driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC); - positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag()); - positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX()); - positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY()); - positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ()); + //positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag()); + // positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX()); + // positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY()); + // positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ()); positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false); // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0 LimelightHelpers.SetRobotOrientation( diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java index 7a77f9b..91a556f 100644 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java @@ -83,26 +83,13 @@ public class IntakerSubsystem extends SubsystemBase{ stopIntakerotater(); } public void IntakerDownNonStop(){ - double currentPosition = getIntakerotaterPosition(); - if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) { - // move down (negative power) while above the lower limit - intakerotater.set(-0.05); - } else { - // at or below lower limit: stop - stopIntakerotater(); + intakerotater.set(-0.1); } - } public void IntakerUpNonStop(){ - double currentPosition = getIntakerotaterPosition(); - if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) { - // move up while below the upper limit - intakerotater.set(0.3); - } else { - // at or above upper limit: stop - stopIntakerotater(); - } + intakerotater.set(0.2); } + public void setIntakerRotater(double rotaterspeed){ intakerotater.set(rotaterspeed); } diff --git a/src/main/java/frc/robot/subsystems/LimelightSupplier.java b/src/main/java/frc/robot/subsystems/LimelightSupplier.java deleted file mode 100644 index 7836b78..0000000 --- a/src/main/java/frc/robot/subsystems/LimelightSupplier.java +++ /dev/null @@ -1,108 +0,0 @@ -package frc.robot.subsystems; - -import frc.robot.Constants; - -import java.lang.annotation.Target; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.LimelightHelpers; -import frc.robot.subsystems.LimelightHelpers.RawFiducial; - -public class LimelightSupplier extends SubsystemBase { - - public LimelightSupplier() { - setPipeline(8); - } - - public static double getTX() { - return LimelightHelpers.getTX(Constants.Limelight.LIMELIGHT_NAME_Intaker); - } - - public static double getTY() { - return LimelightHelpers.getTY(Constants.Limelight.LIMELIGHT_NAME_Intaker); - } - - public static double getTA() { - // Logic to get area from Limelight - return LimelightHelpers.getTA(Constants.Limelight.LIMELIGHT_NAME_Intaker); - } - - public static boolean isTargetVisible() { - if(!LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker)){ - return false; - } - // Logic to check if a target is visible - return LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker); - } - public static int getAprilTagID(){ - RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); - int id = -1; - for (RawFiducial fiducial : fiducials) { - id = fiducial.id; // 标签ID - } - return id; - } - public static void setPipeline(int pipeline) { - if(((int)LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)) != pipeline){ - LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker, pipeline); - } - } - - public static double[] getTargetPose() { - // Logic to get the target pose from Limelight - return LimelightHelpers.getBotPose_TargetSpace(Constants.Limelight.LIMELIGHT_NAME_Intaker); - } - - public static Pose2d getBotPose2d_wpiBlue(){ - return LimelightHelpers.getBotPose2d_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker); - } - public static double getTargetRotationY(){ - if(isTargetVisible()){ - return getTargetPose()[4]; - } - else{ - return 0.00; - } - } - public static double getTargetTZ(){ - if(isTargetVisible()){ - return getTargetPose()[2]; - } - else{ - return 0.00; - } - } - @Override - public void periodic() { - SmartDashboard.putNumber("LimelightPipeIndex", LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); - SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker)); - if (isTargetVisible()){ - SmartDashboard.putString("LimelightTargetVisible", "YES"); - SmartDashboard.putNumber("LimelightTA", getTA()); - SmartDashboard.putNumber("TargetRY", getTargetRotationY()); - SmartDashboard.putNumber("TargetTX", getTX()); - SmartDashboard.putNumber("TargetTZ", getTargetTZ()); - RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker); - int id = -1; - for (RawFiducial fiducial : fiducials) { - id = fiducial.id; // 标签ID - } - SmartDashboard.putNumber("LimelightTargetID", id); - - } - else { - SmartDashboard.putString("LimelightTargetVisible", "NO"); - SmartDashboard.putNumber("TargetRY", 0); - SmartDashboard.putNumber("TargetTX", 0); - SmartDashboard.putNumber("TargetTZ", 0); - SmartDashboard.putNumber("LimelightTargetID", -1); - } - - } -} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9a75144..6aa831b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance;