diff --git a/.gitignore b/.gitignore index f809adc..2c7dcf3 100644 --- a/.gitignore +++ b/.gitignore @@ -53,3 +53,6 @@ Thumbs.db # VS Code Settings .vscode/ + +# sim files +.ctre_sim/ diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 130b3fc..8cc61a0 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "currentLanguage": "none", + "currentLanguage": "java", "enableCppIntellisense": false, - "projectYear": "none", + "projectYear": "2026", "teamNumber": 3926 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 919fc7a..8c1b3c9 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.1.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat deleted file mode 100644 index 2bbdfc9..0000000 Binary files a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat deleted file mode 100644 index 28822ad..0000000 Binary files a/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat deleted file mode 100644 index 28822ad..0000000 Binary files a/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat deleted file mode 100644 index 2bbdfc9..0000000 Binary files a/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat deleted file mode 100644 index 95ad7c5..0000000 Binary files a/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat and /dev/null differ diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index ac5f521..069d99d 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,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,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,true,true,true,true,true,true,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,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,true,true,true,true,true,true,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,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,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,false,false,false,false,false,false,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,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,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,true,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,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,true,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,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,true,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,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,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,true,true,true,true,true,true,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,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,true,true,true,true,true,true,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,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,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,true,true,true,true,true,true,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,false,false,false,false,false,false,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,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,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,true,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,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,true,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,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,true,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]} diff --git a/src/main/deploy/pathplanner/paths/GoToDepot.path b/src/main/deploy/pathplanner/paths/GoToDepot.path new file mode 100644 index 0000000..b091b45 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GoToDepot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.555, + "y": 6.34 + }, + "prevControl": null, + "nextControl": { + "x": 3.618047995229281, + "y": 5.549921996799937 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8138370036801503, + "y": 5.961968906269622 + }, + "prevControl": { + "x": 2.076736993577922, + "y": 6.092338697684089 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.0798355625638 + }, + "reversed": false, + "folder": "Offset Depot", + "idealStartingState": { + "velocity": 0, + "rotation": -0.19916319085310868 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootAfterDepot.path b/src/main/deploy/pathplanner/paths/ShootAfterDepot.path new file mode 100644 index 0000000..93fa06e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootAfterDepot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.814, + "y": 5.962 + }, + "prevControl": null, + "nextControl": { + "x": 2.26670189525463, + "y": 5.905031394675925 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7384416232638893, + "y": 4.878029730902778 + }, + "prevControl": { + "x": 2.4099296875000005, + "y": 5.91428247974537 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 57.826290910926296 + }, + "reversed": false, + "folder": "Offset Depot", + "idealStartingState": { + "velocity": 0, + "rotation": -0.19916319085310868 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..145ffd7 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,34 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "Offset Depot" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java new file mode 100644 index 0000000..f9b2712 --- /dev/null +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class CenterLemonAuto extends SequentialCommandGroup { + + public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addCommands( + new InstantCommand(()->drive.drive(0, 0.4,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/Auto/LeftLemonAuto.java b/src/main/java/frc/robot/Auto/LeftLemonAuto.java new file mode 100644 index 0000000..a23d4e4 --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeftLemonAuto.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class LeftLemonAuto extends SequentialCommandGroup { + + public LeftLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; + addCommands( + new InstantCommand(()->drive.drive(-0.5, 0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java new file mode 100644 index 0000000..d361b89 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class RightLemonAuto extends SequentialCommandGroup { + + public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; + addCommands( + new InstantCommand(()->drive.drive(0.5, 0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-30.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-30.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/Auto/ShootEightAuto.java b/src/main/java/frc/robot/Auto/ShootEightAuto.java new file mode 100644 index 0000000..3ec5bf5 --- /dev/null +++ b/src/main/java/frc/robot/Auto/ShootEightAuto.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class ShootEightAuto extends SequentialCommandGroup { + + public ShootEightAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; + addCommands( + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/Command/AltAutoAlign.java b/src/main/java/frc/robot/Command/AltAutoAlign.java new file mode 100644 index 0000000..34d1ede --- /dev/null +++ b/src/main/java/frc/robot/Command/AltAutoAlign.java @@ -0,0 +1,134 @@ +package frc.robot.Command; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +/* Drives the robot in an orbit around the hub while continuously facing the hub center */ + +public class AltAutoAlign extends Command { + + private SwerveSubsystem swerveSubsystem; + private ShooterSubsystem shooterSubsystem; + + private final PIDController headingController = new PIDController(kHeadingKp,0,0); + private final PIDController radiusController = new PIDController(kRadialKp, kRadialKi, kRadialKd); + + private static final double kDesiredOrbitRadiusMeters = 2; //placeholder + private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors + private static final double kRadialKp = 0.1; //P-gain for radial distance correction + private static final double kRadialKi = 0.0; + private static final double kRadialKd = 0.0; + private static final double kHeadingKp = 0.1; //P-gain for yaw control that faces the hub + + public AltAutoAlign(SwerveSubsystem swerveSubsystem, ShooterSubsystem shooterSubsystem){ + this.swerveSubsystem = swerveSubsystem; + this.shooterSubsystem = shooterSubsystem; + addRequirements(swerveSubsystem, shooterSubsystem); + headingController.enableContinuousInput(-Math.PI, Math.PI); + radiusController.setSetpoint(kDesiredOrbitRadiusMeters); + } + + @Override + public void initialize(){ + headingController.reset(); //Reset yaw PID state every time the command starts + radiusController.reset(); + } + + + @Override + public void execute(){ + Pose2d FieldPosition = swerveSubsystem.getPose(); //Get robot position on field + + Translation2d HubLocation = new Translation2d(4.61,4.03); //Hub location + HubLocation = FieldConstants.flipForAlliance(HubLocation); //Mirror the hub point when we are Red + + Translation2d robotToHub = HubLocation.minus(FieldPosition.getTranslation()); //Vector from robot to hub. + double radialDistance = robotToHub.getNorm(); + /*translation2d that points from the robot to the hub + * getNorm() returns the vector's magnitude (length) + * this line computes how far the robot currently is from the hub + */ + // Stop driving if odometry is incorrect + if (radialDistance < 0.05){ + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(), true); + return; + } + + Translation2d radialDirection = robotToHub.div(radialDistance); //Unit vector that always points toward the hub + //Radial vector rotated 90 degrees counterclockwise + + double radialPidOutput = radiusController.calculate(radialDistance); + + double radialSpeed = MathUtil.clamp( + -radialPidOutput, + -kMaxRadialSpeedMetersPerSecond, + kMaxRadialSpeedMetersPerSecond + ); + + Translation2d fieldRelativeVelocity = radialDirection.times(radialSpeed); + + double speedMagnitude = fieldRelativeVelocity.getNorm(); // Total requested speed + if(speedMagnitude > SwerveConstants.maxSpeed){ + fieldRelativeVelocity = + fieldRelativeVelocity.times(SwerveConstants.maxSpeed / speedMagnitude); + // respect drivetrain max velocity + } + + double desiredHeadingRadians = radialDirection.getAngle().getRadians() + Math.PI / 2.0; + + //Face straight at the hub while moving + double headingFeedforward = 0.0; + if (radialDistance > 1e-3){ + headingFeedforward = (radialDirection.getY()*fieldRelativeVelocity.getX() + - radialDirection.getX() * fieldRelativeVelocity.getY()) / radialDistance; + } + + double headingRate = MathUtil.clamp( + headingFeedforward + + headingController.calculate((FieldPosition.getRotation().getRadians()), desiredHeadingRadians), + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + // Yaw PID output limited to drivetrain capabilities + + ChassisSpeeds requestedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeVelocity.getX(), + fieldRelativeVelocity.getY(), + headingRate, + FieldPosition.getRotation()); + // Convert into chassis-relative speeds + + swerveSubsystem.driveFromChassisSpeeds(requestedSpeeds, false); + // Command the swerve in closed loop + + shooterSubsystem.setHoodAngle(ShooterSubsystem.HoodAngle.MED); + //put up hood angle + + shooterSubsystem.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + //Start shooter motor + + } + + + + @Override + public void end(boolean interrupted){ + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(), true); + // Stop the drivetrain + + } + + @Override + public boolean isFinished(){ + return false; + // Driver holds the trigger to stay in auto align + } +} diff --git a/src/main/java/frc/robot/Command/AutoAlign.java b/src/main/java/frc/robot/Command/AutoAlign.java index 664eb12..ee1bf3a 100644 --- a/src/main/java/frc/robot/Command/AutoAlign.java +++ b/src/main/java/frc/robot/Command/AutoAlign.java @@ -20,7 +20,7 @@ public class AutoAlign extends Command { //Orbit tuning constants (NEED CHANGE - kDesiredOrbitRadiusMeters, kTangentialSpeedMetersPerSecond) - private static final double kDesiredOrbitRadiusMeters = 2.22; //How far from the hub we want the robot to be + private static final double kDesiredOrbitRadiusMeters = 2.4384; //How far from the hub we want the robot to be private static final double kTangentialSpeedMetersPerSecond = 1.25; // Constant speed for sliding around the hub private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors private static final double kRadialKp = 1.6; //P-gain for radial distance correction diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99f33f6..416a1ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,116 +29,118 @@ public final class Constants { // Swerve Constants - public static final class SwerveConstants{ - public static final double inputDeadband = .1; // Deadzone for joystick inputs to prevent drift - public static final int PIGEON_ID = 17; //CAN ID for Pigeon gyro sensor - public static final boolean invertPigeon = false; // Whether to invert gyro readings - - /* Drivetrain Constants */ - public static final double halfTrackWidth = Units.inchesToMeters(27/2.0);//to find - public static final double halfWheelBase = Units.inchesToMeters(27/2.0);//to find - public static final double wheelDiameter = Units.inchesToMeters(4.0); - public static final double wheelCircumference = wheelDiameter * Math.PI; - //halfTrackWidth/halfwheelBase are already "half" distances, so don't divide again. - //public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); - public static final double driveBaseRadius = Math.hypot(halfWheelBase, halfTrackWidth); - - - public static final double openLoopRamp = 0.25; - public static final double closedLoopRamp = 0.0; - - public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 L2 Mk4 Modules - //L1 is 8.14:1, L2 is 6.75:1, L3 is 6.12:1, L4 is 5.14:1 - public static final double angleGearRatio = (21.4 / 1.0); // 21.4:1 MK4i Modules - //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 - - public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics( - //WPILib coordinate system: +X = forward, +Y = left - new Translation2d(halfTrackWidth, halfWheelBase), //Front left - new Translation2d(halfTrackWidth, -halfWheelBase), //Front right - new Translation2d(-halfTrackWidth, -halfWheelBase), //Back right - new Translation2d(-halfTrackWidth, halfWheelBase)); //Back Left - //translation 2d locates the swerve module in cords - //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, - //which contains velocities and angles for each swerve module of a swerve drive robot. - - /* Swerve Voltage Compensation */ - public static final double voltageComp = 12.0; +public static final class SwerveConstants{ + public static final double inputDeadband = .1; // Deadzone for joystick inputs to prevent drift + public static final int PIGEON_ID = 17; //CAN ID for Pigeon gyro sensor + public static final boolean invertPigeon = false; // Whether to invert gyro readings + + /* Drivetrain Constants */ + public static final double halfTrackWidth = Units.inchesToMeters(27/2.0);//to find + public static final double halfWheelBase = Units.inchesToMeters(27/2.0);//to find + public static final double wheelDiameter = Units.inchesToMeters(4.0); + public static final double wheelCircumference = wheelDiameter * Math.PI; + //halfTrackWidth/halfwheelBase are already "half" distances, so don't divide again. + //public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); + public static final double driveBaseRadius = Math.hypot(halfWheelBase, halfTrackWidth); + + + public static final double openLoopRamp = 0.25; + public static final double closedLoopRamp = 0.0; + + public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 L2 Mk4 Modules + //L1 is 8.14:1, L2 is 6.75:1, L3 is 6.12:1, L4 is 5.14:1 + public static final double angleGearRatio = (21.4 / 1.0); // 21.4:1 MK4i Modules + //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 + + public static final SwerveDriveKinematics swerveKinematics = + new SwerveDriveKinematics( + //WPILib coordinate system: +X = forward, +Y = left + new Translation2d(halfTrackWidth, halfWheelBase), //Front left + new Translation2d(halfTrackWidth, -halfWheelBase), //Front right + new Translation2d(-halfTrackWidth, -halfWheelBase), //Back right + new Translation2d(-halfTrackWidth, halfWheelBase)); //Back Left + //translation 2d locates the swerve module in cords + //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html + //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, + //which contains velocities and angles for each swerve module of a swerve drive robot. + + /* Swerve Voltage Compensation */ + public static final double voltageComp = 12.0; - //Swerve Current Limiting for neos - public static final int angleContinuousCurrentLimit = 20; //limits current draw of turning motor - public static final int driveContinuousCurrentLimit = 40; //limits current draw of drive motor - - - - /* Drive Motor PID Values */ - public static final double driveKP = 0.1; //to tune - public static final double driveKI = 0.0; //to tune - public static final double driveKD = 0.0; //to tune + //Swerve Current Limiting for neos + public static final int angleContinuousCurrentLimit = 20; //limits current draw of turning motor + public static final int driveContinuousCurrentLimit = 40; //limits current draw of drive motor + + + + /* Drive Motor PID Values */ + public static final double driveKP = 0.1; //to tune + public static final double driveKI = 0.0; //to tune + public static final double driveKD = 0.0; //to tune + + /* Drive Motor Characterization Values */ + //values to calculate the drive feedforward (KFF) + public static final double driveKS = 0.667; //to calculate + public static final double driveKV = 2.4; //to calculate + public static final double driveKA = 0.5; //to calculate + + /* Angle Motor PID Values */ + public static final double angleKP = 0.01; //to tune + public static final double angleKI = 0.0; //to tune, keep it at zero unless you see a persistent offset + public static final double angleKD = 0.0; //to tune + + /* Drive Motor Conversion Factors */ + public static final double driveConversionPositionFactor = + (wheelDiameter * Math.PI) / driveGearRatio; + public static final double driveConversionVelocityFactor = driveConversionPositionFactor / 60.0; + public static final double angleConversionFactor = 360.0 / angleGearRatio; + + /* Swerve Profiling Values */ + public static final double maxSpeed = 5; // meters per second + public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin + + /* Neutral Modes */ + public static final IdleMode angleNeutralMode = IdleMode.kBrake; + public static final IdleMode driveNeutralMode = IdleMode.kBrake; + + /* Motor Inverts */ + public static final boolean canCoderInvert = false; + public static final boolean driveInvert = false; + public static final boolean angleInvert = true; + + //Location of modules + public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); + public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); + + /* Module Specific Constants */ + public record ModuleData( + int driveMotorID, + int angleMotorID, + int encoderID, + double angleOffset, + Translation2d location, + boolean driveInvert, + boolean angleInvert + ){} + + public static ModuleData[] moduleData = { + new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + // Module 1 is currently the only module oscillating; flip its angle motor invert so its + // steering closed-loop sign matches the encoder direction. + // Module 1: also invert drive so +X command drives forward like the others. + new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left + }; - /* Drive Motor Characterization Values */ - //values to calculate the drive feedforward (KFF) - public static final double driveKS = 0.667; //to calculate - public static final double driveKV = 2.4; //to calculate - public static final double driveKA = 0.5; //to calculate - - /* Angle Motor PID Values */ - public static final double angleKP = 0.01; //to tune - public static final double angleKI = 0.0; //to tune, keep it at zero unless you see a persistent offset - public static final double angleKD = 0.0; //to tune - - /* Drive Motor Conversion Factors */ - public static final double driveConversionPositionFactor = - (wheelDiameter * Math.PI) / driveGearRatio; - public static final double driveConversionVelocityFactor = driveConversionPositionFactor / 60.0; - public static final double angleConversionFactor = 360.0 / angleGearRatio; - - /* Swerve Profiling Values */ - public static final double maxSpeed = 3; // meters per second - public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin - - /* Neutral Modes */ - public static final IdleMode angleNeutralMode = IdleMode.kBrake; - public static final IdleMode driveNeutralMode = IdleMode.kBrake; - - /* Motor Inverts */ - public static final boolean canCoderInvert = false; - public static final boolean driveInvert = false; - public static final boolean angleInvert = true; - - //Location of modules - public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); - public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); - - /* Module Specific Constants */ - public record ModuleData( - int driveMotorID, - int angleMotorID, - int encoderID, - double angleOffset, - Translation2d location, - boolean driveInvert, - boolean angleInvert - ){} - - public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - // Module 1 is currently the only module oscillating; flip its angle motor invert so its - // steering closed-loop sign matches the encoder direction. - // Module 1: also invert drive so +X command drives forward like the others. - new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right - new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left - }; - - } +} public static final class AutoConstants { + private static boolean dashboardInitialized = false; + public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(SwerveConstants.wheelDiameter/2, SwerveConstants.maxSpeed, 1.2, @@ -153,98 +155,184 @@ public static final class AutoConstants { new PIDConstants(5.0, 0.005, 0.001) ); public enum AutoMode{ - DriveTestAuto, - EightLemonAuto + None, + LeftLemonAuto, + RightLemonAuto, + ShootEightAuto, + CenterLemonAuto } private static SendableChooser sideChooser = new SendableChooser(); private static SendableChooser autoModeChooser = new SendableChooser(); - static{ + public static void initDashboard() { + if (dashboardInitialized) { + return; + } + dashboardInitialized = true; + sideChooser.addOption("RIGHT", true); sideChooser.setDefaultOption("LEFT", false); - for(AutoMode mode : AutoMode.values()){ - autoModeChooser.addOption(mode.toString(), mode); - } + autoModeChooser.setDefaultOption("LeftLemonAuto", AutoMode.LeftLemonAuto); + autoModeChooser.addOption("None", AutoMode.None); + autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); + autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); + autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); + autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); - autoModeChooser.setDefaultOption(AutoMode.DriveTestAuto.toString(), AutoMode.DriveTestAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); } public static AutoMode getSelectedAutoMode(){ + initDashboard(); AutoMode selection = autoModeChooser.getSelected(); - return selection != null ? selection : AutoMode.DriveTestAuto; + return selection != null ? selection : AutoMode.LeftLemonAuto; } public static boolean isRightSideAuto(){ + initDashboard(); return Boolean.TRUE.equals(sideChooser.getSelected()); } } -public class FieldConstants { - public static final double FIELD_LENGTH = 17.54824934; - public static final double FIELD_WIDTH = 8.052; +public static final class FieldConstants { + public static final double FIELD_LENGTH = 16.54; + public static final double FIELD_WIDTH = 8.07; - public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); + public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); - public static boolean isRedAlliance(){ - return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - } - - public static Rotation2d flipForAlliance(Rotation2d rotation){ - if(isRedAlliance()){ - return Rotation2d.fromDegrees(rotation.getDegrees() + 180); - }else{ - return rotation; - } - } - public static Translation2d flipForAlliance(Translation2d pos){ - if(isRedAlliance()){ - return new Translation2d(FIELD_LENGTH - pos.getX(), FIELD_WIDTH - pos.getY()); - }else{ - return pos; - } - } - public static Pose2d flipForAlliance(Pose2d pose){ - return new Pose2d(flipForAlliance(pose.getTranslation()), flipForAlliance(pose.getRotation())); - } - + /** + * If true, the robot will behave as if it is always on the Blue alliance (no field mirroring), + * even when connected to FMS / Driver Station reports Red.1 + */ + public static final boolean FORCE_BLUE_ALLIANCE = true; + + public static boolean isRedAlliance(){ + if (FORCE_BLUE_ALLIANCE) { + return false; + } + // Default to Blue when alliance is unknown (common in sim/practice). + return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; } - /* Shooter Constants */ - public class ShooterConstants { - public static final int SHOOTER_ID = 70; //Placeholder ID - public static final int FEEDER_ID = 61; //Feeder ID - public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) - - public static final double SHOOTER_SPEED = 0.5; //Placeholder speed - public static final double FEEDER_SPEED = 0.5; - - public static final double HOOD_ANGLE_LOW = 0.0; - public static final double HOOD_ANGLE_HIGH = 0.5; - public static final double HOOD_KP = 1.2; - public static final double HOOD_MAX_OUTPUT = 0.4; - public static final double HOOD_TOLERANCE = 0.02; + + public static Rotation2d flipForAlliance(Rotation2d rotation){ + if(isRedAlliance()){ + return Rotation2d.fromDegrees(rotation.getDegrees() + 180); + }else{ + return rotation; + } + } + public static Translation2d flipForAlliance(Translation2d pos){ + if(isRedAlliance()){ + return new Translation2d(FIELD_LENGTH - pos.getX(), FIELD_WIDTH - pos.getY()); + }else{ + return pos; + } } - public class IntakeConstants { - // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). - // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. - public static int INTAKE_ID = 63; // TODO: set to your intake motor CAN ID - public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - - public static int INTAKE_ARM_ID = 64; // TODO: set to your intake arm motor CAN ID - public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later - public static double INTAKE_ARM_LOWERED_POSITION = 0; - public static double INTAKE_ARM_MINIMUM = 0; // placeholders - public static double INTAKE_ARM_MAXIMUM = 90; - public static int GEAR_RATIO = 3; - - public static double INTAKE_ARM_kP = 0.01; - public static double INTAKE_ARM_kI = 0; - public static double INTAKE_ARM_kD = 0; + public static Pose2d flipForAlliance(Pose2d pose){ + return new Pose2d(flipForAlliance(pose.getTranslation()), flipForAlliance(pose.getRotation())); } + +} + +/** Vision constants (Limelight, etc). */ +public static final class VisionConstants { + public static final String[] LIMELIGHT_NAMES = {"limelight-a", "limelight-b"}; + + // Limelight MJPEG stream endpoints. + // Using fixed IPs avoids mDNS/DNS resolution issues on the roboRIO. + public static final String LIMELIGHT_A_STREAM_URL = "http://10.39.26.4:5801/stream.mjpg"; + public static final String LIMELIGHT_B_STREAM_URL = "http://10.39.26.5:5801/stream.mjpg"; + public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; + + public static final boolean VISION_ENABLED_DEFAULT = true; + public static final double MAX_VISION_ANGULAR_RATE_DEG_PER_SEC = 720.0; + + /** Standard deviations for vision measurements: (x meters, y meters, theta radians). */ + public static final double VISION_STD_DEV_X_METERS = 0.7; + public static final double VISION_STD_DEV_Y_METERS = 0.7; + public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; + + public static String getLimelightStreamUrl(String limelightName) { + switch (limelightName) { + case "limelight-a": + return LIMELIGHT_A_STREAM_URL; + case "limelight-b": + return LIMELIGHT_B_STREAM_URL; + default: + // Fallback for any future Limelight names. + return "http://" + limelightName + ".local:5801/stream.mjpg"; + } + } +} + +/* Shooter Constants */ +public static final class ShooterConstants { + public static final int SHOOTER_ID = 22; + public static final int KICKER_ID = 21; + public static final int HOOD_ID = 20; + public static final int INDEXER_ID = 23; + + // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. + public static final double SHOOTER_SPEED = 0.6; + public static final double KICKER_SPEED = 0.6; + public static final double INDEXER_SPEED = 0.4; //placeholder + + // Shooter readiness (SparkMax encoder velocity is RPM). Tune on the real robot. + public static final double SHOOTER_READY_RPM = 3000.0; + + // Electrical limits/compensation. + public static final double SHOOTER_VOLTAGE_COMP = 12.0; + public static final int SHOOTER_CURRENT_LIMIT_AMPS = 60; + public static final int KICKER_CURRENT_LIMIT_AMPS = 60; + + // Hood position units are motor rotations (NEO internal encoder). + // Max travel is 3 rotations = 1080 degrees. + public static final double HOOD_MIN_ROTATIONS = 0.0; + public static final double HOOD_MED_ROTATIONS = 20.0; + public static final double HOOD_MAX_ROTATIONS = 36.0; + + // Preset positions. + public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; + public static final double HOOD_ANGLE_MED = HOOD_MED_ROTATIONS; + public static final double HOOD_ANGLE_HIGH = HOOD_MAX_ROTATIONS; // "up" (about 2 inches) + public static final double HOOD_KP = 0.1; + public static final double HOOD_MAX_OUTPUT = 0.4; + public static final double HOOD_TOLERANCE = 0.02; +} + +public static final class IntakeConstants { + // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). + // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. + public static int INTAKE_ID = 19; + public static double INTAKE_SPEED = 90; //percent output scaling for intake motor + + public static int INTAKE_ARM_ID = 18; + public static int GEAR_RATIO = 25; + + //Intake arm position units are degrees + public static final double INTAKE_ARM_MIN_DEG = 20.0; + public static final double INTAKE_ARM_MAX_DEG = 90.0; + + //Preset positions + public static final double INTAKE_ARM_LOWERED_POSITION = INTAKE_ARM_MIN_DEG; + public static final double INTAKE_ARM_RAISED_POSITION = INTAKE_ARM_MAX_DEG; + + //PID constants for intake arm (degrees). + public static final double INTAKE_ARM_kP = 6.0; + public static final double INTAKE_ARM_kI = 1.5; + public static final double INTAKE_ARM_kD = 0.15; + public static final double INTAKE_ARM_TOLERANCE_DEG = 2.0; + + //Percent output cap (0..1) for gentler motion + //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] + public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; + public static final double INTAKE_ARM_MIN_OUTPUT = -0.10; +} + +public static final class CANdleConstants { + public static final int CANDLE_ID = 18; //Placeholder ID - public class CANdleConstants { - public static final int CANDLE_ID = 18; //Placeholder ID } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e15cf3c..b4ec70d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -13,11 +14,17 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; + private final RobotSimulation m_robotSimulation; public Robot() { m_robotContainer = new RobotContainer(); + m_robotSimulation = new RobotSimulation(m_robotContainer); } + @Override + public void robotInit() { + CameraServer.startAutomaticCapture(); + } @Override public void robotPeriodic() { @@ -55,6 +62,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + } @Override @@ -74,4 +82,14 @@ public void testPeriodic() {} @Override public void testExit() {} + + @Override + public void simulationInit() { + m_robotSimulation.simulationInit(); + } + + @Override + public void simulationPeriodic() { + m_robotSimulation.simulationPeriodic(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae8c531..7c0b076 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,30 +4,36 @@ package frc.robot; - +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.cscore.HttpCamera; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; 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.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Auto.DriveTestAuto; -import frc.robot.Auto.EightLemonAuto; +import frc.robot.Auto.LeftLemonAuto; +import frc.robot.Auto.RightLemonAuto; +import frc.robot.Auto.ShootEightAuto; +import frc.robot.Auto.CenterLemonAuto; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Command.AltAutoAlign; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class RobotContainer { @@ -55,11 +61,63 @@ public class RobotContainer { //ShooterSubsystem for shooter private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + + private boolean lastHelmsRightBumperPressed = false; + private double helmsRightBumperPressTimestampSec = 0.0; + + private final java.util.Map limelightCameras = new java.util.HashMap<>(); + private UsbCamera driverCamera; public RobotContainer() { + AutoConstants.initDashboard(); + startLimelightStreams(); + startDriverCameraStream(); configureBindings(); } + private void startLimelightStreams() { + SmartDashboard.putBoolean("Limelight Stream Enabled", VisionConstants.LIMELIGHT_STREAM_ENABLED_DEFAULT); + + if (RobotBase.isSimulation()) { + return; + } + + if (!SmartDashboard.getBoolean("Limelight Stream Enabled", VisionConstants.LIMELIGHT_STREAM_ENABLED_DEFAULT)) { + return; + } + + for (String limelightName : VisionConstants.LIMELIGHT_NAMES) { + String url = VisionConstants.getLimelightStreamUrl(limelightName); + SmartDashboard.putString("Vision/" + limelightName + "/StreamURL", url); + + HttpCamera camera = limelightCameras.computeIfAbsent(limelightName, (name) -> new HttpCamera(name, url)); + camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); + CameraServer.startAutomaticCapture(camera); + } + } + + private void startDriverCameraStream() { + SmartDashboard.putBoolean("Driver Camera Enabled", true); + + if (RobotBase.isSimulation()) { + return; + } + + if (!SmartDashboard.getBoolean("Driver Camera Enabled", true)) { + return; + } + + if (driverCamera != null) { + return; + } + + // Microsoft LifeCam HD-3000 (or any USB UVC camera) connected to the roboRIO. + driverCamera = CameraServer.startAutomaticCapture("DriverCam", 0); + driverCamera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); + driverCamera.setResolution(640, 480); + driverCamera.setFPS(30); + } + private void configureBindings() { // Y Button = Zero gyro (reset heading to 0° or 180° based on alliance) @@ -73,32 +131,61 @@ private void configureBindings() { // SHOOTER CONTROLLER - helmsController.axisGreaterThan(Axis.kRightTrigger.value, 0.1) - .whileTrue(Commands.startEnd( - () -> m_shooter.runShooter(true), - () -> m_shooter.runShooter(false), - m_shooter)); - m_shooter.setDefaultCommand( - Commands.run( - () -> { - double feederAxis = helmsController.getRawAxis(Axis.kRightY.value); - double feederSpeed = 0.0; - if (Math.abs(feederAxis) > 0.1) { - feederSpeed = -Math.signum(feederAxis) * ShooterConstants.FEEDER_SPEED; - } - m_shooter.runFeederSpeed(feederSpeed); - }, - m_shooter)); - - helmsController.button(Button.kB.value).onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); - helmsController.button(Button.kY.value).onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); + Commands.run( + () -> { + // Right stick Y controls shooter. + // Invert so stick-up (negative on Xbox) produces positive motor output. + double shooterAxis = -MathUtil.applyDeadband( + helmsController.getRawAxis(Axis.kRightY.value), + 0.1); + + m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); + + // Right bumper runs the indexer and kicker forward while held. + // Left bumper runs the indexer and kicker in reverse while held. + boolean leftBumperPressed = helmsController.getHID().getLeftBumper(); + boolean rightBumperPressed = helmsController.getHID().getRightBumper(); + if (rightBumperPressed && !lastHelmsRightBumperPressed) { + helmsRightBumperPressTimestampSec = Timer.getFPGATimestamp(); + } + + boolean indexerEnabled = + rightBumperPressed + && (Timer.getFPGATimestamp() - helmsRightBumperPressTimestampSec) >= 1.0; + + double kickerSpeed = 0.0; + double indexerSpeed = 0.0; + + if (leftBumperPressed) { + kickerSpeed = -ShooterConstants.KICKER_SPEED; + indexerSpeed = -ShooterConstants.INDEXER_SPEED; + } else if (rightBumperPressed) { + kickerSpeed = ShooterConstants.KICKER_SPEED; + indexerSpeed = indexerEnabled ? ShooterConstants.INDEXER_SPEED : 0.0; + } + + m_shooter.setKickerSpeed(kickerSpeed); + m_shooter.setIndexerSpeed(indexerSpeed); + + lastHelmsRightBumperPressed = rightBumperPressed; + }, + m_shooter)); + + + // Hood controls (helms controller). + // Y = hood up (2 inches / max travel), B = hood down. + helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); + helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), m_shooter)); + helmsController.a().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); // Left Trigger = Auto-align to left scoring position driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); // Right Trigger = Auto-align to right scoring position driveController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, false)); + // Right Bumper = Alt-Auto-Align + driveController.button(Button.kRightBumper.value).whileTrue(new AltAutoAlign(m_drive, m_shooter)); // Default command runs continuously when no other command requires the subsystem. // It automatically pauses when commands like AutoAlign take control, then resumes @@ -108,9 +195,9 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.7, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.7, // rotationSupplier - Rotation speed () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) @@ -120,40 +207,43 @@ private void configureBindings() { )); //INTAKE - // raises the intake using the A button on the helms controller m_intake.setDefaultCommand( - new RunCommand( - () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), - m_intake)); + new RunCommand( + () -> m_intake.setIntakePower(MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), + m_intake)); + - //lowers the intake using the A button on the helms controller - helmsController.button(Button.kA.value).onTrue( - new InstantCommand(() -> m_intake.raiseIntake(), m_intake) - ); - - // lowers the intake using the X button on the helms controller - helmsController.button(Button.kX.value).onTrue( - new InstantCommand(() -> m_intake.lowerIntake(), m_intake) - ); + // Intake arm buttons. + // Left Trigger = lower arm, Right Trigger = Raise arm. + // Bound on both controllers so it works regardless of which one you're pressing. + helmsController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); + helmsController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } private double getSpeedMultiplier(){ // getHID() accesses the underlying XboxController to read button states directly. // CommandXboxController doesn't provide a method for stick button presses, so we use // the HID (Human Interface Device) object's getRawButton() method instead. - return driveController.getHID().getRawButton(Button.kLeftStick.value)? 0.7: 1; + return driveController.getHID().getRawButton(Button.kLeftStick.value)? 1: 1; } public Command getAutonomousCommand() { AutoConstants.AutoMode selected = AutoConstants.getSelectedAutoMode(); return switch (selected) { - case DriveTestAuto -> new DriveTestAuto(m_drive); - case EightLemonAuto -> new EightLemonAuto(m_drive, m_shooter, m_intake); + case None -> Commands.none(); + case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); + case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); + case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); + case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); + + default -> Commands.none(); }; } - + public SwerveSubsystem getDriveSubsystem() { + return m_drive; + } } diff --git a/src/main/java/frc/robot/RobotSimulation.java b/src/main/java/frc/robot/RobotSimulation.java new file mode 100644 index 0000000..08b4007 --- /dev/null +++ b/src/main/java/frc/robot/RobotSimulation.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + + +public class RobotSimulation { + private final SwerveSubsystem drive; + private double lastTimestampSeconds = Timer.getFPGATimestamp(); + + public RobotSimulation(RobotContainer robotContainer) { + this.drive = robotContainer.getDriveSubsystem(); + } + + public void simulationInit() { + if (!RobotBase.isSimulation()) { + return; + } + + // Leave the robot disabled by default so the Sim GUI Driver Station can control mode + // (Disabled / Auto / Teleop). + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(false); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.notifyNewData(); + drive.simulationReset(); + lastTimestampSeconds = Timer.getFPGATimestamp(); + } + + public void simulationPeriodic() { + if (!RobotBase.isSimulation()) { + return; + } + + final double now = Timer.getFPGATimestamp(); + final double dtSeconds = MathUtil.clamp(now - lastTimestampSeconds, 0.0, 0.05); + lastTimestampSeconds = now; + + drive.simulationUpdate(dtSeconds); + + var speeds = drive.getLastCommandedSpeeds(); + double driveFraction = + Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) / SwerveConstants.maxSpeed; + double rotateFraction = + Math.abs(speeds.omegaRadiansPerSecond) / SwerveConstants.maxAngularVelocity; + double estimatedCurrentAmps = 8.0 + 80.0 * MathUtil.clamp(driveFraction, 0.0, 1.0) + + 40.0 * MathUtil.clamp(rotateFraction, 0.0, 1.0); + + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(estimatedCurrentAmps)); + } +} diff --git a/src/main/java/frc/robot/Subsystems/CandleSubsystem.java b/src/main/java/frc/robot/Subsystems/CandleSubsystem.java index 9e9f1de..4424fdb 100644 --- a/src/main/java/frc/robot/Subsystems/CandleSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/CandleSubsystem.java @@ -11,7 +11,7 @@ import com.ctre.phoenix6.controls.EmptyAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; -import com.ctre.phoenix6.signals.RGBWColor; +import com.ctre.phoenix6.signals.RGBWColor; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.CANdleConstants; diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 18a7d89..412243c 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -4,9 +4,8 @@ package frc.robot.Subsystems; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; @@ -23,17 +22,24 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkMax intakeMotor = new SparkMax(IntakeConstants.INTAKE_ID, MotorType.kBrushless); private final SparkMax intakeArmMotor = new SparkMax(IntakeConstants.INTAKE_ARM_ID, MotorType.kBrushless); - private RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); + private final RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); - private PIDController intakeArmPID = new PIDController(IntakeConstants.INTAKE_ARM_kP, IntakeConstants.INTAKE_ARM_kI, IntakeConstants.INTAKE_ARM_kD); - - private ArmFeedforward intakeArmFeedForward = new ArmFeedforward(0,0,0); - - public double targetPosition; + private final PIDController intakeArmController = new PIDController( + IntakeConstants.INTAKE_ARM_kP, + IntakeConstants.INTAKE_ARM_kI, + IntakeConstants.INTAKE_ARM_kD); + + private double intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + private boolean intakeArmActive = false; private boolean intakeOn = false; private boolean intakeUp = true; + public enum IntakeArmAngle { + DOWN, + UP + } + /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { SparkMaxConfig intakeConfig = new SparkMaxConfig(); @@ -43,22 +49,26 @@ public IntakeSubsystem() { intakeMotor.configure(intakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); SparkMaxConfig intakeArmConfig = new SparkMaxConfig(); - intakeArmConfig.inverted(false); + intakeArmConfig.inverted(true); intakeArmConfig.idleMode(IdleMode.kBrake); - intakeArmConfig.encoder.positionConversionFactor(360/IntakeConstants.GEAR_RATIO); + + intakeArmConfig.encoder.positionConversionFactor(360.0 / IntakeConstants.GEAR_RATIO); intakeArmMotor.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + //On enable, assume the arm starts raised at 90 degrees intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); - targetPosition = IntakeConstants.INTAKE_ARM_RAISED_POSITION; // start with arm raised + intakeArmController.setTolerance(IntakeConstants.INTAKE_ARM_TOLERANCE_DEG); + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + intakeArmActive = false; } public void toggleIntake() { if (!intakeOn) { - intakeOn = true; + intakeOn = false; intakeMotor.set(IntakeConstants.INTAKE_SPEED); } else { - intakeOn = false; + intakeOn = true; intakeMotor.set(0); } } @@ -70,18 +80,32 @@ public void setIntakePower(double power) { } - public void setTargetPosition(double position) { - targetPosition = Math.max(IntakeConstants.INTAKE_ARM_MINIMUM, Math.min(IntakeConstants.INTAKE_ARM_MAXIMUM, position)); + public void setIntakeArmAngle(IntakeArmAngle angle){ + switch (angle){ + case DOWN: + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_LOWERED_POSITION; + intakeUp = false; + break; + case UP: + default: + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + intakeUp = true; + break; + } + + intakeArmTargetDeg = Math.max( + IntakeConstants.INTAKE_ARM_MIN_DEG, + Math.min(IntakeConstants.INTAKE_ARM_MAX_DEG, intakeArmTargetDeg)); + intakeArmController.reset(); + intakeArmActive = true; } public void raiseIntake() { - setTargetPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); - intakeUp = true; + setIntakeArmAngle (IntakeArmAngle.UP); } public void lowerIntake() { - setTargetPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); - intakeUp = false; + setIntakeArmAngle(IntakeArmAngle.DOWN); } public void moveIntake() { @@ -93,16 +117,30 @@ public void moveIntake() { } } - public double getArmPosition() { - return intakeArmEncoder.getPosition() * 360; + public double getArmPositionDeg() { + return intakeArmEncoder.getPosition(); } @Override public void periodic() { - // This method will be called once per scheduler run - double PIDOutput = intakeArmFeedForward.calculate( - Units.degreesToRadians(intakeArmEncoder.getPosition()),0) - + intakeArmPID.calculate(getArmPosition(), targetPosition); - intakeArmMotor.set(PIDOutput); + double currentDeg = getArmPositionDeg(); + + SmartDashboard.putNumber("IntakeArm/TargetDeg", intakeArmTargetDeg); + SmartDashboard.putNumber("IntakeArm/PostionDeg", currentDeg); + SmartDashboard.putBoolean("IntakeArm/Active", intakeArmActive); + + if (intakeArmActive){ + double output = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); + output = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); + + if (intakeArmController.atSetpoint()){ + intakeArmMotor.set(0.0); + intakeArmActive = false; + } else { + intakeArmMotor.set(output); + } + } else{ + intakeArmMotor.set(0.0); + } } } diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index caca87c..4f78c76 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -19,10 +19,15 @@ public class ShooterSubsystem extends SubsystemBase { public boolean isShooterActive = false; //Shooter True - SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); - SparkMax feederMotor = new SparkMax(ShooterConstants.FEEDER_ID, MotorType.kBrushless); - SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); - + public final SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); + private final SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); + private final SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); + private final SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); + + private double shooterCmd = 0.0; + private double kickerCmd = 0.0; + private double indexerCmd = 0.0; + private final PIDController hoodController = new PIDController( ShooterConstants.HOOD_KP, 0.0, @@ -33,6 +38,7 @@ public class ShooterSubsystem extends SubsystemBase { public enum HoodAngle { LOW, + MED, HIGH } @@ -40,20 +46,31 @@ public enum HoodAngle { public ShooterSubsystem() { SparkMaxConfig shootConfig = new SparkMaxConfig(); - shootConfig.inverted(false); + shootConfig.smartCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT_AMPS); + shootConfig.inverted(true); shootConfig.idleMode(IdleMode.kCoast); + shootConfig.voltageCompensation(ShooterConstants.SHOOTER_VOLTAGE_COMP); SparkMaxConfig feedConfig = new SparkMaxConfig(); + feedConfig.smartCurrentLimit(ShooterConstants.KICKER_CURRENT_LIMIT_AMPS); feedConfig.inverted(false); feedConfig.idleMode(IdleMode.kBrake); + feedConfig.voltageCompensation(ShooterConstants.SHOOTER_VOLTAGE_COMP); SparkMaxConfig hoodConfig = new SparkMaxConfig(); - hoodConfig.inverted(false); + hoodConfig.inverted(true); hoodConfig.idleMode(IdleMode.kBrake); + SparkMaxConfig indexConfig = new SparkMaxConfig(); + indexConfig.inverted(true); + indexConfig.idleMode(IdleMode.kBrake); + + + shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - feederMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + kickerMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + indexerMotor.configure(indexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodController.setTolerance(ShooterConstants.HOOD_TOLERANCE); } @@ -80,13 +97,24 @@ public void runShooter(boolean shooterOn) { } } + public void setShooterSpeed(double speed) { + isShooterActive = Math.abs(speed) > 0.0; + shooterCmd = speed; + shooterMotor.set(speed); + } + + public double getShooterVelocityRpm() { + return shooterMotor.getEncoder().getVelocity(); + } + - public void runFeeder(boolean feederOn){ - runFeederSpeed(feederOn ? ShooterConstants.FEEDER_SPEED : 0); + public void runKicker(boolean kickerOn){ + setKickerSpeed(kickerOn ? ShooterConstants.KICKER_SPEED : 0); } - public void runFeederSpeed(double speed) { - feederMotor.set(speed); + public void setKickerSpeed(double speed) { + kickerCmd = speed; + kickerMotor.set(speed); } public void setHoodAngle(HoodAngle angle) { @@ -94,12 +122,18 @@ public void setHoodAngle(HoodAngle angle) { case LOW: hoodTargetPosition = ShooterConstants.HOOD_ANGLE_LOW; break; + case MED: + hoodTargetPosition = ShooterConstants.HOOD_ANGLE_MED; + break; case HIGH: hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; break; default: hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; } + hoodTargetPosition = Math.max( + ShooterConstants.HOOD_MIN_ROTATIONS, + Math.min(ShooterConstants.HOOD_MAX_ROTATIONS, hoodTargetPosition)); hoodController.reset(); hoodActive = true; } @@ -108,12 +142,37 @@ public double getHoodPosition() { return hoodMotor.getEncoder().getPosition(); } + public void runIndexer(boolean indexerOn) { + setIndexerSpeed(indexerOn ? ShooterConstants.INDEXER_SPEED : 0); + } + + public void setIndexerSpeed(double speed) { + indexerCmd = speed; + indexerMotor.set(speed); + } + + public void AutoToggleShoot (boolean AutoShootOn) { + setKickerSpeed(AutoShootOn ? 0 : ShooterConstants.KICKER_SPEED); + setShooterSpeed(AutoShootOn ? 0 : ShooterConstants.SHOOTER_SPEED); + } + + public void AutoToggleKickIndex (boolean AutoIndexKickOn) { + setKickerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.KICKER_SPEED); + setIndexerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.SHOOTER_SPEED); + } + @Override public void periodic() { // This method will be called once per scheduler run SmartDashboard.putBoolean("Is Shooter Active", isShooterActive); SmartDashboard.putNumber("Hood Target Position", hoodTargetPosition); SmartDashboard.putNumber("Hood Position", getHoodPosition()); + SmartDashboard.putNumber("Shooter/Cmd", shooterCmd); + SmartDashboard.putNumber("Shooter/VelocityRPM", getShooterVelocityRpm()); + SmartDashboard.putNumber("Kicker/Cmd", kickerCmd); + SmartDashboard.putNumber("Kicker/VelocityRPM", kickerMotor.getEncoder().getVelocity()); + SmartDashboard.putNumber("Indexer/Cmd", indexerCmd); + SmartDashboard.putNumber("Indexer/VelocityRPM", indexerMotor.getEncoder().getVelocity()); if (hoodActive) { @@ -130,4 +189,4 @@ public void periodic() { hoodMotor.set(0); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 0ff6f64..27244d3 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -30,6 +31,7 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.Constants.SwerveConstants.ModuleData; import frc.robot.SwerveModule; @@ -42,6 +44,12 @@ public class SwerveSubsystem extends SubsystemBase { private SwerveModule[] mSwerveMods; private Field2d field; + private ChassisSpeeds lastCommandedSpeeds = new ChassisSpeeds(); + + private double simYawDegrees = 0.0; + private final double[] simWheelPositionsMeters = new double[4]; + private final Rotation2d[] simWheelAngles = + new Rotation2d[] {new Rotation2d(), new Rotation2d(), new Rotation2d(), new Rotation2d()}; private final StructArrayPublisher swerveDataPublisher = NetworkTableInstance.getDefault() @@ -70,10 +78,30 @@ public SwerveSubsystem() { //puts out the field field = new Field2d(); SmartDashboard.putData("Field", field); + SmartDashboard.putBoolean("Vision Enabled", VisionConstants.VISION_ENABLED_DEFAULT); configurePathPlanner(); } + public void simulationReset() { + if (!RobotBase.isSimulation()) { + return; + } + + simYawDegrees = getYaw().getDegrees(); + for (int i = 0; i < 4; i++) { + simWheelPositionsMeters[i] = 0.0; + simWheelAngles[i] = new Rotation2d(); + } + + pigeon.setYaw(simYawDegrees); + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = new SwerveModulePosition(0.0, simWheelAngles[i]); + } + odometry.resetPosition(Rotation2d.fromDegrees(simYawDegrees), positions, new Pose2d()); + } + private void configurePathPlanner(){ AutoBuilder.configure(this::getPose, @@ -114,28 +142,40 @@ public Command startAutoAt(double x, double y, double direction){ + private boolean isVisionEnabled() { + return SmartDashboard.getBoolean("Vision Enabled", VisionConstants.VISION_ENABLED_DEFAULT); + } + private void updateOdometryWithVision (String limelightName){ boolean doRejectUpdate = false; LimelightHelpers.SetRobotOrientation(limelightName, odometry.getEstimatedPosition().getRotation().getDegrees(),0,0,0,0,0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - if (mt2 == null){ + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + if (mt1 == null){ return; } - if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble())> 720) + if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble()) > VisionConstants.MAX_VISION_ANGULAR_RATE_DEG_PER_SEC) { doRejectUpdate = true; } - if(mt2.tagCount == 0) + if(mt1.tagCount == 0) { doRejectUpdate = true; } if(!doRejectUpdate) { - odometry.setVisionMeasurementStdDevs(VecBuilder.fill (.7,.7,99999));// need to measure + odometry.setVisionMeasurementStdDevs( + VecBuilder.fill( + VisionConstants.VISION_STD_DEV_X_METERS, + VisionConstants.VISION_STD_DEV_Y_METERS, + VisionConstants.VISION_STD_DEV_THETA_RADIANS)); // need to measure odometry.addVisionMeasurement( - mt2.pose, - mt2.timestampSeconds); + mt1.pose, + mt1.timestampSeconds); } + + SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt1.tagCount); + SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt1.avgTagDist); + SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt1.latency); } @@ -152,6 +192,7 @@ public void drive(double xInput, double yInput, double rotationInput, boolean is } public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop){ + lastCommandedSpeeds = driveSpeeds; SwerveModuleState[] desiredStates = SwerveConstants.swerveKinematics.toSwerveModuleStates(driveSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.maxSpeed); @@ -166,6 +207,10 @@ public ChassisSpeeds getChassisSpeeds(){ return SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); } + public ChassisSpeeds getLastCommandedSpeeds() { + return lastCommandedSpeeds; + } + public Pose2d getPose() { return odometry.getEstimatedPosition(); } @@ -246,9 +291,14 @@ public void saveModuleOffsets(Rotation2d desiredAngle){ @Override public void periodic() { - odometry.update(getYaw(), getPositions()); - updateOdometryWithVision("limelight-a"); - updateOdometryWithVision("limelight-b"); + if (!RobotBase.isSimulation()) { + odometry.update(getYaw(), getPositions()); + if (isVisionEnabled()) { + for (String limelightName : VisionConstants.LIMELIGHT_NAMES) { + updateOdometryWithVision(limelightName); + } + } + } field.setRobotPose(getPose()); SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); @@ -270,4 +320,31 @@ public void periodic() { swerveDataPublisher.set(getStates()); } + /** + * Simple swerve simulation: integrates the last commanded chassis speeds into wheel positions and + * a yaw angle, then updates odometry from those simulated sensors. + */ + public void simulationUpdate(double dtSeconds) { + if (!RobotBase.isSimulation()) { + return; + } + + ChassisSpeeds speeds = DriverStation.isDisabled() ? new ChassisSpeeds() : lastCommandedSpeeds; + + simYawDegrees += Math.toDegrees(speeds.omegaRadiansPerSecond * dtSeconds); + pigeon.setYaw(simYawDegrees); + + SwerveModuleState[] states = SwerveConstants.swerveKinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.maxSpeed); + + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + simWheelPositionsMeters[i] += states[i].speedMetersPerSecond * dtSeconds; + simWheelAngles[i] = states[i].angle; + positions[i] = new SwerveModulePosition(simWheelPositionsMeters[i], simWheelAngles[i]); + } + + odometry.update(Rotation2d.fromDegrees(simYawDegrees), positions); + } + } diff --git a/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java new file mode 100644 index 0000000..34a761f --- /dev/null +++ b/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java @@ -0,0 +1,91 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.UnusedAuto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +public class CenterToDepotAuto extends SequentialCommandGroup { + public CenterToDepotAuto (SwerveSubsystem drive) { + final double[] startYawRad = new double[1]; + addCommands( + drive.startAutoAt(4.61, 4.03, 90.0), + new InstantCommand(()->drive.drive(0,0.5,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + + Commands.waitSeconds(1), + + //SHOOT + + new InstantCommand(()-> drive.drive(0.9,0,0,false),drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0,false), drive), + + //Turn ~90 degrees in place (robot-centric) + Commands.runOnce(()->startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + (Math.PI / 2.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + (Math.PI / 2.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + // Move forward ~1m (0.5 m/s for 2s) after turning (to the depot) + new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + Commands.waitSeconds(2), + + //INTAKE + + + // Back up ~0.5m, then turn 180 degrees + new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + new InstantCommand(() -> drive.drive(0.4, 0, 0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + //Turn 40 degrees left (counterclockwise) + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + ); + } +} + diff --git a/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java new file mode 100644 index 0000000..105578a --- /dev/null +++ b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.UnusedAuto; + + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Subsystems.SwerveSubsystem; + +/* +public class DriveTestAuto extends SequentialCommandGroup { + public DriveTestAuto (SwerveSubsystem drive) { + addCommands( + new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + ); + } +} +*/ + + + +public class DriveTestAuto extends SequentialCommandGroup { + public DriveTestAuto (SwerveSubsystem drive){ + addCommands( + drive.startAutoAt(1.165, 6.000, 0.000), + drive.autoDrive("DriveTestPath") + ); + } +} diff --git a/src/main/java/frc/robot/UnusedAuto/EightLemonAuto.java b/src/main/java/frc/robot/UnusedAuto/EightLemonAuto.java new file mode 100644 index 0000000..5677ef4 --- /dev/null +++ b/src/main/java/frc/robot/UnusedAuto/EightLemonAuto.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.UnusedAuto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +//With PATHPLANNER +public class EightLemonAuto extends SequentialCommandGroup { + public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shooter, IntakeSubsystem intake){ + addCommands( + drive.startAutoAt(3.53, 7.13, -130.45), + drive.autoDrive("8FuelPath") + ); + } +} diff --git a/src/main/java/frc/robot/UnusedAuto/OffsetDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/OffsetDepotAuto.java new file mode 100644 index 0000000..53e9c4e --- /dev/null +++ b/src/main/java/frc/robot/UnusedAuto/OffsetDepotAuto.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.UnusedAuto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class OffsetDepotAuto extends SequentialCommandGroup { + + public OffsetDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addCommands( + drive.startAutoAt(3.555, 6.40, 0), + drive.autoDrive("GoToDepot"), + new InstantCommand(() -> intake.raiseIntake(), intake), + new InstantCommand(() -> intake.setIntakePower(IntakeConstants.INTAKE_SPEED), intake), + Commands.waitSeconds(3), + new InstantCommand(() -> intake.setIntakePower(0), intake), + drive.autoDrive("ShootAfterDepot"), + new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + new InstantCommand(() -> shooter.AutoToggleShoot(false)), + Commands.waitSeconds(3), + new InstantCommand(() -> shooter.AutoToggleKickIndex(false)), + new InstantCommand(() -> intake.lowerIntake(), intake), + new InstantCommand(() -> intake.raiseIntake(), intake), + Commands.waitSeconds(1), + new InstantCommand(() -> intake.lowerIntake(), intake), + new InstantCommand(() -> intake.raiseIntake(), intake), + Commands.waitSeconds(4), + new InstantCommand(() -> shooter.AutoToggleShoot(true)) + ); + } +} diff --git a/src/main/java/frc/robot/UnusedAuto/TrenchToDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/TrenchToDepotAuto.java new file mode 100644 index 0000000..d2a72cd --- /dev/null +++ b/src/main/java/frc/robot/UnusedAuto/TrenchToDepotAuto.java @@ -0,0 +1,122 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.UnusedAuto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +public class TrenchToDepotAuto extends SequentialCommandGroup { + public TrenchToDepotAuto (SwerveSubsystem drive){ + final double[] startYawRad = new double[1]; + addCommands( + drive.startAutoAt(3.5, 6.9, 0), + new InstantCommand(()->drive.drive(-0.5,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + + //Turn 40 degrees left (counterclockwise) + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.waitSeconds(2), + + //Turn back 40 degrees right (clockwise) to the starting heading + Commands.run(() -> { + double targetYawRad = startYawRad[0]; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp( + errorRad * 4.0, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0]; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + + //SHOOT + + + //Move to the right (infront of the depot) + new InstantCommand(() -> drive.drive(0, -0.4, 0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + //Turn 180 degrees + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Move forward to the depot + new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.waitSeconds(2), + + + //INTAKE + + + // Back up ~0.5m, then turn 180 degrees + new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + new InstantCommand(() -> drive.drive(0.4, 0, 0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + //Turn 40 degrees left (counterclockwise) + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + + ); + } +} \ No newline at end of file