From d692bab745faa6608c79c26396249a6619e8e2b4 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 17 Feb 2025 18:33:49 -0500 Subject: [PATCH] limelight --- simgui-ds.json | 5 + .../autos/BlueBas3 sans composantes.auto | 126 ++++++++++++++++++ .../deploy/pathplanner/autos/New Auto.auto | 19 +++ src/main/deploy/pathplanner/autos/Sortir.auto | 6 + src/main/deploy/pathplanner/navgrid.json | 2 +- .../deploy/pathplanner/paths/New Path.path | 54 ++++++++ src/main/deploy/pathplanner/paths/Sortir.path | 22 +-- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 31 +++-- .../java/frc/robot/commands/AprilTag3.java | 33 +++-- .../java/frc/robot/commands/AprilTag3G.java | 35 +++-- src/main/java/frc/robot/commands/Forme3.java | 35 +++-- .../subsystems/CommandSwerveDrivetrain.java | 4 +- 13 files changed, 322 insertions(+), 52 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto b/src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto new file mode 100644 index 0000000..febe7c4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto @@ -0,0 +1,126 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasStart" + } + }, + { + "type": "named", + "data": { + "name": "AprilTag" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasChercher" + } + }, + { + "type": "named", + "data": { + "name": "AprilTag" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AprilTag" + } + }, + { + "type": "path", + "data": { + "pathName": "BlueBasPorter" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasChercher2" + } + }, + { + "type": "sequential", + "data": { + "commands": [] + } + }, + { + "type": "named", + "data": { + "name": "AprilTag" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasPorter2" + } + }, + { + "type": "named", + "data": { + "name": "AprilTag" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..268147b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Sortir.auto b/src/main/deploy/pathplanner/autos/Sortir.auto index 191a9fc..aaa5f9f 100644 --- a/src/main/deploy/pathplanner/autos/Sortir.auto +++ b/src/main/deploy/pathplanner/autos/Sortir.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 23e0db9..239a79b 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ 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..c864bb5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.8820696721311476, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.8706512438743716, + "y": 5.690872003047288 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8820696721311476, + "y": 3.1279999999999997 + }, + "prevControl": { + "x": 1.8863317961339294, + "y": 3.3779636659576444 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sortir.path b/src/main/deploy/pathplanner/paths/Sortir.path index 17ff327..adca024 100644 --- a/src/main/deploy/pathplanner/paths/Sortir.path +++ b/src/main/deploy/pathplanner/paths/Sortir.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.084733606557376, - "y": 6.488473360655737 + "x": 7.119564732833961, + "y": 6.478684258786044 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.849999999999999, - "y": 6.464497950819672 + "x": 5.838012295081967, + "y": 7.189754098360656 }, "prevControl": { - "x": 6.32950819672131, - "y": 6.464497950819672 + "x": 6.656071206876743, + "y": 7.180867730637985 }, "nextControl": null, "isLocked": false, @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 0.1, + "maxAcceleration": 0.1, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -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/settings.json b/src/main/deploy/pathplanner/settings.json index e97c2f6..1b76a41 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -14,7 +14,7 @@ "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, - "maxDriveSpeed": 5.45, + "maxDriveSpeed": 5.0, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b65ec6c..e4db502 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc.robot; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -15,12 +16,15 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -56,7 +60,12 @@ public class RobotContainer { Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); Limelight3 limelight3 = new Limelight3(); - public RobotContainer() { + Pose2d pose = new Pose2d(); + private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2 + public double getAngle() { + return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot + } + public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("New Auto"); SmartDashboard.putData("Auto Mode", autoChooser); configureBindings(); @@ -68,25 +77,25 @@ public class RobotContainer { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.15)) // Drive forward with negative Y (forward) - .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.15)) // Drive left with negative X (left) - .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left) + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left) ) ); // reset the field-centric heading on left bumper press manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain)); - manette1.rightTrigger().whileTrue(new Forme3(limelight3)); - manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g)); + manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); drivetrain.registerTelemetry(logger::telemeterize); } public Command getAutonomousCommand() { - return new ParallelCommandGroup( - autoChooser.getSelected(), - new RainBow(bougie)); - } + return new SequentialCommandGroup(autoChooser.getSelected(), + new RainBow(bougie)); + } + } diff --git a/src/main/java/frc/robot/commands/AprilTag3.java b/src/main/java/frc/robot/commands/AprilTag3.java index 20a9de9..d141a04 100644 --- a/src/main/java/frc/robot/commands/AprilTag3.java +++ b/src/main/java/frc/robot/commands/AprilTag3.java @@ -5,6 +5,8 @@ package frc.robot.commands; import static edu.wpi.first.units.Units.*; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -18,16 +20,19 @@ public class AprilTag3 extends Command { private Limelight3 limelight3; private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private DoubleSupplier x; + private DoubleSupplier y; /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors /** Creates a new AprilTag3G. */ - public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain) { + public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) { this.limelight3 = limelight3; this.drivetrain = drivetrain; + this.x = x; + this.y = y; addRequirements(limelight3,drivetrain); // Use addRequirements() here to declare subsystem dependencies. @@ -42,24 +47,30 @@ public class AprilTag3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double a = limelight3.getX(); if(limelight3.getV() == true){ - drivetrain.applyRequest(() -> - drive.withRotationalRate(limelight3.getX()/48)); - System.out.println(limelight3.getX()/48); + drivetrain.setControl(drive. + withRotationalRate(a/10). + withVelocityX(x.getAsDouble()). + withVelocityY(y.getAsDouble())); + System.out.println(a/10); } else{ - drivetrain.applyRequest(() -> - drive.withRotationalRate(0) - ); + drivetrain.setControl(drive. + withRotationalRate(0). + withVelocityX(0). + withVelocityY(0)); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drivetrain.applyRequest(() -> - drive.withRotationalRate(0)); + drivetrain.setControl(drive. + withRotationalRate(0) + .withVelocityX(0) + .withVelocityY(0)); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/AprilTag3G.java b/src/main/java/frc/robot/commands/AprilTag3G.java index 88245b8..a36937d 100644 --- a/src/main/java/frc/robot/commands/AprilTag3G.java +++ b/src/main/java/frc/robot/commands/AprilTag3G.java @@ -4,29 +4,37 @@ package frc.robot.commands; import static edu.wpi.first.units.Units.*; + +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.TunerConstants.TunerConstants; - +import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Limelight3G; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class AprilTag3G extends Command { private Limelight3G limelight3g; + private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private DoubleSupplier x; + private DoubleSupplier y; /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors /** Creates a new AprilTag3G. */ /** Creates a new AprilTag3G. */ - public AprilTag3G(Limelight3G limelight3g) { + public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) { this.limelight3g = limelight3g; - addRequirements(limelight3g); + this.drivetrain = drivetrain; + this.x = x; + this.y = y; + addRequirements(limelight3g,drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -37,18 +45,29 @@ public class AprilTag3G extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double a = limelight3g.getX(); if(limelight3g.getV() == true){ - drive.withRotationalRate(limelight3g.getX()/48); + drivetrain.setControl(drive. + withRotationalRate(-a/5). + withVelocityX(x.getAsDouble()). + withVelocityY(y.getAsDouble())); + System.out.println(a/5); } else{ - drive.withRotationalRate(0); + drivetrain.setControl(drive. + withRotationalRate(0). + withVelocityX(0). + withVelocityY(0)); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drive.withRotationalRate(0); + drivetrain.setControl(drive. + withRotationalRate(0). + withVelocityX(0). + withVelocityY(0)); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Forme3.java b/src/main/java/frc/robot/commands/Forme3.java index 53f1c0c..4b47baa 100644 --- a/src/main/java/frc/robot/commands/Forme3.java +++ b/src/main/java/frc/robot/commands/Forme3.java @@ -4,28 +4,36 @@ package frc.robot.commands; import static edu.wpi.first.units.Units.*; + +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.TunerConstants.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Limelight3; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class Forme3 extends Command { private Limelight3 limelight3; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private DoubleSupplier x; + private DoubleSupplier y; /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors /** Creates a new Forme3. */ - public Forme3(Limelight3 limelight3) { + public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) { this.limelight3 = limelight3; - - addRequirements(limelight3); + this.drivetrain = drivetrain; + this.x = x; + this.y = y; + addRequirements(limelight3,drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -38,18 +46,29 @@ public class Forme3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double a = limelight3.getX(); if(limelight3.getV() == true){ - drive.withRotationalRate(limelight3.getX()/48); + drivetrain.setControl(drive. + withRotationalRate(a/10). + withVelocityX(x.getAsDouble()). + withVelocityY(y.getAsDouble())); + System.out.println(a/10); } else{ - drive.withRotationalRate(0); + drivetrain.setControl(drive. + withRotationalRate(0). + withVelocityX(0). + withVelocityY(0)); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drive.withRotationalRate(0); + drivetrain.setControl(drive. + withRotationalRate(0). + withVelocityX(0). + withVelocityY(0)); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 023db69..7a46f6d 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -6,6 +6,7 @@ import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -53,6 +54,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( null, // Use default ramp rate (1 V/s) @@ -255,7 +257,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.dynamic(direction); } - + @Override public void periodic() { /*