From 24df58935365d9b446b80ae9025008221c068404 Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Mon, 1 Apr 2024 17:49:07 -0400 Subject: [PATCH 01/22] New feedforward constants --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 445b0a5..f04f86c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -97,8 +97,8 @@ 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 From 85f46ba2ab6186979a9073f4da12fefbab12a3bc Mon Sep 17 00:00:00 2001 From: CONTROLS LAPTOP2 Date: Mon, 1 Apr 2024 20:29:48 -0400 Subject: [PATCH 02/22] All kinds of stuff --- src/main/.pathplanner/settings.json | 11 +- .../autos/1 Piece Exit Wing - Amp.auto | 2 +- .../autos/1 Piece, Exit Wing - Source.auto | 2 +- .../autos/2 Piece (Inner Note) - Source.auto | 2 +- .../2 Piece (Outer Note) No LL - Source.auto | 2 +- .../2 Piece (Outer Note), No Lime - Amp.auto | 2 +- .../2 Piece - Center - No Limelight.auto | 2 +- .../pathplanner/autos/2.5 Piece - Amp.auto | 2 +- .../2.5 Piece W Extra Travel- Source.auto | 2 +- .../autos/2.5 Piece W Lime - Source.auto | 2 +- .../autos/4 Piece W Limelight - Center.auto | 79 +++++++---- .../autos/4 Piece, No Limelight - Center.auto | 2 +- .../autos/Amp - 3 Piece No LL.auto | 2 +- .../autos/Amp - 3 Piece W Lime.auto | 2 +- .../N&I 4 Piece, No Limelight - Center.auto | 2 +- .../autos/Source - Shoot, Ram Notes.auto | 2 +- .../autos/The Almighty 5 Piece.auto | 131 ++++++++++++++++++ .../(C, LL) Amp Side Note to Shooting.path | 6 +- .../(C, LL) Source Side Note to Shooting.path | 6 +- .../paths/(C, LL) Sub to Amp Side Note.path | 4 +- ..., LL) Sub to source side note With LL.path | 6 +- .../paths/Amp Note to Center Note LL.path | 52 +++++++ .../Center note to Shooting With LL.path | 6 +- .../paths/Sub to Amp Note W LL.path | 52 +++++++ .../paths/Sub to center note LL.path | 6 +- 25 files changed, 322 insertions(+), 65 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto create mode 100644 src/main/deploy/pathplanner/paths/Amp Note to Center Note LL.path create mode 100644 src/main/deploy/pathplanner/paths/Sub to Amp Note W LL.path diff --git a/src/main/.pathplanner/settings.json b/src/main/.pathplanner/settings.json index 7a35407..6d437f9 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/1 Piece Exit Wing - Amp.auto b/src/main/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto index 6b6a5b1..52bc3db 100644 --- a/src/main/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto +++ b/src/main/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto @@ -26,6 +26,6 @@ ] } }, - "folder": null, + "folder": "ignore for now", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto b/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto index 0b025a2..b7dd7f5 100644 --- a/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto +++ b/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto @@ -26,6 +26,6 @@ ] } }, - "folder": null, + "folder": "ignore for now", "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/2 Piece (Inner Note) - Source.auto index e0e20c8..d67b87a 100644 --- a/src/main/deploy/pathplanner/autos/2 Piece (Inner Note) - Source.auto +++ b/src/main/deploy/pathplanner/autos/2 Piece (Inner Note) - Source.auto @@ -51,6 +51,6 @@ ] } }, - "folder": "Source Autos", + "folder": null, "choreoAuto": false } \ No newline at end of file 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 ca68e25..6f615ca 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 5e52d8f..c8946fa 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 e4bae74..996e606 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 - Amp.auto b/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto index 9c91717..8463d9a 100644 --- a/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto +++ b/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto @@ -69,6 +69,6 @@ ] } }, - "folder": null, + "folder": "ignore for now", "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/2.5 Piece W Extra Travel- Source.auto index 1b67861..c480c81 100644 --- a/src/main/deploy/pathplanner/autos/2.5 Piece W Extra Travel- Source.auto +++ b/src/main/deploy/pathplanner/autos/2.5 Piece W Extra Travel- Source.auto @@ -106,6 +106,6 @@ ] } }, - "folder": "Source 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 b3e83fb..8906228 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": null, "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 c3ea297..34ebe1a 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": null, "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 b46c7a9..34d6732 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 @@ -107,6 +107,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/Amp - 3 Piece No LL.auto b/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto index 5a9a4de..2b4edf2 100644 --- a/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto +++ b/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto @@ -82,6 +82,6 @@ ] } }, - "folder": null, + "folder": "ignore for now", "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 6424ee3..a3531f0 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": "ignore for now", "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 2b1d60b..fdfdde7 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": "ignore for now", "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 d6dae84..88bca25 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": "Source Side Autos", "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 0000000..c036012 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto @@ -0,0 +1,131 @@ +{ + "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": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Shooting_From_Podium" + } + } + ] + } + }, + { + "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": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Shooting_From_Podium" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Auto_Shoot_With_Auto_Align" + } + }, + { + "type": "named", + "data": { + "name": "Pickup_Note_With_Limelight" + } + }, + { + "type": "named", + "data": { + "name": "Auto_Shoot_With_Auto_Align" + } + } + ] + } + }, + "folder": null, + "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 b79a47e..5d5ee8a 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 44d670c..1670474 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 f70665d..8fb44c3 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 32d59d2..7edef13 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 0000000..09bf0cb --- /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 Shooting With LL.path b/src/main/deploy/pathplanner/paths/Center note to Shooting With LL.path index 0b96d53..ae8bde2 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/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 0000000..e835098 --- /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 36a13cb..43403c8 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 From e7309eb3687909a87a71b5a2e6de550aec0c6f0a Mon Sep 17 00:00:00 2001 From: area5188 Date: Tue, 2 Apr 2024 09:02:03 -0400 Subject: [PATCH 03/22] changes --- .../java/frc/robot/subsystems/drive/Drive.java | 4 +++- .../robot/subsystems/drive/ModuleIOSparkFlex.java | 7 +++++-- .../frc/robot/subsystems/vision/RealVisionIO.java | 4 +++- .../frc/robot/subsystems/vision/VisionIO.java | 15 +++++++++++++++ 4 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 483d5a1..889faf5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -332,7 +332,9 @@ public Rotation2d getGyroscopeRotation() { * @param timestamp The timestamp of the vision measurement in seconds. */ public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - _poseEstimator.addVisionMeasurement(visionPose, timestamp); + if (VisionIO.shouldUsePoseEstimation(visionPose, this.getPose())){ + _poseEstimator.addVisionMeasurement(visionPose, timestamp); + } } private Pose2d getSpeakerPos() { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java index ae37970..ccc83f4 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/vision/RealVisionIO.java b/src/main/java/frc/robot/subsystems/vision/RealVisionIO.java index 01418c5..c33ea94 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 6601f6b..62610af 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); + } } From ce7bb0b27c4c9425e00319d7eed9bd8f3353717c Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Tue, 2 Apr 2024 10:40:52 -0400 Subject: [PATCH 04/22] PID Tuning --- .../frc/robot/subsystems/shooter/RealShooterIO.java | 4 ++++ .../java/frc/robot/subsystems/shooter/Shooter.java | 11 +++++------ .../robot/subsystems/shooter/ShooterConstants.java | 6 +++--- .../java/frc/robot/subsystems/shooter/ShooterIO.java | 2 ++ 4 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index 5e0a441..345b388 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -91,6 +91,10 @@ public void setAngleMotorSpeed(double speed) { _angleMotor.set(speed); } + public void setAngleMotorVoltage(double voltage) { + _angleMotor.setVoltage(voltage); + } + private void configAngleMotor() { _angleMotor = new CANSparkFlex(HardwareConstants.CanIds.ANGLE_MOTOR_ID, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e2fc4d5..8b65e6d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -97,11 +97,9 @@ public double getFlywheelSpeed() { public Shooter(ShooterIO shooterIO) { _shooterIO = shooterIO; _currentShooterZone = ShooterZone.Unknown; - // 0.017, 0.00008, 0.25 - - //_anglePID = new ProfiledPIDController(0.0055, 0.001, 0.0015, new Constraints(40, 70)); - _anglePID = new PIDController(0.0065, 0.0015, 0.0015); - _anglePID.setIZone(5); + + _anglePID = new PIDController(0.0525, 0.015, 0.02); //0.0065, 0.0015, 0.0015 + _anglePID.setIZone(8); // Set up the zone mappings // This is a funky hack that lets us modify zone data during a match, like angle, @@ -325,7 +323,8 @@ public boolean isReady() { public void runAnglePID() { double output = calcAnglePID(); - _shooterIO.setAngleMotorSpeed(output); + // _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 f04f86c..94f9f66 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); @@ -105,8 +105,8 @@ public abstract class ShooterConstants { 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; /****************** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 58fb25d..ed85888 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){} } From ad89e5077990e05426fe73e98bdb2e8187836afe Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:09:43 -0400 Subject: [PATCH 05/22] Added alliance color logic to resetting robot pose to subwoofer. --- src/main/java/frc/robot/RobotContainer.java | 41 ++++++++++++++++----- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1e64af..b87845b 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; @@ -306,16 +308,35 @@ private void configureButtonBindings() { 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 - _driveController.b().onTrue( - Commands.runOnce( - () -> _drive.setPose(robotOnSubwooferRed), _drive) - .ignoringDisable(true)); + + if (DriverStation.getAlliance().get() == Alliance.Red) { + 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 + _driveController.b().onTrue( + Commands.runOnce( + () -> _drive.setPose(robotOnSubwooferRed), _drive) + .ignoringDisable(true)); + } + + else { + Pose2d robotOnSubwooferBlue = new Pose2d( + DriveConstants.BLUE_SPEAKER.getX() + + Units.inchesToMeters(inchesFromSubwoofer + robotWidth), + DriveConstants.BLUE_SPEAKER.getY(), + new Rotation2d(Math.PI)); + + // Change the robot pose to think it is in front of the blue speaker + _driveController.b().onTrue( + Commands.runOnce( + () -> _drive.setPose(robotOnSubwooferBlue), _drive) + .ignoringDisable(true)); + } + /* * ================================ From 0e764e8b97bab60cac3e816118140df3863bfd3b Mon Sep 17 00:00:00 2001 From: Mackenzie Racoop Date: Tue, 2 Apr 2024 20:10:55 -0400 Subject: [PATCH 06/22] added LED hasNote changes --- .../frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 fb4319f..b146380 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -47,7 +47,7 @@ public void execute() { _leds.runAnimation(LEDAnimation.SolidGreen); } else{ // if we have a note but are not in position then we are not ready. - _leds.runAnimation(LEDAnimation.SolidRed); + _leds.runAnimation(LEDAnimation.PickedUpNote); } // } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { // // Only run this animation one time, right when the intake first picks up a note @@ -55,7 +55,7 @@ public void execute() { // _leds.runAnimation(LEDAnimation.PickedUpNote); // } } else if(_intake.hasNote()){ - _leds.runAnimation(LEDAnimation.SolidYellow); + _leds.runAnimation(LEDAnimation.PickedUpNote); } else{ // solid orange when we are doing nothing From 80fb11f83f60e623c375c4b088c2c9992353a299 Mon Sep 17 00:00:00 2001 From: Mackenzie Racoop Date: Tue, 2 Apr 2024 20:19:49 -0400 Subject: [PATCH 07/22] added controller rumble when hasnote is true --- src/main/java/frc/robot/RobotContainer.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b87845b..204d88e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,6 +94,7 @@ public class RobotContainer { // Controller private final CommandXboxController _driveController = new CommandXboxController(0); + GenericHID _driveRumble = _driveController.getHID(); // Button box // Top half of buttons @@ -415,6 +416,18 @@ private void configureButtonBindings() { // _controller.povDown().whileTrue(_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } + // RUMBLE + // rumbles the controller when intake has a note + + public void rumbleDriverController() { + if (_intake.hasNote()) { + _driveRumble.setRumble(GenericHID.RumbleType.kLeftRumble, 1); + } else { + _driveRumble.setRumble(GenericHID.RumbleType.kLeftRumble, 0); + } + + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * From 8cffe8c523122d06d8efb46a910e9fc375b1b8bb Mon Sep 17 00:00:00 2001 From: area5188 Date: Tue, 2 Apr 2024 21:15:36 -0400 Subject: [PATCH 08/22] ending of meeting from tuesday --- src/main/java/frc/robot/RobotContainer.java | 23 +++++++++++-------- .../frc/robot/subsystems/drive/Drive.java | 3 ++- .../subsystems/shooter/RealShooterIO.java | 6 ++--- .../frc/robot/subsystems/shooter/Shooter.java | 21 +++++++++-------- .../subsystems/shooter/ShooterConstants.java | 2 +- .../robot/subsystems/shooter/ShooterIO.java | 2 +- 6 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1e64af..0ea31d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -304,8 +304,8 @@ private void configureButtonBindings() { new Rotation2d(Math.PI))), _drive).ignoringDisable(true)); - double inchesFromSubwoofer = 39.0; - double robotWidth = 13.0 + 1.5; + double inchesFromSubwoofer = 36.0; + double robotWidth = 32 / 2; // we actuwlly want half the width Pose2d robotOnSubwooferRed = new Pose2d( DriveConstants.RED_SPEAKER.getX() - Units.inchesToMeters(inchesFromSubwoofer + robotWidth), @@ -329,8 +329,8 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(() -> _shooter.setAutoShootEnabled(false))); // Adjust shooter angle from current position - _opButtonOne.onTrue(this._shooter.buildCommand().adjustAngle(1)); - _opButtonFour.onTrue(this._shooter.buildCommand().adjustAngle(-1)); + _opButtonOne.onTrue(this._shooter.buildCommand().adjustAngle(0.5)); + _opButtonFour.onTrue(this._shooter.buildCommand().adjustAngle(-0.5)); // Move intake to different positions _opButtonNine.onTrue(_intake.buildCommand().spit(IntakeConstants.INTAKE_SPIT_TIME.get())); @@ -350,13 +350,16 @@ private void configureButtonBindings() { .andThen(new CmdIntakeWaitForIntake(_intake)) .andThen(_intake.buildCommand().spit(IntakeConstants.INTAKE_SPIT_TIME.get())) .andThen(_intake.buildCommand().setPosition(IntakePosition.Stowed))); - _op2ButtonFive.onTrue(new GrpShootNoteInZone(_intake, _shooter, - ShooterZone.Subwoofer)); + // _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)); - // _op2ButtonEight.onTrue(_shooter.buildCommand().runForZone( - // ShooterZone.Podium)); + // _op2ButtonEight.onTrue(new GrpShootNoteInZone(_intake, _shooter, + // ShooterZone.Podium)); + _op2ButtonEight.onTrue(_shooter.buildCommand().runForZone( + ShooterZone.Podium)); // sad face button _op2ButtonThree.onTrue(new InstantCommand( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 889faf5..6a0189b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -116,6 +116,8 @@ public void periodic() { module.periodic(); } + Logger.recordOutput("Drive/radiustospeaker", getRadiusToSpeakerInMeters()); + // Stop moving when disabled if (DriverStation.isDisabled()) { for (var module : _modules) { @@ -125,7 +127,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()); } diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index 5e0a441..de74d70 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -87,14 +87,14 @@ 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); } private void configAngleMotor() { _angleMotor = new CANSparkFlex(HardwareConstants.CanIds.ANGLE_MOTOR_ID, MotorType.kBrushless); - _angleMotor.enableVoltageCompensation(12.0); + _angleMotor.enableVoltageCompensation(8.0); _angleMotor.setInverted(false); _angleMotor.setCANTimeout(100); _angleMotor.setIdleMode(IdleMode.kBrake); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e2fc4d5..265d230 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -97,10 +97,10 @@ public double getFlywheelSpeed() { public Shooter(ShooterIO shooterIO) { _shooterIO = shooterIO; _currentShooterZone = ShooterZone.Unknown; - // 0.017, 0.00008, 0.25 + // 0.0065, 0.0015, 0.0015 //_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,0,0); _anglePID.setIZone(5); // Set up the zone mappings @@ -262,20 +262,21 @@ 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; + //angle = -21.02 * Math.log(0.1106 * radius); + angle = (101 * (Math.exp(-0.89 * radius))) + 12.37; + //angle -= 2; if (radius > 2.0 && radius < 2.75) { - angle -= 2.25; + //angle -= 2.25; } else if (radius >= 2.75 && radius < 3.75) { - angle -= 1.25; + //angle -= 1.25; } // else if (radius >= 3.75) { // angle -= 0.; @@ -325,7 +326,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 f04f86c..0461d4a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -106,7 +106,7 @@ public abstract class ShooterConstants { 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); + / (NEO_VORTEX_TORQUE_CONSTANT * ANGLE_GEAR_RATIO); /****************** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 58fb25d..967336a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -49,7 +49,7 @@ public default void setRightFlywheelSpeedRPM(double velocityRotationsPerMinute) public default void setTargetPositionAsDegrees(double degrees){} - public default void setAngleMotorSpeed(double speed){} + public default void setAngleMotorVoltage(double speed){} public default void setFeederMotorSpeed(double speed){} } From 48cbec6a79d1eeec48812da656c3f6eff2131096 Mon Sep 17 00:00:00 2001 From: area5188 Date: Tue, 2 Apr 2024 21:17:39 -0400 Subject: [PATCH 09/22] actual end of tuesday work --- src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index de74d70..43947be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -94,7 +94,7 @@ public void setAngleMotorVoltage(double voltage) { private void configAngleMotor() { _angleMotor = new CANSparkFlex(HardwareConstants.CanIds.ANGLE_MOTOR_ID, MotorType.kBrushless); - _angleMotor.enableVoltageCompensation(8.0); + _angleMotor.enableVoltageCompensation(6.0); _angleMotor.setInverted(false); _angleMotor.setCANTimeout(100); _angleMotor.setIdleMode(IdleMode.kBrake); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 0461d4a..f04f86c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -106,7 +106,7 @@ public abstract class ShooterConstants { 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); + / (NEO_VORTEX_TORQUE_CONSTANT * ANGLE_GEAR_RATIO * MAX_VOLTAGE); /****************** From f0c95a5f373e59e0e7e809e9c8c5680c10a93da4 Mon Sep 17 00:00:00 2001 From: area5188 Date: Tue, 2 Apr 2024 21:28:24 -0400 Subject: [PATCH 10/22] THIS IS BROKEN CODE --- .../java/frc/robot/subsystems/shooter/RealShooterIO.java | 6 +++--- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index c7e186d..ad2928f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -88,11 +88,11 @@ public void setTargetPositionAsDegrees(double degrees) { } public void setAngleMotorVoltage(double voltage) { - _angleMotor.setVoltage(voltage); + _angleMotor.setVoltage(voltage); } - 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 1cd6987..eef8a2a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -326,7 +326,7 @@ public boolean isReady() { public void runAnglePID() { double output = calcAnglePID(); - _shooterIO.setAngleMotorVoltage(output); + _shooterIO.setAngleMotorSpeed(output); } private double calcAnglePID() { From a0a1708df99aea4bc830fe8205c9829f4d91fd17 Mon Sep 17 00:00:00 2001 From: Mitchell Date: Wed, 3 Apr 2024 06:38:04 -0400 Subject: [PATCH 11/22] Cut-Off Filter for Pose Estimation --- .../frc/robot/subsystems/drive/Drive.java | 25 +++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 889faf5..59402d0 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -34,6 +34,7 @@ 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 +48,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(); @@ -326,13 +329,31 @@ 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) { - if (VisionIO.shouldUsePoseEstimation(visionPose, this.getPose())){ + // 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); } } From 74b2e8ccb23f2824087202ce4f9fbad6d8aef59d Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Wed, 3 Apr 2024 09:34:18 -0400 Subject: [PATCH 12/22] Put feedforward back to percent speed output --- .../frc/robot/subsystems/shooter/Shooter.java | 19 +++++++++++-------- .../subsystems/shooter/ShooterConstants.java | 2 +- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index eef8a2a..cb24f33 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -99,7 +99,6 @@ public Shooter(ShooterIO shooterIO) { _currentShooterZone = ShooterZone.Unknown; // 0.0065, 0.0015, 0.0015 - // _anglePID = new ProfiledPIDController(0.0055, 0.001, 0.0015, new Constraints(40, 70)); _anglePID = new PIDController(0.0065,0.0015,0.0015); _anglePID.setIZone(5); @@ -270,14 +269,18 @@ public void setShooterPositionWithRadius(double radius) { // meter radius = 1.3; } - //angle = -21.02 * Math.log(0.1106 * radius); + angle = (101 * (Math.exp(-0.89 * radius))) + 12.37; - //angle -= 2; - if (radius > 2.0 && radius < 2.75) { - //angle -= 2.25; - } else if (radius >= 2.75 && radius < 3.75) { - //angle -= 1.25; - } + + // 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.; // } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 94f9f66..28e0403 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -106,7 +106,7 @@ public abstract class ShooterConstants { 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)) + 0.07; + / (NEO_VORTEX_TORQUE_CONSTANT * ANGLE_GEAR_RATIO * MAX_VOLTAGE)); /****************** From adb6311724862090dfaef5bc17bcb07845af8cdd Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Wed, 3 Apr 2024 10:19:38 -0400 Subject: [PATCH 13/22] Added equation for changing angle tolerance on shooter by distance from speaker --- .../CmdAdjustShooterAutomatically.java | 8 ++++++-- .../frc/robot/subsystems/shooter/Shooter.java | 17 +++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 15 +++++++++++---- 3 files changed, 32 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/CmdAdjustShooterAutomatically.java index 804f9b1..356f00a 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/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index cb24f33..b93a65a 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,8 +94,8 @@ 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; @@ -111,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() { @@ -291,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; } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 28e0403..443cfd7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -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; /*************** * @@ -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); From c4a9e0aa55c269f778d6976fe85291df64c32f5b Mon Sep 17 00:00:00 2001 From: first5188 Date: Wed, 3 Apr 2024 17:38:02 -0400 Subject: [PATCH 14/22] auto stuff --- .pathplanner/settings.json | 10 +-- .../autos/1 Piece Exit Wing - Amp.auto | 2 +- .../autos/1 Piece, Exit Wing - Source.auto | 2 +- .../pathplanner/autos/2.5 Piece - Amp.auto | 2 +- .../autos/2.5 Piece W Lime - Source.auto | 2 +- .../autos/4 Piece W Limelight - Center.auto | 2 +- .../autos/4 Piece, No Limelight - Center.auto | 2 +- .../autos/Amp - 3 Piece No LL.auto | 2 +- .../autos/Amp - 3 Piece W Lime.auto | 2 +- .../N&I 4 Piece, No Limelight - Center.auto | 2 +- ...uto => Source - 2 Piece (Inner Note).auto} | 2 +- ...to => Source - 3 Piece W Center Note.auto} | 52 +++++++---- ...uto => Source - 3 Piece W Lower Note.auto} | 52 +++++++---- .../autos/Source - Shoot, Ram Notes.auto | 2 +- .../autos/The Almighty 5 Piece.auto | 90 ++++++++++++++----- .../Center Note to Source Side Note.path | 52 +++++++++++ .../pathplanner/paths/Mid Note to Shoot.path | 52 +++++++++++ .../N&I Lower S Side Note to Shooting.path | 12 +-- .../paths/N&I S - 2nd S Note to Shoot.path | 8 +- .../paths/N&I S - Middle Note to Shoot.path | 12 +-- .../paths/N&I S - Sub to 2nd S Side Note.path | 20 ++--- .../paths/N&I S - Sub to Middle Note.path | 12 +-- .../paths/S - Sub to 2nd S Side Note.path | 4 +- .../paths/Source Note To Mid Note Pickup.path | 52 +++++++++++ 24 files changed, 338 insertions(+), 112 deletions(-) rename src/main/deploy/pathplanner/autos/{2 Piece (Inner Note) - Source.auto => Source - 2 Piece (Inner Note).auto} (96%) rename src/main/deploy/pathplanner/autos/{3 Piece W Center Note - Source.auto => Source - 3 Piece W Center Note.auto} (67%) rename src/main/deploy/pathplanner/autos/{2.5 Piece W Extra Travel- Source.auto => Source - 3 Piece W Lower Note.auto} (66%) create mode 100644 src/main/deploy/pathplanner/paths/Center Note to Source Side Note.path create mode 100644 src/main/deploy/pathplanner/paths/Mid Note to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/Source Note To Mid Note Pickup.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 43de025..2766236 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/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto b/src/main/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto index 52bc3db..6b6a5b1 100644 --- a/src/main/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto +++ b/src/main/deploy/pathplanner/autos/1 Piece Exit Wing - Amp.auto @@ -26,6 +26,6 @@ ] } }, - "folder": "ignore for now", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto b/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto index b7dd7f5..0b025a2 100644 --- a/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto +++ b/src/main/deploy/pathplanner/autos/1 Piece, Exit Wing - Source.auto @@ -26,6 +26,6 @@ ] } }, - "folder": "ignore for now", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto b/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto index 8463d9a..9c91717 100644 --- a/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto +++ b/src/main/deploy/pathplanner/autos/2.5 Piece - Amp.auto @@ -69,6 +69,6 @@ ] } }, - "folder": "ignore for now", + "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 8906228..0169d16 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": null, + "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 34ebe1a..aeafd5f 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 @@ -170,6 +170,6 @@ ] } }, - "folder": null, + "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 34d6732..3a07595 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 @@ -107,6 +107,6 @@ ] } }, - "folder": null, + "folder": "The Cool Useful Ones", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto b/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto index 2b4edf2..5a9a4de 100644 --- a/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto +++ b/src/main/deploy/pathplanner/autos/Amp - 3 Piece No LL.auto @@ -82,6 +82,6 @@ ] } }, - "folder": "ignore for now", + "folder": null, "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 a3531f0..90cdd03 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": "ignore for now", + "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 fdfdde7..828a676 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": "ignore for now", + "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 d67b87a..1cd5446 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": null, + "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 67% 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 31a2fda..57638cb 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,21 +43,28 @@ } }, { - "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" + } + } + ] } }, { "type": "named", "data": { - "name": "Auto_Shoot_With_Auto_Align" + "name": "Shooting_From_Podium" } }, { @@ -86,21 +93,28 @@ } }, { - "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" + } + } + ] } }, { "type": "named", "data": { - "name": "Auto_Shoot_With_Auto_Align" + "name": "Shooting_From_Podium" } }, { @@ -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 66% 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 c480c81..c8811a2 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,21 +43,28 @@ } }, { - "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" + } + } + ] } }, { "type": "named", "data": { - "name": "Auto_Shoot_With_Auto_Align" + "name": "Shooting_From_Podium" } }, { @@ -86,26 +93,33 @@ } }, { - "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" + } + } + ] } }, { "type": "named", "data": { - "name": "Auto_Shoot_With_Auto_Align" + "name": "Shooting_From_Podium" } } ] } }, - "folder": null, + "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 88bca25..1ffd2ce 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 Side 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 index c036012..82f8905 100644 --- a/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto +++ b/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto @@ -43,22 +43,15 @@ } }, { - "type": "parallel", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Stow_Pickup" - } - }, - { - "type": "named", - "data": { - "name": "Shooting_From_Podium" - } - } - ] + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Shooting_From_Podium" } }, { @@ -86,20 +79,32 @@ "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": "named", + "type": "path", "data": { - "name": "Stow_Pickup" + "pathName": "Center Note to Source Side Note" } }, { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Deploy_Pickup" } } ] @@ -108,7 +113,31 @@ { "type": "named", "data": { - "name": "Auto_Shoot_With_Auto_Align" + "name": "Pickup_Note_With_Limelight" + } + }, + { + "type": "named", + "data": { + "name": "Stow_Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Shooting_From_Podium" + } + }, + { + "type": "path", + "data": { + "pathName": "Source Note To Mid Note Pickup" + } + }, + { + "type": "named", + "data": { + "name": "Deploy_Pickup" } }, { @@ -117,15 +146,34 @@ "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" + "name": "Shooting_From_Podium" } } ] } }, - "folder": null, + "folder": "The Cool Useful Ones", "choreoAuto": 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 0000000..529013d --- /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/Mid Note to Shoot.path b/src/main/deploy/pathplanner/paths/Mid Note to Shoot.path new file mode 100644 index 0000000..fedda60 --- /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 999d735..79e304f 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 5f88f50..396e747 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 35bb9f9..5fc7f0f 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 fba30c1..eeba957 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 e919688..fc0ccd0 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 0aeac74..d9632ef 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 0000000..4a619f8 --- /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.201681833292409, + "y": 6.015600819800215 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.373278784562321, + "y": 4.124147725730131 + }, + "prevControl": { + "x": 6.661546434731825, + "y": 4.494638537970663 + }, + "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 From 9ae577e8b443ca81d63b29f7cf8d535242d052e1 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 3 Apr 2024 18:41:59 -0400 Subject: [PATCH 15/22] updated autos --- .../pathplanner/paths/Source Note To Mid Note Pickup.path | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/leds/commands/CmdLEDsRunLEDs.java | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) 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 index 4a619f8..de9a20e 100644 --- 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 @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.201681833292409, - "y": 6.015600819800215 + "x": 1.477971343959805, + "y": 6.031080821077477 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.124147725730131 }, "prevControl": { - "x": 6.661546434731825, - "y": 4.494638537970663 + "x": 6.080330106451938, + "y": 3.9816844255657124 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbe7cda..8c74049 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -310,7 +310,7 @@ private void configureButtonBindings() { double inchesFromSubwoofer = 39.0; double robotWidth = 13.0 + 1.5; - if (DriverStation.getAlliance().get() == Alliance.Red) { + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { Pose2d robotOnSubwooferRed = new Pose2d( DriveConstants.RED_SPEAKER.getX() - Units.inchesToMeters(inchesFromSubwoofer + robotWidth), 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 b146380..40c1fec 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -47,7 +47,7 @@ public void execute() { _leds.runAnimation(LEDAnimation.SolidGreen); } else{ // if we have a note but are not in position then we are not ready. - _leds.runAnimation(LEDAnimation.PickedUpNote); + _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 From f1c0d34a5dfca0546f787b94e83a6cf824491768 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Wed, 3 Apr 2024 19:12:56 -0400 Subject: [PATCH 16/22] B button centers based on alliance. Now updates every time button is pressed. --- src/main/java/frc/robot/RobotContainer.java | 73 ++++--------------- .../frc/robot/subsystems/drive/Drive.java | 28 +++++++ 2 files changed, 44 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fbe7cda..f157f1a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,7 +94,6 @@ public class RobotContainer { // Controller private final CommandXboxController _driveController = new CommandXboxController(0); - GenericHID _driveRumble = _driveController.getHID(); // Button box // Top half of buttons @@ -307,38 +306,13 @@ private void configureButtonBindings() { new Rotation2d(Math.PI))), _drive).ignoringDisable(true)); - double inchesFromSubwoofer = 39.0; - double robotWidth = 13.0 + 1.5; - - if (DriverStation.getAlliance().get() == Alliance.Red) { - 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 - _driveController.b().onTrue( - Commands.runOnce( - () -> _drive.setPose(robotOnSubwooferRed), _drive) - .ignoringDisable(true)); - } - - else { - Pose2d robotOnSubwooferBlue = new Pose2d( - DriveConstants.BLUE_SPEAKER.getX() - + Units.inchesToMeters(inchesFromSubwoofer + robotWidth), - DriveConstants.BLUE_SPEAKER.getY(), - new Rotation2d(Math.PI)); - - // Change the robot pose to think it is in front of the blue speaker - _driveController.b().onTrue( - Commands.runOnce( - () -> _drive.setPose(robotOnSubwooferBlue), _drive) - .ignoringDisable(true)); - } - - + + // Change the robot pose to think it is centered in front of the speaker of its alliance + _driveController.b().onTrue( + Commands.runOnce( + () -> _drive.alignToSubwoofer(), _drive) + .ignoringDisable(true)); + /* * ================================ * Button Box @@ -351,8 +325,8 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(() -> _shooter.setAutoShootEnabled(false))); // Adjust shooter angle from current position - _opButtonOne.onTrue(this._shooter.buildCommand().adjustAngle(0.5)); - _opButtonFour.onTrue(this._shooter.buildCommand().adjustAngle(-0.5)); + _opButtonOne.onTrue(this._shooter.buildCommand().adjustAngle(1)); + _opButtonFour.onTrue(this._shooter.buildCommand().adjustAngle(-1)); // Move intake to different positions _opButtonNine.onTrue(_intake.buildCommand().spit(IntakeConstants.INTAKE_SPIT_TIME.get())); @@ -372,16 +346,13 @@ private void configureButtonBindings() { .andThen(new CmdIntakeWaitForIntake(_intake)) .andThen(_intake.buildCommand().spit(IntakeConstants.INTAKE_SPIT_TIME.get())) .andThen(_intake.buildCommand().setPosition(IntakePosition.Stowed))); - // _op2ButtonFive.onTrue(new GrpShootNoteInZone(_intake, _shooter, - // ShooterZone.Subwoofer)); - _op2ButtonFive.onTrue(_shooter.buildCommand().runForZone( - ShooterZone.Subwoofer - )); + _op2ButtonFive.onTrue(new GrpShootNoteInZone(_intake, _shooter, + ShooterZone.Subwoofer)); _op2ButtonSeven.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Feeder)); - // _op2ButtonEight.onTrue(new GrpShootNoteInZone(_intake, _shooter, - // ShooterZone.Podium)); - _op2ButtonEight.onTrue(_shooter.buildCommand().runForZone( - ShooterZone.Podium)); + _op2ButtonEight.onTrue(new GrpShootNoteInZone(_intake, _shooter, + ShooterZone.Podium)); + // _op2ButtonEight.onTrue(_shooter.buildCommand().runForZone( + // ShooterZone.Podium)); // sad face button _op2ButtonThree.onTrue(new InstantCommand( @@ -419,18 +390,6 @@ private void configureButtonBindings() { // _controller.povDown().whileTrue(_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } - // RUMBLE - // rumbles the controller when intake has a note - - public void rumbleDriverController() { - if (_intake.hasNote()) { - _driveRumble.setRumble(GenericHID.RumbleType.kLeftRumble, 1); - } else { - _driveRumble.setRumble(GenericHID.RumbleType.kLeftRumble, 0); - } - - } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -459,4 +418,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 3f45b7a..9f4638f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -25,11 +25,13 @@ 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; @@ -70,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, @@ -522,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); + } + } } From 9cd62b4f52bb452f867fb93e9d48cb593db3fbf9 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 3 Apr 2024 20:07:36 -0400 Subject: [PATCH 17/22] commit hehe --- .../autos/4 Piece, No Limelight - Center.auto | 8 +++++++- .../autos/Source - 3 Piece W Center Note.auto | 4 ++-- .../autos/Source - 3 Piece W Lower Note.auto | 4 ++-- .../autos/The Almighty 5 Piece.auto | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++--------- .../subsystems/shooter/RealShooterIO.java | 4 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 4 ++-- .../subsystems/shooter/ShooterConstants.java | 2 +- 8 files changed, 28 insertions(+), 22 deletions(-) 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 3a07595..2f878b8 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": { diff --git a/src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto index 57638cb..d3db118 100644 --- a/src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto +++ b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Center Note.auto @@ -64,7 +64,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } }, { @@ -114,7 +114,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } }, { diff --git a/src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto index c8811a2..3cbd9fd 100644 --- a/src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto +++ b/src/main/deploy/pathplanner/autos/Source - 3 Piece W Lower Note.auto @@ -64,7 +64,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } }, { @@ -114,7 +114,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } } ] diff --git a/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto b/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto index 82f8905..6221ca2 100644 --- a/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto +++ b/src/main/deploy/pathplanner/autos/The Almighty 5 Piece.auto @@ -51,7 +51,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } }, { @@ -125,7 +125,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } }, { @@ -168,7 +168,7 @@ { "type": "named", "data": { - "name": "Shooting_From_Podium" + "name": "Auto_Shoot_With_Auto_Align" } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c74049..9e165a9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -372,16 +372,16 @@ private void configureButtonBindings() { .andThen(new CmdIntakeWaitForIntake(_intake)) .andThen(_intake.buildCommand().spit(IntakeConstants.INTAKE_SPIT_TIME.get())) .andThen(_intake.buildCommand().setPosition(IntakePosition.Stowed))); - // _op2ButtonFive.onTrue(new GrpShootNoteInZone(_intake, _shooter, - // ShooterZone.Subwoofer)); - _op2ButtonFive.onTrue(_shooter.buildCommand().runForZone( - ShooterZone.Subwoofer - )); + _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)); - _op2ButtonEight.onTrue(_shooter.buildCommand().runForZone( - ShooterZone.Podium)); + _op2ButtonEight.onTrue(new GrpShootNoteInZone(_intake, _shooter, + ShooterZone.Podium)); + // _op2ButtonEight.onTrue(_shooter.buildCommand().runForZone( + // ShooterZone.Podium)); // sad face button _op2ButtonThree.onTrue(new InstantCommand( diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index ad2928f..9981a37 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -92,13 +92,13 @@ public void setAngleMotorVoltage(double voltage) { } public void setAngleMotorSpeed(double speed){ - _angleMotor.set(speed); + _angleMotor.setVoltage(speed); } private void configAngleMotor() { _angleMotor = new CANSparkFlex(HardwareConstants.CanIds.ANGLE_MOTOR_ID, MotorType.kBrushless); - _angleMotor.enableVoltageCompensation(6.0); + _angleMotor.enableVoltageCompensation(12.0); _angleMotor.setInverted(false); _angleMotor.setCANTimeout(100); _angleMotor.setIdleMode(IdleMode.kBrake); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b93a65a..60997f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -100,9 +100,9 @@ public double getFlywheelSpeed() { public Shooter(ShooterIO shooterIO) { _shooterIO = shooterIO; _currentShooterZone = ShooterZone.Unknown; - // 0.0065, 0.0015, 0.0015 + // 0.01, 0.001, 0.008 - _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 diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 443cfd7..d55dbec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -114,7 +114,7 @@ public abstract class ShooterConstants { 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)); + / (NEO_VORTEX_TORQUE_CONSTANT * ANGLE_GEAR_RATIO)) + 0.07; /****************** From 8ffca67b2fd61430169e84aa01beef61ea2a04a6 Mon Sep 17 00:00:00 2001 From: collinkn Date: Wed, 3 Apr 2024 20:13:15 -0400 Subject: [PATCH 18/22] merging states monday branch --- src/main/java/frc/robot/RobotContainer.java | 8 +++---- .../frc/robot/subsystems/intake/Intake.java | 8 ++++--- .../intake/IntakeCommandFactory.java | 23 +++++++++++++++++-- 3 files changed, 30 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f157f1a..880e586 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,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); @@ -150,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()); @@ -169,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() { @@ -194,7 +194,7 @@ public RobotContainer() { new ModuleIO() { }); _intake = new Intake(new IntakeIO() { - }); + }, _driveRumble); _shooter = new Shooter(new ShooterIO() { }); // _climber = new Climber(new RealClimberIO() { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 65d78b4..3c9e472 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 1f2a813..9f02d72 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); + } } From 8bf5698f92ac453b95e46608cfc54b42fc78004b Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Thu, 4 Apr 2024 14:41:15 -0400 Subject: [PATCH 19/22] Adjusted flywheel equation to better reflect speeds used when making regression --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 60997f9..8040469 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -320,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); From 591ae49dce8647d72694b2ebd53e9efd3a6a7b9e Mon Sep 17 00:00:00 2001 From: rhit-edgingza Date: Thu, 4 Apr 2024 14:42:21 -0400 Subject: [PATCH 20/22] Fixed shooter motor voltage call --- src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java | 2 +- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java index 9981a37..5e966f3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/RealShooterIO.java @@ -92,7 +92,7 @@ public void setAngleMotorVoltage(double voltage) { } public void setAngleMotorSpeed(double speed){ - _angleMotor.setVoltage(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 8040469..b1356fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -340,7 +340,7 @@ public boolean isReady() { public void runAnglePID() { double output = calcAnglePID(); - _shooterIO.setAngleMotorSpeed(output); + _shooterIO.setAngleMotorVoltage(output); } private double calcAnglePID() { From c8a8060f273c00040f0a0cf951c31807d2fc9de5 Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Thu, 4 Apr 2024 17:56:09 -0400 Subject: [PATCH 21/22] up turning PID P --- src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java b/src/main/java/frc/robot/subsystems/visiondrive/VisionDrive.java index c39c738..90b2b7e 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 From 5d16176d71bdebc9b9a72dc2558dc1b4f078b208 Mon Sep 17 00:00:00 2001 From: LunaMoon9860 Date: Thu, 4 Apr 2024 19:38:55 -0400 Subject: [PATCH 22/22] modified led logic --- .../subsystems/leds/commands/CmdLEDsRunLEDs.java | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) 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 40c1fec..5ecb37f 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.PickedUpNote); - } else{ // solid orange when we are doing nothing _leds.runAnimation(LEDAnimation.RobotIdle);