From 4b0262fa4d46438d11b1a48f69196b73ed0c8f3e Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 27 Jan 2025 20:15:31 -0500 Subject: [PATCH 01/14] Swerve + commencer pathplanner --- src/main/deploy/pathplanner/navgrid.json | 1 + .../deploy/pathplanner/paths/21 x 27.5.path | 54 ++++ .../pathplanner/paths/BlueBasChercher.path | 70 +++++ .../pathplanner/paths/BlueBasStart.path | 54 ++++ .../pathplanner/paths/BlueHautChercher.path | 70 +++++ .../pathplanner/paths/BlueHautPorter.path | 54 ++++ .../pathplanner/paths/BlueHautStart.path | 54 ++++ .../pathplanner/paths/BlueMilieuChercher.path | 54 ++++ .../pathplanner/paths/BlueMilieuStart.path | 54 ++++ src/main/deploy/pathplanner/settings.json | 32 ++ src/main/java/frc/robot/Telemetry.java | 124 ++++++++ .../robot/TunerConstants/TunerConstants.java | 286 +++++++++++++++++ .../subsystems/CommandSwerveDrivetrain.java | 294 ++++++++++++++++++ 13 files changed, 1201 insertions(+) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/21 x 27.5.path create mode 100644 src/main/deploy/pathplanner/paths/BlueBasChercher.path create mode 100644 src/main/deploy/pathplanner/paths/BlueBasStart.path create mode 100644 src/main/deploy/pathplanner/paths/BlueHautChercher.path create mode 100644 src/main/deploy/pathplanner/paths/BlueHautPorter.path create mode 100644 src/main/deploy/pathplanner/paths/BlueHautStart.path create mode 100644 src/main/deploy/pathplanner/paths/BlueMilieuChercher.path create mode 100644 src/main/deploy/pathplanner/paths/BlueMilieuStart.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/frc/robot/Telemetry.java create mode 100644 src/main/java/frc/robot/TunerConstants/TunerConstants.java create mode 100644 src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/21 x 27.5.path b/src/main/deploy/pathplanner/paths/21 x 27.5.path new file mode 100644 index 0000000..ff69bf9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/21 x 27.5.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.066905737704918, + "y": 1.058043032786885 + }, + "prevControl": null, + "nextControl": { + "x": 2.4151156514267984, + "y": 3.2911993104946093 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7641393442622952, + "y": 2.832223360655737 + }, + "prevControl": { + "x": 2.7003578170234857, + "y": 1.9970799919720519 + }, + "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": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueBasChercher.path b/src/main/deploy/pathplanner/paths/BlueBasChercher.path new file mode 100644 index 0000000..463ed3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueBasChercher.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.154713114754099, + "y": 2.832223360655737 + }, + "prevControl": null, + "nextControl": { + "x": 4.627254098360655, + "y": 2.2568135245901635 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.296618852459016, + "y": 1.6334528688524586 + }, + "prevControl": { + "x": 4.5553278688524586, + "y": 1.9091700819672122 + }, + "nextControl": { + "x": 2.853313965378558, + "y": 1.5363479888253109 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1508196721311474, + "y": 1.1299692622950817 + }, + "prevControl": { + "x": 2.1498657381584616, + "y": 1.3782484974959424 + }, + "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": -125.00000000000001 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueBasStart.path b/src/main/deploy/pathplanner/paths/BlueBasStart.path new file mode 100644 index 0000000..e774cab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueBasStart.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.204610655737704, + "y": 1.058043032786885 + }, + "prevControl": null, + "nextControl": { + "x": 6.400623816881145, + "y": 1.8077934662846444 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.25061475409836, + "y": 2.820235655737705 + }, + "prevControl": { + "x": 6.057528863906536, + "y": 2.075180182319969 + }, + "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": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueHautChercher.path b/src/main/deploy/pathplanner/paths/BlueHautChercher.path new file mode 100644 index 0000000..088c9bb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueHautChercher.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.202663934426229, + "y": 5.205788934426229 + }, + "prevControl": null, + "nextControl": { + "x": 4.195696721311475, + "y": 6.104866803278688 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.104815573770492, + "y": 6.296670081967212 + }, + "prevControl": { + "x": 4.495389344262295, + "y": 5.949026639344262 + }, + "nextControl": { + "x": 2.5881297183495136, + "y": 6.425841545822457 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2107581967213117, + "y": 6.955993852459016 + }, + "prevControl": { + "x": 2.302517803906852, + "y": 6.503295936938067 + }, + "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": 125.00000000000001 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueHautPorter.path b/src/main/deploy/pathplanner/paths/BlueHautPorter.path new file mode 100644 index 0000000..bdf1ec8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueHautPorter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.234733606557377, + "y": 6.9679815573770485 + }, + "prevControl": null, + "nextControl": { + "x": 2.271389052764998, + "y": 4.855519714825475 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7761270491803276, + "y": 5.205788934426229 + }, + "prevControl": { + "x": 2.911254448656024, + "y": 6.06899512479262 + }, + "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": -59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueHautStart.path b/src/main/deploy/pathplanner/paths/BlueHautStart.path new file mode 100644 index 0000000..ec8fb0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueHautStart.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.22858606557377, + "y": 7.075870901639344 + }, + "prevControl": null, + "nextControl": { + "x": 5.75022271481963, + "y": 5.75328924697144 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.166700819672132, + "y": 5.205788934426229 + }, + "prevControl": { + "x": 6.247675208676453, + "y": 6.156109471736201 + }, + "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": -119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueMilieuChercher.path b/src/main/deploy/pathplanner/paths/BlueMilieuChercher.path new file mode 100644 index 0000000..4c5fee3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueMilieuChercher.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.838012295081967, + "y": 3.983043032786885 + }, + "prevControl": null, + "nextControl": { + "x": 6.916905737704918, + "y": 2.304764344262295 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1508196721311474, + "y": 1.1299692622950817 + }, + "prevControl": { + "x": 2.125184035848923, + "y": 1.1165271593444053 + }, + "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": -125.00000000000001 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueMilieuStart.path b/src/main/deploy/pathplanner/paths/BlueMilieuStart.path new file mode 100644 index 0000000..1a47993 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueMilieuStart.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.300512295081968, + "y": 3.971055327868852 + }, + "prevControl": null, + "nextControl": { + "x": 6.294286688638351, + "y": 3.9754565484205786 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8619877049180324, + "y": 3.971055327868852 + }, + "prevControl": { + "x": 6.836352068635808, + "y": 3.9576132249181755 + }, + "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": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..7d80073 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "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.35, + "flModuleY": 0.267, + "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/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..edf1979 --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,124 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } + + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); + + /* Telemeterize the module states to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } +} diff --git a/src/main/java/frc/robot/TunerConstants/TunerConstants.java b/src/main/java/frc/robot/TunerConstants/TunerConstants.java new file mode 100644 index 0000000..fb38c67 --- /dev/null +++ b/src/main/java/frc/robot/TunerConstants/TunerConstants.java @@ -0,0 +1,286 @@ +package frc.robot.TunerConstants; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + +import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.91).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 0; + + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 13; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 2; + private static final int kFrontLeftSteerMotorId = 6; + private static final int kFrontLeftEncoderId = 9; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.046142578125); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(13.75); + private static final Distance kFrontLeftYPos = Inches.of(13.75); + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 12; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.116455078125); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(13.75); + private static final Distance kFrontRightYPos = Inches.of(-13.75); + + // Back Left + private static final int kBackLeftDriveMotorId = 3; + private static final int kBackLeftSteerMotorId = 7; + private static final int kBackLeftEncoderId = 10; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.046630859375); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-13.75); + private static final Distance kBackLeftYPos = Inches.of(13.75); + + // Back Right + private static final int kBackRightDriveMotorId = 18; + private static final int kBackRightSteerMotorId = 8; + private static final int kBackRightEncoderId = 11; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.016357421875); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-13.75); + private static final Distance kBackRightYPos = Inches.of(-13.75); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..023db69 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -0,0 +1,294 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; + +/** + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements + * Subsystem so it can easily be used in command-based projects. + */ +public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this + ) + ); + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this + ) + ); + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this + ) + ); + + private void configureAutoBuilder() { + try { + var config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, // Supplier of current robot pose + this::resetPose, // Consumer for seeding pose against auto + () -> getState().Speeds, // Supplier of current robot speeds + // Consumer of ChassisSpeeds and feedforwards to drive the robot + (speeds, feedforwards) -> setControl( + m_pathApplyRobotSpeeds.withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) + ), + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(10, 0, 0), + // PID constants for rotation + new PIDConstants(7, 0, 0) + ), + config, + // Assume the path needs to be flipped for Red vs Blue, this is normally the case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this // Subsystem for requirements + ); + } catch (Exception ex) { + DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + } + } + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + configureAutoBuilder(); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + configureAutoBuilder(); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + configureAutoBuilder(); + } + + + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + @Override + public void periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); + m_hasAppliedOperatorPerspective = true; + }); + } + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } +} From 773e3b2244e97367ef8f7c9665fe970ba60a96ce Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Wed, 29 Jan 2025 19:23:28 -0500 Subject: [PATCH 02/14] pathplanner --- .../deploy/pathplanner/autos/New Auto.auto | 133 ++++++++++++++++++ .../pathplanner/paths/BlueBasChercher.path | 16 +-- .../{21 x 27.5.path => BlueBasChercher2.path} | 20 +-- .../pathplanner/paths/BlueBasPorter.path | 54 +++++++ ...ilieuChercher.path => BlueBasPorter2.path} | 20 +-- .../pathplanner/paths/BlueBasStart.path | 8 +- src/main/deploy/pathplanner/settings.json | 12 +- 7 files changed, 225 insertions(+), 38 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto rename src/main/deploy/pathplanner/paths/{21 x 27.5.path => BlueBasChercher2.path} (77%) create mode 100644 src/main/deploy/pathplanner/paths/BlueBasPorter.path rename src/main/deploy/pathplanner/paths/{BlueMilieuChercher.path => BlueBasPorter2.path} (75%) diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..e11a51b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,133 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasChercher" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "path", + "data": { + "pathName": "BlueBasPorter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueBasChercher.path b/src/main/deploy/pathplanner/paths/BlueBasChercher.path index 463ed3f..7a2e9c8 100644 --- a/src/main/deploy/pathplanner/paths/BlueBasChercher.path +++ b/src/main/deploy/pathplanner/paths/BlueBasChercher.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.154713114754099, - "y": 2.832223360655737 + "x": 5.226639344262295, + "y": 2.748309426229507 }, "prevControl": null, "nextControl": { - "x": 4.627254098360655, - "y": 2.2568135245901635 + "x": 4.699180327868851, + "y": 2.1728995901639334 }, "isLocked": false, "linkedName": null @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 1.1508196721311474, - "y": 1.1299692622950817 + "x": 1.2227459016393443, + "y": 1.141956967213115 }, "prevControl": { - "x": 2.1498657381584616, - "y": 1.3782484974959424 + "x": 2.2217919676666584, + "y": 1.3902362024139756 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/21 x 27.5.path b/src/main/deploy/pathplanner/paths/BlueBasChercher2.path similarity index 77% rename from src/main/deploy/pathplanner/paths/21 x 27.5.path rename to src/main/deploy/pathplanner/paths/BlueBasChercher2.path index ff69bf9..9216503 100644 --- a/src/main/deploy/pathplanner/paths/21 x 27.5.path +++ b/src/main/deploy/pathplanner/paths/BlueBasChercher2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.066905737704918, - "y": 1.058043032786885 + "x": 3.7761270491803276, + "y": 2.7602971311475404 }, "prevControl": null, "nextControl": { - "x": 2.4151156514267984, - "y": 3.2911993104946093 + "x": 3.152766393442623, + "y": 1.2138831967213104 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7641393442622952, - "y": 2.832223360655737 + "x": 1.1987704918032787, + "y": 1.153944672131147 }, "prevControl": { - "x": 2.7003578170234857, - "y": 1.9970799919720519 + "x": 2.049897540983607, + "y": 1.1899077868852443 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -125.00000000000001 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueBasPorter.path b/src/main/deploy/pathplanner/paths/BlueBasPorter.path new file mode 100644 index 0000000..8157caa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueBasPorter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1987704918032787, + "y": 1.153944672131147 + }, + "prevControl": null, + "nextControl": { + "x": 2.1697745901639345, + "y": 1.0820184426229504 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.788114754098361, + "y": 2.7722848360655736 + }, + "prevControl": { + "x": 3.5483606557377048, + "y": 1.1899077868852452 + }, + "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": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueMilieuChercher.path b/src/main/deploy/pathplanner/paths/BlueBasPorter2.path similarity index 75% rename from src/main/deploy/pathplanner/paths/BlueMilieuChercher.path rename to src/main/deploy/pathplanner/paths/BlueBasPorter2.path index 4c5fee3..1ae7e8c 100644 --- a/src/main/deploy/pathplanner/paths/BlueMilieuChercher.path +++ b/src/main/deploy/pathplanner/paths/BlueBasPorter2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.838012295081967, - "y": 3.983043032786885 + "x": 1.2107581967213117, + "y": 1.141956967213115 }, "prevControl": null, "nextControl": { - "x": 6.916905737704918, - "y": 2.304764344262295 + "x": 3.392520491803279, + "y": 1.7293545081967203 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1508196721311474, - "y": 1.1299692622950817 + "x": 2.9969262295081966, + "y": 4.019006147540983 }, "prevControl": { - "x": 2.125184035848923, - "y": 1.1165271593444053 + "x": 1.738217213114754, + "y": 3.5035348360655734 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": -125.00000000000001 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueBasStart.path b/src/main/deploy/pathplanner/paths/BlueBasStart.path index e774cab..7ac4271 100644 --- a/src/main/deploy/pathplanner/paths/BlueBasStart.path +++ b/src/main/deploy/pathplanner/paths/BlueBasStart.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.25061475409836, - "y": 2.820235655737705 + "x": 5.214651639344262, + "y": 2.7722848360655736 }, "prevControl": { - "x": 6.057528863906536, - "y": 2.075180182319969 + "x": 6.021565749152438, + "y": 2.0272293626478377 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 7d80073..e97c2f6 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -20,12 +20,12 @@ "wheelCOF": 1.2, "flModuleX": 0.35, "flModuleY": 0.267, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "frModuleX": 0.35, + "frModuleY": -0.267, + "blModuleX": -0.35, + "blModuleY": 0.267, + "brModuleX": -0.35, + "brModuleY": -0.267, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] From 76ea02c3dd465bf5c317d5d7fde237c49e901abf Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Wed, 29 Jan 2025 20:19:13 -0500 Subject: [PATCH 03/14] MODES AUTO FAITSES --- .../deploy/pathplanner/autos/BlueBas1.auto | 57 +++++ .../autos/{New Auto.auto => BlueBas2.auto} | 12 + .../deploy/pathplanner/autos/BlueBas3.auto | 233 ++++++++++++++++++ .../deploy/pathplanner/autos/BlueHaut1.auto | 57 +++++ .../deploy/pathplanner/autos/BlueHaut2.auto | 145 +++++++++++ .../deploy/pathplanner/autos/BlueHaut3.auto | 233 ++++++++++++++++++ .../deploy/pathplanner/autos/MilieuAlgue.auto | 69 ++++++ .../pathplanner/autos/MilieuPasAlgue.auto | 57 +++++ src/main/deploy/pathplanner/autos/Sortir.auto | 19 ++ .../pathplanner/paths/BlueHautChercher.path | 32 +-- .../pathplanner/paths/BlueHautChercher2.path | 54 ++++ .../pathplanner/paths/BlueHautPorter.path | 16 +- .../pathplanner/paths/BlueHautPorter2.path | 54 ++++ .../pathplanner/paths/BlueHautStart.path | 8 +- .../pathplanner/paths/BlueMilieuStart.path | 4 +- src/main/deploy/pathplanner/paths/Sortir.path | 54 ++++ 16 files changed, 1066 insertions(+), 38 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BlueBas1.auto rename src/main/deploy/pathplanner/autos/{New Auto.auto => BlueBas2.auto} (93%) create mode 100644 src/main/deploy/pathplanner/autos/BlueBas3.auto create mode 100644 src/main/deploy/pathplanner/autos/BlueHaut1.auto create mode 100644 src/main/deploy/pathplanner/autos/BlueHaut2.auto create mode 100644 src/main/deploy/pathplanner/autos/BlueHaut3.auto create mode 100644 src/main/deploy/pathplanner/autos/MilieuAlgue.auto create mode 100644 src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto create mode 100644 src/main/deploy/pathplanner/autos/Sortir.auto create mode 100644 src/main/deploy/pathplanner/paths/BlueHautChercher2.path create mode 100644 src/main/deploy/pathplanner/paths/BlueHautPorter2.path create mode 100644 src/main/deploy/pathplanner/paths/Sortir.path diff --git a/src/main/deploy/pathplanner/autos/BlueBas1.auto b/src/main/deploy/pathplanner/autos/BlueBas1.auto new file mode 100644 index 0000000..7c320ae --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BlueBas1.auto @@ -0,0 +1,57 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/BlueBas2.auto similarity index 93% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/BlueBas2.auto index e11a51b..62b0de7 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas2.auto @@ -48,6 +48,12 @@ "name": null } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "parallel", "data": { @@ -123,6 +129,12 @@ } ] } + }, + { + "type": "named", + "data": { + "name": null + } } ] } diff --git a/src/main/deploy/pathplanner/autos/BlueBas3.auto b/src/main/deploy/pathplanner/autos/BlueBas3.auto new file mode 100644 index 0000000..ee917a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BlueBas3.auto @@ -0,0 +1,233 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasChercher" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "path", + "data": { + "pathName": "BlueBasPorter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasChercher2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueBasPorter2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BlueHaut1.auto b/src/main/deploy/pathplanner/autos/BlueHaut1.auto new file mode 100644 index 0000000..90a7062 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BlueHaut1.auto @@ -0,0 +1,57 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BlueHaut2.auto b/src/main/deploy/pathplanner/autos/BlueHaut2.auto new file mode 100644 index 0000000..f54dfe2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BlueHaut2.auto @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautChercher" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "path", + "data": { + "pathName": "BlueHautPorter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BlueHaut3.auto b/src/main/deploy/pathplanner/autos/BlueHaut3.auto new file mode 100644 index 0000000..f9be6f9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BlueHaut3.auto @@ -0,0 +1,233 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautChercher" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "path", + "data": { + "pathName": "BlueHautPorter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautChercher2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueHautPorter2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MilieuAlgue.auto b/src/main/deploy/pathplanner/autos/MilieuAlgue.auto new file mode 100644 index 0000000..17073b3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MilieuAlgue.auto @@ -0,0 +1,69 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueMilieuStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto b/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto new file mode 100644 index 0000000..b724388 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto @@ -0,0 +1,57 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueMilieuStart" + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": null + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Sortir.auto b/src/main/deploy/pathplanner/autos/Sortir.auto new file mode 100644 index 0000000..191a9fc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Sortir.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sortir" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueHautChercher.path b/src/main/deploy/pathplanner/paths/BlueHautChercher.path index 088c9bb..3bcf7fb 100644 --- a/src/main/deploy/pathplanner/paths/BlueHautChercher.path +++ b/src/main/deploy/pathplanner/paths/BlueHautChercher.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.202663934426229, - "y": 5.205788934426229 + "x": 5.262602459016393, + "y": 5.33765368852459 }, "prevControl": null, "nextControl": { - "x": 4.195696721311475, - "y": 6.104866803278688 + "x": 4.567315573770491, + "y": 6.272694672131147 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.104815573770492, - "y": 6.296670081967212 + "x": 1.3066598360655735, + "y": 6.920030737704917 }, "prevControl": { - "x": 4.495389344262295, - "y": 5.949026639344262 - }, - "nextControl": { - "x": 2.5881297183495136, - "y": 6.425841545822457 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2107581967213117, - "y": 6.955993852459016 - }, - "prevControl": { - "x": 2.302517803906852, - "y": 6.503295936938067 + "x": 2.074600566094311, + "y": 6.608197684403563 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueHautChercher2.path b/src/main/deploy/pathplanner/paths/BlueHautChercher2.path new file mode 100644 index 0000000..aab02f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueHautChercher2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.728176229508197, + "y": 5.277715163934426 + }, + "prevControl": null, + "nextControl": { + "x": 3.6884488572050125, + "y": 5.738000701502482 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2946721311475409, + "y": 6.88406762295082 + }, + "prevControl": { + "x": 1.5370661625758562, + "y": 6.822870126858832 + }, + "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": 125.00000000000001 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueHautPorter.path b/src/main/deploy/pathplanner/paths/BlueHautPorter.path index bdf1ec8..e206561 100644 --- a/src/main/deploy/pathplanner/paths/BlueHautPorter.path +++ b/src/main/deploy/pathplanner/paths/BlueHautPorter.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.234733606557377, - "y": 6.9679815573770485 + "x": 1.2946721311475409, + "y": 6.88406762295082 }, "prevControl": null, "nextControl": { - "x": 2.271389052764998, - "y": 4.855519714825475 + "x": 1.8197466808473133, + "y": 6.152201615570423 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7761270491803276, - "y": 5.205788934426229 + "x": 3.7401639344262296, + "y": 5.313678278688524 }, "prevControl": { - "x": 2.911254448656024, - "y": 6.06899512479262 + "x": 2.9533875048545335, + "y": 6.158498893994983 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueHautPorter2.path b/src/main/deploy/pathplanner/paths/BlueHautPorter2.path new file mode 100644 index 0000000..f2c8025 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueHautPorter2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2467213114754097, + "y": 6.896055327868852 + }, + "prevControl": null, + "nextControl": { + "x": 3.728176229508196, + "y": 6.28468237704918 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9969262295081966, + "y": 3.9950307377049183 + }, + "prevControl": { + "x": 2.6372950819672134, + "y": 4.906096311475409 + }, + "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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 125.00000000000001 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueHautStart.path b/src/main/deploy/pathplanner/paths/BlueHautStart.path index ec8fb0a..7bf89b1 100644 --- a/src/main/deploy/pathplanner/paths/BlueHautStart.path +++ b/src/main/deploy/pathplanner/paths/BlueHautStart.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.166700819672132, - "y": 5.205788934426229 + "x": 5.226639344262295, + "y": 5.313678278688524 }, "prevControl": { - "x": 6.247675208676453, - "y": 6.156109471736201 + "x": 6.307613733266616, + "y": 6.263998815998496 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueMilieuStart.path b/src/main/deploy/pathplanner/paths/BlueMilieuStart.path index 1a47993..379e3bf 100644 --- a/src/main/deploy/pathplanner/paths/BlueMilieuStart.path +++ b/src/main/deploy/pathplanner/paths/BlueMilieuStart.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 5.8619877049180324, + "x": 5.9818647540983605, "y": 3.971055327868852 }, "prevControl": { - "x": 6.836352068635808, + "x": 6.956229117816136, "y": 3.9576132249181755 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Sortir.path b/src/main/deploy/pathplanner/paths/Sortir.path new file mode 100644 index 0000000..17ff327 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sortir.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.504303278688524, + "y": 6.464497950819672 + }, + "prevControl": null, + "nextControl": { + "x": 7.084733606557376, + "y": 6.488473360655737 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.849999999999999, + "y": 6.464497950819672 + }, + "prevControl": { + "x": 6.32950819672131, + "y": 6.464497950819672 + }, + "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": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From b0ce8aaad77aa1b5483b8a4cbafb89c0c45e7c7e Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 30 Jan 2025 19:18:56 -0500 Subject: [PATCH 04/14] Modes Autos complets --- .../deploy/pathplanner/autos/BlueBas1.auto | 6 +- .../deploy/pathplanner/autos/BlueBas2.auto | 22 ++++-- .../deploy/pathplanner/autos/BlueBas3.auto | 42 ++++++---- .../deploy/pathplanner/autos/BlueHaut1.auto | 6 +- .../deploy/pathplanner/autos/BlueHaut2.auto | 24 +++--- .../deploy/pathplanner/autos/BlueHaut3.auto | 42 ++++++---- .../deploy/pathplanner/autos/MilieuAlgue.auto | 28 +++++-- .../pathplanner/autos/MilieuPasAlgue.auto | 6 +- src/main/java/frc/robot/RobotContainer.java | 78 +++++++++++++++++-- 9 files changed, 186 insertions(+), 68 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BlueBas1.auto b/src/main/deploy/pathplanner/autos/BlueBas1.auto index 7c320ae..6b665ea 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas1.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas1.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueBas2.auto b/src/main/deploy/pathplanner/autos/BlueBas2.auto index 62b0de7..57d7bf7 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas2.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas2.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralExpire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueBas3.auto b/src/main/deploy/pathplanner/autos/BlueBas3.auto index ee917a3..fe42865 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas3.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas3.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -159,13 +165,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoralInspire" } } ] @@ -174,12 +180,18 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -197,13 +209,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] @@ -212,7 +224,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] @@ -221,7 +233,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueHaut1.auto b/src/main/deploy/pathplanner/autos/BlueHaut1.auto index 90a7062..05c0205 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut1.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut1.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueHaut2.auto b/src/main/deploy/pathplanner/autos/BlueHaut2.auto index f54dfe2..f3cc72f 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut2.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut2.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,13 +45,13 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 1.0 } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueHaut3.auto b/src/main/deploy/pathplanner/autos/BlueHaut3.auto index f9be6f9..e492533 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut3.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut3.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -159,13 +165,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -174,12 +180,18 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -197,13 +209,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -212,7 +224,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] @@ -221,7 +233,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/MilieuAlgue.auto b/src/main/deploy/pathplanner/autos/MilieuAlgue.auto index 17073b3..f618a26 100644 --- a/src/main/deploy/pathplanner/autos/MilieuAlgue.auto +++ b/src/main/deploy/pathplanner/autos/MilieuAlgue.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,25 +33,43 @@ { "type": "named", "data": { - "name": null + "name": "L4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoralExpire" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "L3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoraletAlgue" } } ] diff --git a/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto b/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto index b724388..999860b 100644 --- a/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto +++ b/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,13 +33,13 @@ { "type": "named", "data": { - "name": null + "name": "L4" } }, { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..8c59bf6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,17 +4,81 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + +import frc.robot.TunerConstants.TunerConstants; +import frc.robot.commands.Grimpe; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Grimpeur; + public class RobotContainer { - public RobotContainer() { - configureBindings(); - } + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private void configureBindings() {} + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + private final Telemetry logger = new Telemetry(MaxSpeed); + + private final CommandXboxController joystick = new CommandXboxController(0); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + private final SendableChooser autoChooser; + + + + public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser("New Auto"); + SmartDashboard.putData("Auto Mode", autoChooser); + configureBindings(); + } + + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + ); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + + drivetrain.registerTelemetry(logger::telemeterize); + } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } } From a5df627b6499107462fb35fbd95b42cd7df8eda3 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 30 Jan 2025 19:20:09 -0500 Subject: [PATCH 05/14] oups --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c59bf6..6827398 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.TunerConstants.TunerConstants; -import frc.robot.commands.Grimpe; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Grimpeur; From cf29380c648b9adb17a82de81f6ee854741de493 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 6 Feb 2025 17:43:16 -0500 Subject: [PATCH 06/14] simulation --- simgui-ds.json | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 simgui-ds.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} From de156e3789c85cd0a7b9b505b5303e382d5272aa Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 6 Feb 2025 17:59:57 -0500 Subject: [PATCH 07/14] joystick -> manettes --- src/main/java/frc/robot/RobotContainer.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6827398..dc06d05 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,7 +38,8 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); - private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController manette1 = new CommandXboxController(0); + private final CommandXboxController manette2 = new CommandXboxController(0); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @@ -58,21 +59,21 @@ public class RobotContainer { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + drive.withVelocityX(-manette1.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-manette1.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-manette1.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) ) ); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + manette1.back().and(manette1.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + manette1.back().and(manette1.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + manette1.start().and(manette1.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + manette1.start().and(manette1.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + manette1.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); drivetrain.registerTelemetry(logger::telemeterize); } From 9af46de189e2377d870813a478170e5c24970225 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 6 Feb 2025 18:03:36 -0500 Subject: [PATCH 08/14] oups --- src/main/java/frc/robot/RobotContainer.java | 46 +++++++++++++++++---- 1 file changed, 37 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9590170..41aa937 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,20 +4,48 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + + +import frc.robot.subsystems.Grimpeur; + public class RobotContainer { - CommandXboxController manette1 = new CommandXboxController(0); - CommandXboxController manette2 = new CommandXboxController(0); - public RobotContainer() { - configureBindings(); - } - private void configureBindings() {} + private final CommandXboxController manette1 = new CommandXboxController(0); + private final CommandXboxController manette2 = new CommandXboxController(0); + + public RobotContainer() { - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + configureBindings(); + } + + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + + } + + public Command getAutonomousCommand() { + return Commands.print(""); + } } From 6fb4e0c1ac687785124e54a16fc23decf4d59d8f Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 6 Feb 2025 18:04:02 -0500 Subject: [PATCH 09/14] w --- src/main/java/frc/robot/RobotContainer.java | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 41aa937..379fa5b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,26 +4,9 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; - -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - - -import frc.robot.subsystems.Grimpeur; public class RobotContainer { From 029bba7bb61705813104408fd69c4de94703cee3 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 6 Feb 2025 18:43:16 -0500 Subject: [PATCH 10/14] huh? --- src/main/deploy/pathplanner/paths/BlueBasPorter2.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 +------- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BlueBasPorter2.path b/src/main/deploy/pathplanner/paths/BlueBasPorter2.path index 1ae7e8c..bf2e09d 100644 --- a/src/main/deploy/pathplanner/paths/BlueBasPorter2.path +++ b/src/main/deploy/pathplanner/paths/BlueBasPorter2.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 3.392520491803279, - "y": 1.7293545081967203 + "y": 1.7293545081967205 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc06d05..e7bb386 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,15 +65,9 @@ public class RobotContainer { ) ); - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - manette1.back().and(manette1.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - manette1.back().and(manette1.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - manette1.start().and(manette1.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - manette1.start().and(manette1.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press - manette1.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); drivetrain.registerTelemetry(logger::telemeterize); } From 9f017968e149d419ad704b21c923f10f251dac76 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 10 Feb 2025 18:33:37 -0500 Subject: [PATCH 11/14] swerve qui fonctionne --- Reefscape-tuner-project.json | 1 + src/main/java/frc/robot/RobotContainer.java | 7 +-- .../robot/TunerConstants/TunerConstants.java | 54 +++++++++---------- tuner-swerve-project2025.json | 1 + 4 files changed, 33 insertions(+), 30 deletions(-) create mode 100644 Reefscape-tuner-project.json create mode 100644 tuner-swerve-project2025.json diff --git a/Reefscape-tuner-project.json b/Reefscape-tuner-project.json new file mode 100644 index 0000000..ac720ea --- /dev/null +++ b/Reefscape-tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":12,"Name":"avant droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":5,"Name":"AvantDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":4,"Name":"AvantDroitDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.353271484375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":5,"ValidatedDriveId":4,"ValidatedEncoderId":12},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":9,"Name":"avant gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":6,"Name":"avantGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":2,"Name":"AvantGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.2119140625,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":6,"ValidatedDriveId":2,"ValidatedEncoderId":9},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":11,"Name":"arriere droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":8,"Name":"ArriereDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":18,"Name":"ArriereDroiteDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.236572265625,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":8,"ValidatedDriveId":18,"ValidatedEncoderId":11},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":10,"Name":"arriere gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"ArriereGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":3,"Name":"ArriereGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.330078125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":3,"ValidatedEncoderId":10}],"SwerveOptions":{"kSpeedAt12Volts":5.213368288877142,"Gyro":{"Id":13,"Name":"Pigeon 2 vers. S (Device ID 13)","Model":"Pigeon 2 vers. S","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":27.0,"HorizontalTrackSizeInches":21.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":4,"SwerveModuleConfiguration":{"ModuleBrand":4,"DriveRatio":6.122448979591837,"SteerRatio":21.428571428571427,"CouplingRatio":3.5714285714285716,"CustomName":"L3"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Swerve Drive Specialties (SDS)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7bb386..0a0ac8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -59,9 +60,9 @@ public class RobotContainer { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(-manette1.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-manette1.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-manette1.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left) ) ); diff --git a/src/main/java/frc/robot/TunerConstants/TunerConstants.java b/src/main/java/frc/robot/TunerConstants/TunerConstants.java index fb38c67..e45fc2a 100644 --- a/src/main/java/frc/robot/TunerConstants/TunerConstants.java +++ b/src/main/java/frc/robot/TunerConstants/TunerConstants.java @@ -25,7 +25,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(1.91).withKA(0) + .withKS(0.1).withKV(2.66).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -78,10 +78,10 @@ public class TunerConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; + private static final double kCoupleRatio = 3.5714285714285716; private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 15.42857142857143; + private static final double kSteerGearRatio = 21.428571428571427; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; @@ -126,48 +126,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 2; - private static final int kFrontLeftSteerMotorId = 6; - private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.046142578125); + private static final int kFrontLeftDriveMotorId = 4; + private static final int kFrontLeftSteerMotorId = 5; + private static final int kFrontLeftEncoderId = 12; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.353271484375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(13.75); - private static final Distance kFrontLeftYPos = Inches.of(13.75); + private static final Distance kFrontLeftXPos = Inches.of(13.5); + private static final Distance kFrontLeftYPos = Inches.of(10.5); // Front Right - private static final int kFrontRightDriveMotorId = 4; - private static final int kFrontRightSteerMotorId = 5; - private static final int kFrontRightEncoderId = 12; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.116455078125); + private static final int kFrontRightDriveMotorId = 2; + private static final int kFrontRightSteerMotorId = 6; + private static final int kFrontRightEncoderId = 9; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2119140625); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(13.75); - private static final Distance kFrontRightYPos = Inches.of(-13.75); + private static final Distance kFrontRightXPos = Inches.of(13.5); + private static final Distance kFrontRightYPos = Inches.of(-10.5); // Back Left - private static final int kBackLeftDriveMotorId = 3; - private static final int kBackLeftSteerMotorId = 7; - private static final int kBackLeftEncoderId = 10; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.046630859375); + private static final int kBackLeftDriveMotorId = 18; + private static final int kBackLeftSteerMotorId = 8; + private static final int kBackLeftEncoderId = 11; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.236572265625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-13.75); - private static final Distance kBackLeftYPos = Inches.of(13.75); + private static final Distance kBackLeftXPos = Inches.of(-13.5); + private static final Distance kBackLeftYPos = Inches.of(10.5); // Back Right - private static final int kBackRightDriveMotorId = 18; - private static final int kBackRightSteerMotorId = 8; - private static final int kBackRightEncoderId = 11; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.016357421875); + private static final int kBackRightDriveMotorId = 3; + private static final int kBackRightSteerMotorId = 7; + private static final int kBackRightEncoderId = 10; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.330078125); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-13.75); - private static final Distance kBackRightYPos = Inches.of(-13.75); + private static final Distance kBackRightXPos = Inches.of(-13.5); + private static final Distance kBackRightYPos = Inches.of(-10.5); public static final SwerveModuleConstants FrontLeft = diff --git a/tuner-swerve-project2025.json b/tuner-swerve-project2025.json new file mode 100644 index 0000000..116bb6d --- /dev/null +++ b/tuner-swerve-project2025.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":10,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":9,"Name":"avant gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":6,"Name":"avantGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":2,"Name":"AvantGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.046142578125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":6,"ValidatedDriveId":2,"ValidatedEncoderId":9},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":12,"Name":"avant droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":5,"Name":"AvantDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":4,"Name":"AvantDroitDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.116455078125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":5,"ValidatedDriveId":4,"ValidatedEncoderId":12},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":10,"Name":"arriere gauche","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"ArriereGaucheAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":3,"Name":"ArriereGaucheDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.046630859375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":3,"ValidatedEncoderId":10},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":11,"Name":"arriere droit","Model":"CANCoder vers. H","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":8,"Name":"ArriereDroitAg","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":18,"Name":"ArriereDroiteDr","Model":"Talon FX vers. C","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.016357421875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":8,"ValidatedDriveId":18,"ValidatedEncoderId":11}],"SwerveOptions":{"kSpeedAt12Volts":5.213368288877142,"Gyro":{"Id":13,"Name":"Pigeon 2 vers. S (Device ID 13)","Model":"Pigeon 2 vers. S","CANbus":"EB65A132463847532020204B103B0DFF","CANbusFriendly":"swerve","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":27.0,"HorizontalTrackSizeInches":21.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":4,"SwerveModuleConfiguration":{"ModuleBrand":4,"DriveRatio":6.122448979591837,"SteerRatio":21.428571428571427,"CouplingRatio":3.5714285714285716,"CustomName":"L3"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Swerve Drive Specialties (SDS)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file From ab241f2f6593c753192df11aa716c91ece120717 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 10 Feb 2025 19:11:48 -0500 Subject: [PATCH 12/14] so;hwistesZ --- src/main/java/frc/robot/RobotContainer.java | 15 ++++++-- src/main/java/frc/robot/commands/RainBow.java | 37 ++++++++++++++++++ .../java/frc/robot/subsystems/Bougie.java | 38 +++++++++++++++++++ 3 files changed, 86 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/RainBow.java create mode 100644 src/main/java/frc/robot/subsystems/Bougie.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a0ac8f..559996c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,10 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.TunerConstants.TunerConstants; +import frc.robot.commands.RainBow; +import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Grimpeur; @@ -45,7 +48,7 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private final SendableChooser autoChooser; - + Bougie bougie = new Bougie(); public RobotContainer() { @@ -73,7 +76,11 @@ public class RobotContainer { drivetrain.registerTelemetry(logger::telemeterize); } - public Command getAutonomousCommand() { - return autoChooser.getSelected(); - } + + public Command getAutonomousCommand() { + return new ParallelCommandGroup( + autoChooser.getSelected(), + new RainBow(bougie) + ); + } } diff --git a/src/main/java/frc/robot/commands/RainBow.java b/src/main/java/frc/robot/commands/RainBow.java new file mode 100644 index 0000000..2c4e5e2 --- /dev/null +++ b/src/main/java/frc/robot/commands/RainBow.java @@ -0,0 +1,37 @@ +// 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.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Bougie; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class RainBow extends Command { + Bougie bougie; + /** Creates a new RainBow. */ + public RainBow(Bougie bougie) { + this.bougie = bougie; + addRequirements(bougie); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {bougie.RainBow();} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {bougie.RainBowStop();} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Bougie.java b/src/main/java/frc/robot/subsystems/Bougie.java new file mode 100644 index 0000000..9876824 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Bougie.java @@ -0,0 +1,38 @@ +// 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.subsystems; + +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.RainbowAnimation; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Bougie extends SubsystemBase { + CANdle candle = new CANdle(5); + CANdleConfiguration config = new CANdleConfiguration(); + RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 64); + /** Creates a new Bougie. */ + public Bougie() { + config.brightnessScalar = 0.5; + candle.configAllSettings(config); + } + public void Rouge() { + candle.setLEDs(255, 0, 0); + } + public void Vert() { + candle.setLEDs(0, 255, 0); + } + public void Bleu() { + candle.setLEDs(0, 0, 255); + } + public void RainBow(){candle.animate(rainbowAnim);} + public void RainBowStop(){candle.animate(null);} + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} + From 8bd9ce36a24a3275fbdc6388933fae6516d8c586 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 10 Feb 2025 19:12:40 -0500 Subject: [PATCH 13/14] hehe --- src/main/java/frc/robot/RobotContainer.java | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 559996c..e457019 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,9 +78,16 @@ public class RobotContainer { public Command getAutonomousCommand() { - return new ParallelCommandGroup( - autoChooser.getSelected(), - new RainBow(bougie) + return + new + ParallelCommandGroup( + autoChooser + .getSelected(), + new + RainBow + ( + bougie + ) ); } } From c0c48a3f248735f5148ab50fcbf114ef61e4dec2 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 10 Feb 2025 20:00:26 -0500 Subject: [PATCH 14/14] message --- src/main/java/frc/robot/RobotContainer.java | 25 --------------------- 1 file changed, 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ad1be7a..e457019 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,18 +20,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -<<<<<<< HEAD -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - - -public class RobotContainer { - - private final CommandXboxController manette1 = new CommandXboxController(0); - private final CommandXboxController manette2 = new CommandXboxController(0); - - public RobotContainer() { - -======= import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -66,24 +54,12 @@ public class RobotContainer { public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("New Auto"); SmartDashboard.putData("Auto Mode", autoChooser); ->>>>>>> Swerve configureBindings(); } private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. -<<<<<<< HEAD - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - - } - - public Command getAutonomousCommand() { - return Commands.print(""); - } -======= drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> @@ -114,5 +90,4 @@ public class RobotContainer { ) ); } ->>>>>>> Swerve }