diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 43de0258..27662369 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,15 +6,15 @@ "Lit Left Ring Auto Paths", "Macho Middle Ring Auto Paths", "Middle 4 No LL Piece Auto Paths", - "Source Side WIP", - "Playoff Path", - "Rad Right Ring Auto Paths" + "5 Piece Paths", + "Rad Right Ring Auto Paths", + "Source Side WIP" ], "autoFolders": [ "Amp Autos", "Center Autos", - "Autos I'd like to use", - "Source Autos" + "Source Autos", + "The Cool Useful Ones" ], "defaultMaxVel": 2.0, "defaultMaxAccel": 1.0, diff --git a/src/main/.pathplanner/settings.json b/src/main/.pathplanner/settings.json index 7a35407c..6d437f92 100644 --- a/src/main/.pathplanner/settings.json +++ b/src/main/.pathplanner/settings.json @@ -3,15 +3,16 @@ "robotLength": 0.9, "holonomicMode": true, "pathFolders": [ - "Amp Side Auto Paths", - "Center Auto Paths", - "Source Side Auto Paths" + "5 Piece Paths", + "Left Ring Autos", + "Middle Ring Autos", + "Right Ring Autos" ], "autoFolders": [ "Amp Side Autos", "Center Autos", - "Test (so exciting!!!!)", - "Source Side Autos" + "Source Side Autos", + "ignore for now" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/deploy/pathplanner/autos/2 Piece (Outer Note) No LL - Source.auto b/src/main/deploy/pathplanner/autos/2 Piece (Outer Note) No LL - Source.auto index ca68e25e..6f615ca0 100644 --- a/src/main/deploy/pathplanner/autos/2 Piece (Outer Note) No LL - Source.auto +++ b/src/main/deploy/pathplanner/autos/2 Piece (Outer Note) No LL - Source.auto @@ -51,6 +51,6 @@ ] } }, - "folder": "Autos I'd like to use", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Piece (Outer Note), No Lime - Amp.auto b/src/main/deploy/pathplanner/autos/2 Piece (Outer Note), No Lime - Amp.auto index 5e52d8f8..c8946fa3 100644 --- a/src/main/deploy/pathplanner/autos/2 Piece (Outer Note), No Lime - Amp.auto +++ b/src/main/deploy/pathplanner/autos/2 Piece (Outer Note), No Lime - Amp.auto @@ -51,6 +51,6 @@ ] } }, - "folder": "Autos I'd like to use", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Piece - Center - No Limelight.auto b/src/main/deploy/pathplanner/autos/2 Piece - Center - No Limelight.auto index e4bae741..996e6069 100644 --- a/src/main/deploy/pathplanner/autos/2 Piece - Center - No Limelight.auto +++ b/src/main/deploy/pathplanner/autos/2 Piece - Center - No Limelight.auto @@ -45,6 +45,6 @@ ] } }, - "folder": "Center Autos", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2.5 Piece W Lime - Source.auto b/src/main/deploy/pathplanner/autos/2.5 Piece W Lime - Source.auto index b3e83fbf..0169d163 100644 --- a/src/main/deploy/pathplanner/autos/2.5 Piece W Lime - Source.auto +++ b/src/main/deploy/pathplanner/autos/2.5 Piece W Lime - Source.auto @@ -106,6 +106,6 @@ ] } }, - "folder": "Source Autos", + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Piece W Limelight - Center.auto b/src/main/deploy/pathplanner/autos/4 Piece W Limelight - Center.auto index c3ea297e..aeafd5f3 100644 --- a/src/main/deploy/pathplanner/autos/4 Piece W Limelight - Center.auto +++ b/src/main/deploy/pathplanner/autos/4 Piece W Limelight - Center.auto @@ -43,15 +43,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "Center note to Shooting With LL" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center note to Shooting With LL" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] } }, { @@ -86,15 +93,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "(C, LL) Amp Side Note to Shooting" + "commands": [ + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "path", + "data": { + "pathName": "(C, LL) Amp Side Note to Shooting" + } + } + ] } }, { @@ -108,15 +122,15 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "(C, LL) Sub to source side note With LL" + "name": "Deploy_Pickup" } }, { - "type": "named", + "type": "path", "data": { - "name": "Deploy_Pickup" + "pathName": "(C, LL) Sub to source side note With LL" } } ] @@ -129,15 +143,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "(C, LL) Source Side Note to Shooting" + "commands": [ + { + "type": "path", + "data": { + "pathName": "(C, LL) Source Side Note to Shooting" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] } }, { @@ -149,6 +170,6 @@ ] } }, - "folder": "Autos I'd like to use", + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Piece, No Limelight - Center.auto b/src/main/deploy/pathplanner/autos/4 Piece, No Limelight - Center.auto index b46c7a9d..2f878b88 100644 --- a/src/main/deploy/pathplanner/autos/4 Piece, No Limelight - Center.auto +++ b/src/main/deploy/pathplanner/autos/4 Piece, No Limelight - Center.auto @@ -24,7 +24,7 @@ { "type": "path", "data": { - "pathName": "(C) Sub to Center Note And Back - No Limelight" + "pathName": "Sub to center note, No LL" } }, { @@ -36,6 +36,12 @@ ] } }, + { + "type": "path", + "data": { + "pathName": "(C, No LL) Center Note to Sub" + } + }, { "type": "named", "data": { @@ -107,6 +113,6 @@ ] } }, - "folder": "Autos I'd like to use", + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Amp - 3 Piece W Lime.auto b/src/main/deploy/pathplanner/autos/Amp - 3 Piece W Lime.auto index 6424ee35..90cdd036 100644 --- a/src/main/deploy/pathplanner/autos/Amp - 3 Piece W Lime.auto +++ b/src/main/deploy/pathplanner/autos/Amp - 3 Piece W Lime.auto @@ -106,6 +106,6 @@ ] } }, - "folder": "Autos I'd like to use", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/N&I 4 Piece, No Limelight - Center.auto b/src/main/deploy/pathplanner/autos/N&I 4 Piece, No Limelight - Center.auto index 2b1d60b9..828a6767 100644 --- a/src/main/deploy/pathplanner/autos/N&I 4 Piece, No Limelight - Center.auto +++ b/src/main/deploy/pathplanner/autos/N&I 4 Piece, No Limelight - Center.auto @@ -101,6 +101,6 @@ ] } }, - "folder": "Autos I'd like to use", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 Piece (Inner Note) - Source.auto b/src/main/deploy/pathplanner/autos/Source - 2 Piece (Inner Note).auto similarity index 96% rename from src/main/deploy/pathplanner/autos/2 Piece (Inner Note) - Source.auto rename to src/main/deploy/pathplanner/autos/Source - 2 Piece (Inner Note).auto index e0e20c8d..1cd54461 100644 --- a/src/main/deploy/pathplanner/autos/2 Piece (Inner Note) - Source.auto +++ b/src/main/deploy/pathplanner/autos/Source - 2 Piece (Inner Note).auto @@ -51,6 +51,6 @@ ] } }, - "folder": "Source Autos", + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Piece W Center Note - Source.auto b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto similarity index 71% rename from src/main/deploy/pathplanner/autos/3 Piece W Center Note - Source.auto rename to src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto index 31a2fda6..d3db1183 100644 --- a/src/main/deploy/pathplanner/autos/3 Piece W Center Note - Source.auto +++ b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto @@ -43,15 +43,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "N&I Lower S Side Note to Shooting" + "commands": [ + { + "type": "path", + "data": { + "pathName": "N&I Lower S Side Note to Shooting" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] } }, { @@ -86,15 +93,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "N&I S - Middle Note to Shoot" + "commands": [ + { + "type": "path", + "data": { + "pathName": "N&I S - Middle Note to Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] } }, { @@ -112,6 +126,6 @@ ] } }, - "folder": null, + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2.5 Piece W Extra Travel- Source.auto b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto similarity index 69% rename from src/main/deploy/pathplanner/autos/2.5 Piece W Extra Travel- Source.auto rename to src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto index 1b67861b..3cbd9fd1 100644 --- a/src/main/deploy/pathplanner/autos/2.5 Piece W Extra Travel- Source.auto +++ b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto @@ -43,15 +43,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "N&I Lower S Side Note to Shooting" + "commands": [ + { + "type": "path", + "data": { + "pathName": "N&I Lower S Side Note to Shooting" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] } }, { @@ -86,15 +93,22 @@ } }, { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "path", + "type": "parallel", "data": { - "pathName": "N&I S - 2nd S Note to Shoot" + "commands": [ + { + "type": "path", + "data": { + "pathName": "N&I S - 2nd S Note to Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] } }, { @@ -106,6 +120,6 @@ ] } }, - "folder": "Source Autos", + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source - Shoot, Ram Notes.auto b/src/main/deploy/pathplanner/autos/Source - Shoot, Ram Notes.auto index d6dae846..1ffd2ce7 100644 --- a/src/main/deploy/pathplanner/autos/Source - Shoot, Ram Notes.auto +++ b/src/main/deploy/pathplanner/autos/Source - Shoot, Ram Notes.auto @@ -32,6 +32,6 @@ ] } }, - "folder": "Source Autos", + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto b/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto new file mode 100644 index 00000000..6221ca2e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto @@ -0,0 +1,179 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.43, + "y": 5.56 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shooting_From_Subwoofer" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sub to Amp Note W LL" + } + }, + { + "type": "named", + "data": { + "name": "Deploy_Pickup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Pickup_Note_With_Limelight" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Auto_Shoot_With_Auto_Align" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Amp Note to Center Note LL" + } + }, + { + "type": "named", + "data": { + "name": "Deploy_Pickup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Pickup_Note_With_Limelight" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Auto_Shoot_With_Auto_Align" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Note to Source Side Note" + } + }, + { + "type": "named", + "data": { + "name": "Deploy_Pickup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Pickup_Note_With_Limelight" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Auto_Shoot_With_Auto_Align" + } + }, + { + "type": "path", + "data": { + "pathName": "Source Note To Mid Note Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Deploy_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Pickup_Note_With_Limelight" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid Note to Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Auto_Shoot_With_Auto_Align" + } + } + ] + } + }, + "folder": "The Cool Useful Ones", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/(C, LL) Amp Side Note to Shooting.path b/src/main/deploy/pathplanner/paths/(C, LL) Amp Side Note to Shooting.path index b79a47e4..5d5ee8a4 100644 --- a/src/main/deploy/pathplanner/paths/(C, LL) Amp Side Note to Shooting.path +++ b/src/main/deploy/pathplanner/paths/(C, LL) Amp Side Note to Shooting.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 3.2, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Middle 4 No LL Piece Auto Paths", + "folder": null, "previewStartingState": { "rotation": 45.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/(C, LL) Source Side Note to Shooting.path b/src/main/deploy/pathplanner/paths/(C, LL) Source Side Note to Shooting.path index 44d670c1..16704744 100644 --- a/src/main/deploy/pathplanner/paths/(C, LL) Source Side Note to Shooting.path +++ b/src/main/deploy/pathplanner/paths/(C, LL) Source Side Note to Shooting.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Middle 4 No LL Piece Auto Paths", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/(C, LL) Sub to Amp Side Note.path b/src/main/deploy/pathplanner/paths/(C, LL) Sub to Amp Side Note.path index f70665d4..8fb44c3f 100644 --- a/src/main/deploy/pathplanner/paths/(C, LL) Sub to Amp Side Note.path +++ b/src/main/deploy/pathplanner/paths/(C, LL) Sub to Amp Side Note.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/(C, LL) Sub to source side note With LL.path b/src/main/deploy/pathplanner/paths/(C, LL) Sub to source side note With LL.path index 32d59d2d..7edef13f 100644 --- a/src/main/deploy/pathplanner/paths/(C, LL) Sub to source side note With LL.path +++ b/src/main/deploy/pathplanner/paths/(C, LL) Sub to source side note With LL.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Center Auto Paths", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Amp Note to Center Note LL.path b/src/main/deploy/pathplanner/paths/Amp Note to Center Note LL.path new file mode 100644 index 00000000..09bf0cb9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp Note to Center Note LL.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8981397630253705, + "y": 6.990576641485824 + }, + "prevControl": null, + "nextControl": { + "x": 2.888390004808514, + "y": 6.766332202498135 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9078895212422267, + "y": 6.366592115607035 + }, + "prevControl": { + "x": 2.888390004808514, + "y": 6.688334136763285 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0, + "rotateFast": false + }, + "reversed": false, + "folder": "5 Piece Paths", + "previewStartingState": { + "rotation": 26.56505117707799, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Note to Source Side Note.path b/src/main/deploy/pathplanner/paths/Center Note to Source Side Note.path new file mode 100644 index 00000000..529013d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Note to Source Side Note.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8981397630253705, + "y": 5.547612425391122 + }, + "prevControl": null, + "nextControl": { + "x": 2.8688904883748028, + "y": 5.264869437102295 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.839641213724234, + "y": 4.787131284476346 + }, + "prevControl": { + "x": 2.859140730157946, + "y": 5.099123547415742 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0, + "rotateFast": true + }, + "reversed": false, + "folder": "5 Piece Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center note to Shooting With LL.path b/src/main/deploy/pathplanner/paths/Center note to Shooting With LL.path index 0b96d532..ae8bde2a 100644 --- a/src/main/deploy/pathplanner/paths/Center note to Shooting With LL.path +++ b/src/main/deploy/pathplanner/paths/Center note to Shooting With LL.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Middle 4 No LL Piece Auto Paths", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Mid Note to Shoot.path b/src/main/deploy/pathplanner/paths/Mid Note to Shoot.path new file mode 100644 index 00000000..fedda606 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mid Note to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.328755089814218, + "y": 4.1046482092964185 + }, + "prevControl": null, + "nextControl": { + "x": 6.17405852388902, + "y": 3.5781612655861896 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 4.274715908576711, + "y": 4.448277321495261 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "5 Piece Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/N&I Lower S Side Note to Shooting.path b/src/main/deploy/pathplanner/paths/N&I Lower S Side Note to Shooting.path index 999d7356..79e304f4 100644 --- a/src/main/deploy/pathplanner/paths/N&I Lower S Side Note to Shooting.path +++ b/src/main/deploy/pathplanner/paths/N&I Lower S Side Note to Shooting.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.4171824469005063, - "y": 2.467466587354827 + "x": 2.8688904883748028, + "y": 2.7884308500208457 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.61028579037215, - "y": 3.1223392361314644 + "x": 2.0889098310263146, + "y": 3.5586617491524772 }, "prevControl": { - "x": 3.230075975821466, - "y": 2.631184749548988 + "x": 2.7087000164756305, + "y": 3.067507262570001 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/N&I S - 2nd S Note to Shoot.path b/src/main/deploy/pathplanner/paths/N&I S - 2nd S Note to Shoot.path index 5f88f508..396e7470 100644 --- a/src/main/deploy/pathplanner/paths/N&I S - 2nd S Note to Shoot.path +++ b/src/main/deploy/pathplanner/paths/N&I S - 2nd S Note to Shoot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.61, - "y": 3.12 + "x": 2.09, + "y": 3.56 }, "prevControl": { - "x": 5.779401644273374, - "y": 1.4617693053049925 + "x": 5.259401644273375, + "y": 1.9017693053049924 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/N&I S - Middle Note to Shoot.path b/src/main/deploy/pathplanner/paths/N&I S - Middle Note to Shoot.path index 35bb9f90..5fc7f0f5 100644 --- a/src/main/deploy/pathplanner/paths/N&I S - Middle Note to Shoot.path +++ b/src/main/deploy/pathplanner/paths/N&I S - Middle Note to Shoot.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.229776385478697, - "y": 4.537331923666697 + "x": 5.0430865707337125, + "y": 4.728632735175209 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.61, - "y": 3.12 + "x": 2.09, + "y": 3.56 }, "prevControl": { - "x": 4.352714802295702, - "y": 2.7481262939733857 + "x": 3.9121146175784047, + "y": 2.105947774840919 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/N&I S - Sub to 2nd S Side Note.path b/src/main/deploy/pathplanner/paths/N&I S - Sub to 2nd S Side Note.path index fba30c12..eeba957c 100644 --- a/src/main/deploy/pathplanner/paths/N&I S - Sub to 2nd S Side Note.path +++ b/src/main/deploy/pathplanner/paths/N&I S - Sub to 2nd S Side Note.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.61, - "y": 3.12 + "x": 2.09, + "y": 3.56 }, "prevControl": null, "nextControl": { - "x": 3.61, - "y": 3.12 + "x": 2.9078895212422267, + "y": 3.2174202115625135 }, "isLocked": false, "linkedName": null @@ -20,21 +20,15 @@ "y": 2.46 }, "prevControl": { - "x": 4.57490373670206, - "y": 0.3859070966005182 + "x": 4.506849868806627, + "y": 1.023724612769891 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.85, - "rotationDegrees": 20.0, - "rotateFast": false - } - ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/N&I S - Sub to Middle Note.path b/src/main/deploy/pathplanner/paths/N&I S - Sub to Middle Note.path index e9196886..fc0ccd08 100644 --- a/src/main/deploy/pathplanner/paths/N&I S - Sub to Middle Note.path +++ b/src/main/deploy/pathplanner/paths/N&I S - Sub to Middle Note.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.61, - "y": 3.12 + "x": 2.09, + "y": 3.56 }, "prevControl": null, "nextControl": { - "x": 4.5865978911445, - "y": 2.7247379850885065 + "x": 3.5611233217715847, + "y": 1.7354569626003862 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.116342363738859 }, "prevControl": { - "x": 4.972504987745019, - "y": 4.701050085860856 + "x": 4.955338746782008, + "y": 4.943127415946043 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S - Sub to 2nd S Side Note.path b/src/main/deploy/pathplanner/paths/S - Sub to 2nd S Side Note.path index 0aeac743..d9632ef2 100644 --- a/src/main/deploy/pathplanner/paths/S - Sub to 2nd S Side Note.path +++ b/src/main/deploy/pathplanner/paths/S - Sub to 2nd S Side Note.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 5.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Source Note To Mid Note Pickup.path b/src/main/deploy/pathplanner/paths/Source Note To Mid Note Pickup.path new file mode 100644 index 00000000..de9a20ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source Note To Mid Note Pickup.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.683645082254536, + "y": 4.065649176428995 + }, + "prevControl": null, + "nextControl": { + "x": 1.477971343959805, + "y": 6.031080821077477 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.373278784562321, + "y": 4.124147725730131 + }, + "prevControl": { + "x": 6.080330106451938, + "y": 3.9816844255657124 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "5 Piece Paths", + "previewStartingState": { + "rotation": -35.21759296819269, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sub to Amp Note W LL.path b/src/main/deploy/pathplanner/paths/Sub to Amp Note W LL.path new file mode 100644 index 00000000..e835098e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sub to Amp Note W LL.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.43, + "y": 5.56 + }, + "prevControl": null, + "nextControl": { + "x": 2.0791600728094584, + "y": 6.142347676619344 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.293654753580292, + "y": 6.3470925991733225 + }, + "prevControl": { + "x": 2.176657654978019, + "y": 6.239845258787905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0, + "rotateFast": false + }, + "reversed": false, + "folder": "5 Piece Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sub to center note LL.path b/src/main/deploy/pathplanner/paths/Sub to center note LL.path index 36a13cbf..43403c80 100644 --- a/src/main/deploy/pathplanner/paths/Sub to center note LL.path +++ b/src/main/deploy/pathplanner/paths/Sub to center note LL.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Middle 4 No LL Piece Auto Paths", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1e64af7..233ce47e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,8 +23,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -92,7 +94,7 @@ public class RobotContainer { // Controller private final CommandXboxController _driveController = new CommandXboxController(0); - + GenericHID _driveRumble = _driveController.getHID(); // Button box // Top half of buttons private final GenericHID _operatorController1 = new GenericHID(1); @@ -148,7 +150,7 @@ public RobotContainer() { new ModuleIOSparkFlex(1), new ModuleIOSparkFlex(2), new ModuleIOSparkFlex(3)); - _intake = new Intake(new RealIntakeIO()); + _intake = new Intake(new RealIntakeIO(), _driveRumble); _shooter = new Shooter(new RealShooterIO()); _leds = new LEDs(); // _climber = new Climber(new RealClimberIO()); @@ -167,7 +169,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); _intake = new Intake(new IntakeIO() { - }); + }, _driveRumble); _shooter = new Shooter(new ShooterIO() { }); // _climber = new Climber(new RealClimberIO() { @@ -192,7 +194,7 @@ public RobotContainer() { new ModuleIO() { }); _intake = new Intake(new IntakeIO() { - }); + }, _driveRumble); _shooter = new Shooter(new ShooterIO() { }); // _climber = new Climber(new RealClimberIO() { @@ -304,19 +306,13 @@ private void configureButtonBindings() { new Rotation2d(Math.PI))), _drive).ignoringDisable(true)); - double inchesFromSubwoofer = 39.0; - double robotWidth = 13.0 + 1.5; - Pose2d robotOnSubwooferRed = new Pose2d( - DriveConstants.RED_SPEAKER.getX() - - Units.inchesToMeters(inchesFromSubwoofer + robotWidth), - DriveConstants.RED_SPEAKER.getY(), - new Rotation2d(Math.PI)); - // Change the robot pose to think it is in front of the red speaker + + // Change the robot pose to think it is centered in front of the speaker of its alliance _driveController.b().onTrue( Commands.runOnce( - () -> _drive.setPose(robotOnSubwooferRed), _drive) + () -> _drive.alignToSubwoofer(), _drive) .ignoringDisable(true)); - + /* * ================================ * Button Box @@ -352,6 +348,9 @@ private void configureButtonBindings() { .andThen(_intake.buildCommand().setPosition(IntakePosition.Stowed))); _op2ButtonFive.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Subwoofer)); + // _op2ButtonFive.onTrue(_shooter.buildCommand().runForZone( + // ShooterZone.Subwoofer + // )); _op2ButtonSeven.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Feeder)); _op2ButtonEight.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Podium)); @@ -422,4 +421,4 @@ public Command getFeederInitialStateCommand() { public Command getRunLEDs() { return this._runLEDsCommand; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 483d5a15..9f4638f5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -25,15 +25,18 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; 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.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.HardwareConstants; import frc.robot.subsystems.vision.VisionIOInputsAutoLogged; +import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; import frc.robot.subsystems.vision.VisionIO; import frc.robot.util.autonomous.AutonomousPathGenerator; @@ -47,6 +50,8 @@ public class Drive extends SubsystemBase { private final Module[] _modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine _sysId; + private boolean hasSeenTag = false; + private final VisionIO _visionIO; private final VisionIOInputsAutoLogged _visionInputs = new VisionIOInputsAutoLogged(); @@ -67,6 +72,21 @@ public class Drive extends SubsystemBase { private Translation2d _centerOfRotation; public Field2d _field; + private double _inchesFromSubwoofer = 39.0; + private double _robotWidth = 13.0 + 1.5; + + private Pose2d _robotOnSubwooferRed = new Pose2d( + DriveConstants.RED_SPEAKER.getX() + - Units.inchesToMeters(_inchesFromSubwoofer + _robotWidth), + DriveConstants.RED_SPEAKER.getY(), + new Rotation2d(Math.PI)); + + private Pose2d _robotOnSubwooferBlue = new Pose2d( + DriveConstants.BLUE_SPEAKER.getX() + + Units.inchesToMeters(_inchesFromSubwoofer + _robotWidth), + DriveConstants.BLUE_SPEAKER.getY(), + new Rotation2d(Math.PI)); + public Drive( GyroIO gyroIO, VisionIO visionIO, @@ -116,6 +136,8 @@ public void periodic() { module.periodic(); } + Logger.recordOutput("Drive/radiustospeaker", getRadiusToSpeakerInMeters()); + // Stop moving when disabled if (DriverStation.isDisabled()) { for (var module : _modules) { @@ -125,7 +147,6 @@ public void periodic() { // Log empty setpoint states when disabled Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - Logger.recordOutput("Drive/radiustospeaker", getRadiusToSpeakerInMeters()); Logger.recordOutput("Drive/Alliance", getAlliance()); } @@ -326,13 +347,33 @@ public Rotation2d getGyroscopeRotation() { } /** - * Adds a vision measurement to the pose estimator. + * Adds a vision measurement to the pose. * * @param visionPose The pose of the robot as measured by the vision camera. * @param timestamp The timestamp of the vision measurement in seconds. */ public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - _poseEstimator.addVisionMeasurement(visionPose, timestamp); + // If we haven't seen a tag then we won't add the Cut-Off Filter so that we can get an initial estimation of pose. + + // We'll first check to see if any of our cameras have seen a tag using a for-loop and if they have then we'll set hasSeenTag to true. + for (int i = 0; i < HardwareConstants.NUMBER_OF_CAMERAS; i++) { + if (_visionInputs._hasPose[i]) { + hasSeenTag = true; + } + } + + // Then if we've seen a tag before we'll add the Cut-Off Filter to the vision measurements. + if (hasSeenTag) { + if (VisionIO.shouldUsePoseEstimation(this.getPose(), visionPose)){ + _poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + else { + System.out.println("[WARNING]: Rejected Vision Estimation. Signifigant Outlier Determined."); + } + } else { + System.out.println("[INFO]: No Tag Seen. Added Initial Vision Esitmation."); + _poseEstimator.addVisionMeasurement(visionPose, timestamp); + } } private Pose2d getSpeakerPos() { @@ -498,4 +539,15 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(-DriveConstants.TRACK_WIDTH_X / 2.0, -DriveConstants.TRACK_WIDTH_Y / 2.0) }; } + + public void alignToSubwoofer() { + if (getAlliance() == Alliance.Red) { + setPose(_robotOnSubwooferRed); + } + + else { + // Alliance is blue. + setPose(_robotOnSubwooferBlue); + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java index ae37970f..ccc83f41 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java @@ -23,7 +23,9 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.HardwareConstants; +import frc.robot.RobotContainer; import frc.robot.util.MotorFrameConfigurator; /** @@ -102,8 +104,9 @@ public ModuleIOSparkFlex(int index) { _turnSparkFlex.setSmartCurrentLimit(30); _driveSparkFlex.enableVoltageCompensation(12.0); _turnSparkFlex.enableVoltageCompensation(12.0); - _driveSparkFlex.setClosedLoopRampRate(0.1); - _turnSparkFlex.setClosedLoopRampRate(0.1); + + _driveSparkFlex.setClosedLoopRampRate(0.05); + _turnSparkFlex.setClosedLoopRampRate(0.05); _driveEncoder.setPosition(0.0); _driveEncoder.setMeasurementPeriod(10); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 65d78b42..3c9e472d 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; // import frc.robot.subsystems.intake.IntakeIOInputsAutoLogged; checkstyle says this is redunant @@ -38,7 +39,7 @@ public double getAngle() { } protected IntakePosition _intakePosition; - private final IntakeCommandFactory _intakeCommandFactory = new IntakeCommandFactory(this); + private final IntakeCommandFactory _intakeCommandFactory; private final IntakeIO _intakeIO; private final IntakeIOInputsAutoLogged _intakeInputs = new IntakeIOInputsAutoLogged(); @@ -46,10 +47,11 @@ public double getAngle() { private boolean _hasNote; private IntakeVisualizer _intakeVisualizer = new IntakeVisualizer(); - public Intake(IntakeIO intakeIO) { + public Intake(IntakeIO intakeIO, GenericHID driveRumble) { + _intakeCommandFactory = new IntakeCommandFactory(this, driveRumble); this._intakeIO = intakeIO; _hasNote = false; - _intakePosition = IntakePosition.Stowed; + _intakePosition = IntakePosition.Stowed; this.setIntakePosition(IntakePosition.Stowed); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeCommandFactory.java b/src/main/java/frc/robot/subsystems/intake/IntakeCommandFactory.java index 1f2a813c..9f02d729 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeCommandFactory.java @@ -1,8 +1,11 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.RobotContainer; import frc.robot.subsystems.intake.Intake.IntakePosition; import frc.robot.subsystems.intake.commands.CmdIntakeWaitForNote; import frc.robot.subsystems.intake.commands.CmdAcquireNoteFor; @@ -10,13 +13,16 @@ public class IntakeCommandFactory { private Intake _intake; + private GenericHID _driveRumble; /** * Use this to access commands to run the intake * @param intake {@link Intake} + * @param driveRumble */ - public IntakeCommandFactory(Intake intake) { + public IntakeCommandFactory(Intake intake, GenericHID driveRumble) { this._intake = intake; + _driveRumble = driveRumble; } @@ -84,7 +90,8 @@ public Command pickUpNoteFrom(IntakePosition position, int timeoutMs) { this.setPosition(position) .andThen(this.acquire()) .andThen(new CmdIntakeWaitForNote(timeoutMs, this._intake)) - .andThen(new CmdAcquireNoteFor(250, _intake, IntakeConstants.INTAKE_ACQUIRE_SPEED.get())) + .andThen(rumbleDriverCommand()) + .alongWith(new CmdAcquireNoteFor(250, _intake, IntakeConstants.INTAKE_ACQUIRE_SPEED.get())) .andThen(this.setPosition(IntakePosition.Stowed)) .andThen(new CmdAcquireNoteFor(350, _intake, IntakeConstants.INTAKE_ACQUIRE_SPEED.get())) .andThen(new CmdIntakeWaitForIntake(_intake)); @@ -111,4 +118,16 @@ public Command pickUpFromGround(int timeoutMs) { public Command acquire(int timeMS) { return new CmdAcquireNoteFor(timeMS, this._intake, IntakeConstants.INTAKE_ACQUIRE_SPEED.get()); } + + public Command rumbleDriverCommand() { + return new RunCommand(() -> rumbleDriverCtrl()).withTimeout(0.25).finallyDo(() -> stopRumbleDriverCtrl()); + } + + public void rumbleDriverCtrl() { + _driveRumble.setRumble(GenericHID.RumbleType.kBothRumble, 1); + } + + public void stopRumbleDriverCtrl() { + _driveRumble.setRumble(GenericHID.RumbleType.kBothRumble, 0); + } } diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index fb4319f0..5ecb37fa 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -36,27 +36,25 @@ public void execute() { if (DriverStation.isDisabled()) { // rainbow when DS is disabled _leds.runAnimation(LEDAnimation.RobotDisabled); - } else if (_leds._isAutoAimEnabled()) { - // the drive wants to use autoaim - if(!_intake.hasNote()){ + } else if(_leds.shouldRunHasNoteAnimation(_intake.hasNote())){ // if we do not have a note, then blink red - _leds.runAnimation(LEDAnimation.BlinkingRed); - }else if(_shooter.shooterInPosition() && _leds._isAutoAimReady()){ + _leds.runAnimation(LEDAnimation.PickedUpNote); + } else if (_leds._isAutoAimEnabled()) { + // the drive wants to use autoaim + if(_shooter.shooterInPosition() && _leds._isAutoAimReady()){ // if the shooter is in position and the autoaim code says its ready // then signal to the drive we are ready - _leds.runAnimation(LEDAnimation.SolidGreen); + _leds.runAnimation(LEDAnimation.ReadyToShoot); } else{ // if we have a note but are not in position then we are not ready. _leds.runAnimation(LEDAnimation.SolidRed); } + } // } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { // // Only run this animation one time, right when the intake first picks up a note // // blinks orange when we pick up a note // _leds.runAnimation(LEDAnimation.PickedUpNote); // } - } else if(_intake.hasNote()){ - _leds.runAnimation(LEDAnimation.SolidYellow); - } else{ // solid orange when we are doing nothing _leds.runAnimation(LEDAnimation.RobotIdle); diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java index 804f9b1a..356f00aa 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.multisubsystemcommands; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.intake.Intake; @@ -27,10 +29,12 @@ public void initialize() { public void execute() { // Only run things if autoshoot is enabled and we have a note if (_shooterSubsystem.isAutoShootEnabled()) { - // update this flag so we can properly disable the flywheels if we turn autoshoot off + // update this flag so we can properly disable the flywheels if we turn + // autoshoot off _prevAutoshootState = true; - // If we have a note, then start running the shooter according to the radius to speaker + // If we have a note, then start running the shooter according to the radius to + // speaker if (_intakeSubsystem.hasNote()) { double radius = _drive.getRadiusToSpeakerInMeters(); _shooterSubsystem.runShooterForRadius(radius); diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index 5e0a441e..5e966f31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -87,8 +87,12 @@ public void setTargetPositionAsDegrees(double degrees) { // _angleMotor.getPIDController().setReference(degrees, ControlType.kPosition); } - public void setAngleMotorSpeed(double speed) { - _angleMotor.set(speed); + public void setAngleMotorVoltage(double voltage) { + _angleMotor.setVoltage(voltage); + } + + public void setAngleMotorSpeed(double speed){ + _angleMotor.set(speed); } private void configAngleMotor() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e2fc4d52..b1356fcd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -11,6 +11,9 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.LoggedTunableNumber; @@ -91,16 +94,15 @@ public double getFlywheelSpeed() { private ShooterZone _currentShooterZone; private ShooterVisualizer _shooterVisualizer = new ShooterVisualizer(); private final ShooterCommandFactory _shooterCommandFactory = new ShooterCommandFactory(this); - //private ProfiledPIDController _anglePID; private PIDController _anglePID; + private DoubleSubscriber _radiusToSpeaker; public Shooter(ShooterIO shooterIO) { _shooterIO = shooterIO; _currentShooterZone = ShooterZone.Unknown; - // 0.017, 0.00008, 0.25 + // 0.01, 0.001, 0.008 - //_anglePID = new ProfiledPIDController(0.0055, 0.001, 0.0015, new Constraints(40, 70)); - _anglePID = new PIDController(0.0065, 0.0015, 0.0015); + _anglePID = new PIDController(0.0525, 0.015, 0.02); _anglePID.setIZone(5); // Set up the zone mappings @@ -112,6 +114,10 @@ public Shooter(ShooterIO shooterIO) { _zoneDataMappings.put(ShooterZone.Unknown, UnknownData); _zoneDataMappings.put(ShooterZone.Amp, AmpData); _zoneDataMappings.put(ShooterZone.Feeder, FeederData); + + NetworkTable adkitNetworkTable = NetworkTableInstance.getDefault().getTable("AdvantageKit"); + _radiusToSpeaker = adkitNetworkTable.getDoubleTopic("RealOutputs/Drive/radiustospeaker") + .subscribe(4.5); } public ShooterCommandFactory buildCommand() { @@ -262,21 +268,26 @@ public void setShooterPositionWithZone(ShooterZone zone) { public void setShooterPositionWithRadius(double radius) { double angle; // only shoot inside this radius - if (radius <= 4.5) { - if(radius < 1){ + if (radius <= 5) { + if(radius < 1.3){ // the subwoofer is about a meter away from the alliance wall // if we think we are less than a meter from the speaker then we // are "inside" the subwoofer which is not possible. so harcode ourselves to one // meter - radius = 1; + radius = 1.3; } - angle = -21.02 * Math.log(0.1106 * radius); - angle -= 2; - if (radius > 2.0 && radius < 2.75) { - angle -= 2.25; - } else if (radius >= 2.75 && radius < 3.75) { - angle -= 1.25; - } + + angle = (101 * (Math.exp(-0.89 * radius))) + 12.37; + + // Previous regression eq + // angle = -21.02 * Math.log(0.1106 * radius); + + // angle -= 2; + // if (radius > 2.0 && radius < 2.75) { + // angle -= 2.25; + // } else if (radius >= 2.75 && radius < 3.75) { + // angle -= 1.25; + // } // else if (radius >= 3.75) { // angle -= 0.; // } @@ -287,8 +298,14 @@ public void setShooterPositionWithRadius(double radius) { } public boolean shooterInPosition() { + // Making a sliding scale kind of function here (it'll probably be linear) + // We don't need to be as specific with our position when we are right against the subwoofer, + // but when we are really far away we have to be pretty exact + // So this function will let us be faster up close, but more accurate further away + double deadband = (ShooterConstants.ANGLE_DEADBAND_SLOPE * _radiusToSpeaker.get()) + ShooterConstants.ANGLE_DEADBAND_INTERCEPT; + return Math.abs(_targetShooterAngle - - getCurrentPositionInDegrees()) <= ShooterConstants.ANGLE_ENCODER_DEADBAND_DEGREES; + - getCurrentPositionInDegrees()) <= deadband; } /** @@ -303,10 +320,8 @@ public void setFlywheelSpeedWithZone(ShooterZone zone) { } public void setFlywheelSpeedWithRadius(double radiusInMeters) { - double speed = 2000; - if (radiusInMeters <= 4 && radiusInMeters > 2) { - speed = 1600; - } else if (radiusInMeters <= 2) { + double speed = 1500; + if (radiusInMeters <= 2) { speed = 1200; } setFlywheelSpeed(speed); @@ -325,7 +340,7 @@ public boolean isReady() { public void runAnglePID() { double output = calcAnglePID(); - _shooterIO.setAngleMotorSpeed(output); + _shooterIO.setAngleMotorVoltage(output); } private double calcAnglePID() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 445b0a5a..d55dbecb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -47,7 +47,7 @@ public abstract class ShooterConstants { public static final LoggedTunableNumber ZONE_SUBWOOFER_SHOOTER_ANGLE = new LoggedTunableNumber("Shooter/SUBWOOFER_SHOOTER_ANGLE", 42.0); // unknown - public static final LoggedTunableNumber ZONE_UNKNOWN_FLYWHEEL_SPEED = new LoggedTunableNumber("Shooter/UNKNOWN_FLYWHEEL_SPEED", 1200); + public static final LoggedTunableNumber ZONE_UNKNOWN_FLYWHEEL_SPEED = new LoggedTunableNumber("Shooter/UNKNOWN_FLYWHEEL_SPEED", 0); //1200 public static final LoggedTunableNumber ZONE_UNKNOWN_LOW_BOUND = new LoggedTunableNumber("Shooter/UNKNOWN_LOW_BOUND", -1); public static final LoggedTunableNumber ZONE_UNKNOWN_UPPER_BOUND = new LoggedTunableNumber("Shooter/UNKNOWN_UPPER_BOUND", -1); public static final LoggedTunableNumber ZONE_UNKNOWN_SHOOTER_ANGLE = new LoggedTunableNumber("Shooter/UNKNOWN_SHOOTER_ANGLE", 35); @@ -74,9 +74,17 @@ public abstract class ShooterConstants { * SHOOTER ENCODER CONSTANTS * **************/ - public static final double MAXIMUM_ANGLE_ENCODER_ANGLE = 90; // These might need changed later depending on how everything is implemented. - public static final double MINIMUM_ANGLE_ENCODER_ANGLE = 10; // These might need changed later depending on how everything is implemented. - public static final double ANGLE_ENCODER_DEADBAND_DEGREES = 0.8; // This will be used when we've determining whether we can shoot or not. + public static final double MAXIMUM_ANGLE_ENCODER_ANGLE = 45; // These might need changed later depending on how everything is implemented. + public static final double MINIMUM_ANGLE_ENCODER_ANGLE = 10; // These might need changed later depending on how everything is implemented. + + public static final double SUBWOOFER_ANGLE_ENCODER_DEADBAND_DEGREES = 1; // This will be used when we've determining whether we can shoot or not. + public static final double SUBWOOFER_SHOT_RADIUS = 1.3; + public static final double CHAINS_ANGLE_ENCODER_DEADBAND_DEGREES = 0.3; + public static final double CHAINS_SHOT_RADIUS = 4.5; + + public static final double ANGLE_DEADBAND_SLOPE = (CHAINS_ANGLE_ENCODER_DEADBAND_DEGREES - SUBWOOFER_ANGLE_ENCODER_DEADBAND_DEGREES) + / (CHAINS_SHOT_RADIUS - SUBWOOFER_SHOT_RADIUS); + public static final double ANGLE_DEADBAND_INTERCEPT = (ANGLE_DEADBAND_SLOPE * -SUBWOOFER_SHOT_RADIUS) + SUBWOOFER_ANGLE_ENCODER_DEADBAND_DEGREES; /*************** * @@ -97,16 +105,16 @@ public abstract class ShooterConstants { public static final double FLYWHEEL_GEAR_RATIO = 2; public static final double ANGLE_GEAR_RATIO = 60; public static final double ANGLE_FROM_ROBOT_ZERO_TO_GROUND_DEGREES = 14.52; - public static final double SHOOTER_MOMENT_ARM_LENGTH_METERS = 0.1219; - public static final double SHOOTER_WEIGHT_KG = 5.222; + public static final double SHOOTER_MOMENT_ARM_LENGTH_METERS = 0.1172; //0.1219 + public static final double SHOOTER_WEIGHT_KG = 5.603; //5.222 public static final double MAX_VOLTAGE = 12.0; public static final double NEO_VORTEX_STALL_CURRENT = 211; // in amps public static final double NEO_VORTEX_STALL_TORQUE = 0.3671; // in kg*m public static final double NEO_VORTEX_INTERNAL_RESISTANCE = MAX_VOLTAGE / NEO_VORTEX_STALL_CURRENT; public static final double NEO_VORTEX_TORQUE_CONSTANT = NEO_VORTEX_STALL_TORQUE / NEO_VORTEX_STALL_CURRENT; public static final double SHOOTER_FEEDFORWARD_CONSTANT = - (SHOOTER_WEIGHT_KG * SHOOTER_MOMENT_ARM_LENGTH_METERS * NEO_VORTEX_INTERNAL_RESISTANCE) - / (NEO_VORTEX_TORQUE_CONSTANT * ANGLE_GEAR_RATIO * MAX_VOLTAGE); + ((SHOOTER_WEIGHT_KG * SHOOTER_MOMENT_ARM_LENGTH_METERS * NEO_VORTEX_INTERNAL_RESISTANCE) + / (NEO_VORTEX_TORQUE_CONSTANT * ANGLE_GEAR_RATIO)) + 0.07; /****************** @@ -122,7 +130,6 @@ public abstract class ShooterConstants { Logger.recordOutput("Constants/Shooter/MAXIMUM_ANGLE_ENCODER_ANGLE", MAXIMUM_ANGLE_ENCODER_ANGLE); Logger.recordOutput("Constants/Shooter/MINIMUM_ANGLE_ENCODER_ANGLE", MINIMUM_ANGLE_ENCODER_ANGLE); - Logger.recordOutput("Constants/Shooter/ANGLE_ENCODER_ANGLE_DEADBAND_DEGREES", ANGLE_ENCODER_DEADBAND_DEGREES); Logger.recordOutput("Constants/Shooter/FLYWHEEL_GEAR_RATIO", FLYWHEEL_GEAR_RATIO); Logger.recordOutput("Constants/Shooter/SHOOTER_CANVAS_WIDTH", SHOOTER_CANVAS_WIDTH); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 58fb25d9..ed858881 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -50,6 +50,8 @@ public default void setRightFlywheelSpeedRPM(double velocityRotationsPerMinute) public default void setTargetPositionAsDegrees(double degrees){} public default void setAngleMotorSpeed(double speed){} + + public default void setAngleMotorVoltage(double voltage){} public default void setFeederMotorSpeed(double speed){} } diff --git a/src/main/java/frc/robot/subsystems/vision/RealVisionIO.java b/src/main/java/frc/robot/subsystems/vision/RealVisionIO.java index 01418c5a..c33ea942 100644 --- a/src/main/java/frc/robot/subsystems/vision/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/RealVisionIO.java @@ -4,12 +4,14 @@ import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonUtils; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import frc.robot.HardwareConstants; public class RealVisionIO implements VisionIO { @@ -77,4 +79,4 @@ private static EstimatedRobotPose getPoseWithAmbiguityCutoff(EstimatedRobotPose return pose; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 6601f6b0..62610af8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.vision; import org.littletonrobotics.junction.AutoLog; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonUtils; import edu.wpi.first.math.geometry.Pose2d; import frc.robot.HardwareConstants; @@ -17,4 +19,17 @@ public static class VisionIOInputs { } public default void updateInputs(VisionIOInputs inputs) {} + + /** + * Boolean whether we should use the estimated pose given by the camera through filtering out the poses that are more than + * 2.0m away from the current pose that we're in which is physically impossible for our robot to do. + * @param estimatedRobotPose Estimated Pose from Vision + * @param currentPose Current Pose of the Robot + * @return Whether we should use this estimation or not. + */ + public static boolean shouldUsePoseEstimation(Pose2d estimatedRobotPose, Pose2d currentPose){ + final double MAXIMUM_ALLOWED_DISTANCE_BETWEEN_POSES = 2.0; + + return !(Math.abs(PhotonUtils.getDistanceToPose(currentPose, estimatedRobotPose)) > MAXIMUM_ALLOWED_DISTANCE_BETWEEN_POSES); + } } diff --git a/src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java b/src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java index c39c7384..90b2b7e0 100644 --- a/src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java +++ b/src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java @@ -29,7 +29,7 @@ public VisionDrive(VisionDriveIO visionDriveIO) { // TODO: What do I do with these??? :( _translatePID = new PIDController(0.2, 0, 0); - _rotatePID = new PIDController(0.075, 0, 0); + _rotatePID = new PIDController(0.1, 0, 0); } @Override