From 3795d57df199673f9b6cd1eee5aef76960c30f65 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Sat, 30 Mar 2024 15:59:09 -0700 Subject: [PATCH 01/59] paths and arm gains --- .pathplanner/settings.json | 3 +- .../autos/0N-S1-Leave (underhand).auto | 6 +-- .../autos/0N-S1-noLeave (underhand).auto | 6 +-- .../pathplanner/autos/0N-S1-noLeave.auto | 4 +- .../autos/0N-S2-noLeave (underhand).auto | 2 +- .../autos/0N-S3-Leave (underhand).auto | 4 +- .../deploy/pathplanner/autos/0N-S3-Leave.auto | 2 +- .../autos/0N-S3-noLeave (underhand).auto | 4 +- .../pathplanner/autos/0N-S3-noLeave.auto | 4 +- .../deploy/pathplanner/autos/1N-S1-A1.auto | 6 +-- .../deploy/pathplanner/autos/1N-S3-C4.auto | 4 +- .../deploy/pathplanner/autos/1N-S3-C5.auto | 4 +- .../pathplanner/autos/1N-Speaker2-A2.auto | 2 +- .../deploy/pathplanner/autos/2N-S1-A1.auto | 6 +-- .../deploy/pathplanner/autos/2N-S1-C1.auto | 6 +-- .../deploy/pathplanner/autos/2N-S1-C2.auto | 4 +- .../deploy/pathplanner/autos/2N-S2-A1.auto | 2 +- .../deploy/pathplanner/autos/2N-S2-A3.auto | 2 +- .../deploy/pathplanner/autos/2N-S2-C1.auto | 2 +- .../deploy/pathplanner/autos/2N-S2-C2.auto | 2 +- .../deploy/pathplanner/autos/2N-S3-A3.auto | 4 +- .../deploy/pathplanner/autos/2N-S3-C4.auto | 4 +- .../deploy/pathplanner/autos/2N-S3-C5.auto | 4 +- .../deploy/pathplanner/autos/3N-S1-C1.auto | 6 +-- .../deploy/pathplanner/autos/3N-S2-C2.auto | 2 +- .../deploy/pathplanner/autos/Hajel all 3.auto | 4 +- .../pathplanner/autos/Hajel center.auto | 29 +++++++++-- .../pathplanner/autos/Hajel middle 1.auto | 4 +- .../autos/Hajel middle bottom 2.auto | 4 +- .../pathplanner/autos/Hajel middle top 2.auto | 4 +- src/main/deploy/pathplanner/paths/A1-C1.path | 2 +- src/main/deploy/pathplanner/paths/A1-C2.path | 2 +- src/main/deploy/pathplanner/paths/A1-C3.path | 10 ++-- src/main/deploy/pathplanner/paths/A1-S1.path | 12 ++--- src/main/deploy/pathplanner/paths/A1-S2.path | 8 +-- .../deploy/pathplanner/paths/A1-Speaker1.path | 52 ------------------- .../deploy/pathplanner/paths/A1-Speaker2.path | 52 ------------------- src/main/deploy/pathplanner/paths/A2-C3.path | 14 ++--- src/main/deploy/pathplanner/paths/A2-S1.path | 10 ++-- src/main/deploy/pathplanner/paths/A2-S2.path | 4 +- .../pathplanner/paths/A2-Speaker2(Amp).path | 52 ------------------- src/main/deploy/pathplanner/paths/A3-C5.path | 10 ++-- src/main/deploy/pathplanner/paths/A3-S2.path | 6 +-- src/main/deploy/pathplanner/paths/A3-S3.path | 14 ++--- src/main/deploy/pathplanner/paths/C1-S2.path | 2 +- src/main/deploy/pathplanner/paths/C2-A1.path | 2 +- src/main/deploy/pathplanner/paths/C4-S3.path | 12 ++--- src/main/deploy/pathplanner/paths/C5-S3.path | 22 ++++---- src/main/deploy/pathplanner/paths/S1-A1.path | 10 ++-- src/main/deploy/pathplanner/paths/S1-A2.path | 10 ++-- src/main/deploy/pathplanner/paths/S1-C1.path | 10 ++-- .../deploy/pathplanner/paths/S1-Leave.path | 2 +- src/main/deploy/pathplanner/paths/S2-A1.path | 4 +- src/main/deploy/pathplanner/paths/S2-A2.path | 4 +- src/main/deploy/pathplanner/paths/S2-A3.path | 4 +- src/main/deploy/pathplanner/paths/S2-C1.path | 4 +- src/main/deploy/pathplanner/paths/S3-A3.path | 12 ++--- src/main/deploy/pathplanner/paths/S3-C4.path | 12 ++--- src/main/deploy/pathplanner/paths/S3-C5.path | 22 ++++---- .../deploy/pathplanner/paths/S3-Leave.path | 12 ++--- .../paths/{A2-Speaker2.path => center.path} | 22 ++++---- .../{A3-Speaker3.path => go to center.path} | 26 +++++----- .../java/frc/robot/GameRobotContainer.java | 2 + .../commands/Arm/ScoreInTrapStutter.java | 2 +- .../robot/commands/Arm/SpinArmCommand.java | 42 +++++++++++++++ .../ClimbParts/ClimbAndShoot.java | 2 +- .../ClimberCommands/ClimbParts/PrepClimb.java | 2 +- .../ClimberCommands/ClimbParts/UnClimb.java | 5 +- ...ClimbPartTwoThatWillBringDownTheMotor.java | 4 +- .../frc/robot/subsystems/ArmSubsystem.java | 36 ++++++++----- .../subsystems/LimeVision/LimeLightSub.java | 2 +- 71 files changed, 298 insertions(+), 379 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/A1-Speaker1.path delete mode 100644 src/main/deploy/pathplanner/paths/A1-Speaker2.path delete mode 100644 src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path rename src/main/deploy/pathplanner/paths/{A2-Speaker2.path => center.path} (69%) rename src/main/deploy/pathplanner/paths/{A3-Speaker3.path => go to center.path} (69%) create mode 100644 src/main/java/frc/robot/commands/Arm/SpinArmCommand.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 75c81966..911efa2a 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -8,12 +8,13 @@ "Starting from Mid Field Notes" ], "autoFolders": [ + "0.", "1. A", "1. C", "2. A+A", "2. A+C", "3. A+A+C", - "0." + "Hajel" ], "defaultMaxVel": 1.5, "defaultMaxAccel": 3.0, diff --git a/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto index 7fd82e82..ef587e51 100644 --- a/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, - "rotation": -121.0 + "rotation": -120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto index 142e3255..44803b72 100644 --- a/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, - "rotation": -121.0 + "rotation": -120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto b/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto index a06ca138..fdd5e5e2 100644 --- a/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto +++ b/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7518994781213513, - "y": 6.686887667641241 + "x": 0.72, + "y": 6.67 }, "rotation": 60.0 }, diff --git a/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto index 67effdcf..4ba02f01 100644 --- a/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.35, + "x": 1.33, "y": 5.54 }, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto index a1518376..38adcdcf 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": 121.0 + "rotation": 120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto b/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto index e9383068..f5b048a9 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, "rotation": -60.0 diff --git a/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto index 8b0fe6a2..88dbd6a0 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": 121.0 + "rotation": 120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto b/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto index 248e2c44..ec3d7fd4 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7167669684840067, - "y": 4.391563704668065 + "x": 0.72, + "y": 4.44 }, "rotation": -60.0 }, diff --git a/src/main/deploy/pathplanner/autos/1N-S1-A1.auto b/src/main/deploy/pathplanner/autos/1N-S1-A1.auto index c29ce8ae..44767520 100644 --- a/src/main/deploy/pathplanner/autos/1N-S1-A1.auto +++ b/src/main/deploy/pathplanner/autos/1N-S1-A1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/1N-S3-C4.auto b/src/main/deploy/pathplanner/autos/1N-S3-C4.auto index 67840707..56cf944b 100644 --- a/src/main/deploy/pathplanner/autos/1N-S3-C4.auto +++ b/src/main/deploy/pathplanner/autos/1N-S3-C4.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/1N-S3-C5.auto b/src/main/deploy/pathplanner/autos/1N-S3-C5.auto index 6e804570..e65a98ea 100644 --- a/src/main/deploy/pathplanner/autos/1N-S3-C5.auto +++ b/src/main/deploy/pathplanner/autos/1N-S3-C5.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto b/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto index e3a73dc0..855d5698 100644 --- a/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto +++ b/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3535296301361848, + "x": 1.33, "y": 5.545221859674489 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto b/src/main/deploy/pathplanner/autos/2N-S1-A1.auto index bdafd5ce..9c541187 100644 --- a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S1-A1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/2N-S1-C1.auto b/src/main/deploy/pathplanner/autos/2N-S1-C1.auto index ea7ad11f..ce9e55e4 100644 --- a/src/main/deploy/pathplanner/autos/2N-S1-C1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S1-C1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/2N-S1-C2.auto b/src/main/deploy/pathplanner/autos/2N-S1-C2.auto index 64e5e9a0..0b6acb16 100644 --- a/src/main/deploy/pathplanner/autos/2N-S1-C2.auto +++ b/src/main/deploy/pathplanner/autos/2N-S1-C2.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, "rotation": 59.0 }, diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto index 388c10e0..ace2e107 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto b/src/main/deploy/pathplanner/autos/2N-S2-A3.auto index 79962fd4..3f12da71 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-A3.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S2-C1.auto b/src/main/deploy/pathplanner/autos/2N-S2-C1.auto index 5e319608..b51c94dc 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-C1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-C1.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S2-C2.auto b/src/main/deploy/pathplanner/autos/2N-S2-C2.auto index fa410321..8986e511 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-C2.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-C2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S3-A3.auto b/src/main/deploy/pathplanner/autos/2N-S3-A3.auto index 01056d2a..1bf46ea7 100644 --- a/src/main/deploy/pathplanner/autos/2N-S3-A3.auto +++ b/src/main/deploy/pathplanner/autos/2N-S3-A3.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/2N-S3-C4.auto b/src/main/deploy/pathplanner/autos/2N-S3-C4.auto index abcf083d..4f76687e 100644 --- a/src/main/deploy/pathplanner/autos/2N-S3-C4.auto +++ b/src/main/deploy/pathplanner/autos/2N-S3-C4.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/2N-S3-C5.auto b/src/main/deploy/pathplanner/autos/2N-S3-C5.auto index fd3436b5..fa4fdf33 100644 --- a/src/main/deploy/pathplanner/autos/2N-S3-C5.auto +++ b/src/main/deploy/pathplanner/autos/2N-S3-C5.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/3N-S1-C1.auto b/src/main/deploy/pathplanner/autos/3N-S1-C1.auto index f3af664f..a04f29cb 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-C1.auto +++ b/src/main/deploy/pathplanner/autos/3N-S1-C1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/3N-S2-C2.auto b/src/main/deploy/pathplanner/autos/3N-S2-C2.auto index c161a855..7e6e3c32 100644 --- a/src/main/deploy/pathplanner/autos/3N-S2-C2.auto +++ b/src/main/deploy/pathplanner/autos/3N-S2-C2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/autos/Hajel all 3.auto b/src/main/deploy/pathplanner/autos/Hajel all 3.auto index eb381c6d..43dc08f3 100644 --- a/src/main/deploy/pathplanner/autos/Hajel all 3.auto +++ b/src/main/deploy/pathplanner/autos/Hajel all 3.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.35, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -152,6 +152,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel center.auto b/src/main/deploy/pathplanner/autos/Hajel center.auto index d6a84759..0032aecf 100644 --- a/src/main/deploy/pathplanner/autos/Hajel center.auto +++ b/src/main/deploy/pathplanner/autos/Hajel center.auto @@ -2,17 +2,36 @@ "version": 1.0, "startingPose": { "position": { - "x": 2.0400914981573175, - "y": 5.632912378520905 + "x": 0.72, + "y": 4.44 }, - "rotation": 0 + "rotation": -60.0 }, "command": { "type": "sequential", "data": { - "commands": [] + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker (S3)" + } + }, + { + "type": "path", + "data": { + "pathName": "go to center" + } + }, + { + "type": "path", + "data": { + "pathName": "center" + } + } + ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle 1.auto b/src/main/deploy/pathplanner/autos/Hajel middle 1.auto index 04f3d20b..8e28c562 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle 1.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle 1.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -58,6 +58,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto b/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto index 6cc51b2d..92d684ec 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -89,6 +89,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto b/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto index aa4d5093..d0758a7a 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -89,6 +89,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index 10a2c01c..ca7af168 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index 4a34aadb..417095c4 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 87fb46f6..0a98cf02 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.671662009990687, - "y": 4.118900460023142 + "x": 8.063050741486098, + "y": 4.499325921728767 }, "prevControl": { - "x": 6.2771340927297254, - "y": 7.380473222484906 + "x": 6.668522824225136, + "y": 7.760898684190531 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index a84dfc92..8c91f4c6 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": { - "x": 0.7679149380520984, - "y": 6.690000000006141 + "x": 0.7379149380520984, + "y": 6.670000000006141 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 60.0, "rotateFast": false }, "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index e4161c81..a84a6bc6 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -12,15 +12,15 @@ "y": 6.75 }, "isLocked": false, - "linkedName": "A1" + "linkedName": null }, { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": { - "x": 1.2699999999999998, + "x": 1.2299999999999998, "y": 5.54412553269551 }, "nextControl": null, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-Speaker1.path b/src/main/deploy/pathplanner/paths/A1-Speaker1.path deleted file mode 100644 index db281f42..00000000 --- a/src/main/deploy/pathplanner/paths/A1-Speaker1.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.5362823244730466, - "y": 6.75 - }, - "prevControl": null, - "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 - }, - "isLocked": false, - "linkedName": "A1" - }, - { - "anchor": { - "x": 0.7713800042984719, - "y": 6.640452511674254 - }, - "prevControl": { - "x": 0.6713800042984719, - "y": 6.640452511674254 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Speaker 1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 60.75, - "rotateFast": false - }, - "reversed": false, - "folder": "Notes Near Starting Zone", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-Speaker2.path b/src/main/deploy/pathplanner/paths/A1-Speaker2.path deleted file mode 100644 index b6eed265..00000000 --- a/src/main/deploy/pathplanner/paths/A1-Speaker2.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.5362823244730466, - "y": 6.75 - }, - "prevControl": null, - "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 - }, - "isLocked": false, - "linkedName": "A1" - }, - { - "anchor": { - "x": 1.3535296301361848, - "y": 5.545221859674489 - }, - "prevControl": { - "x": 1.253529630136185, - "y": 5.545221859674489 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Speaker 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Notes Near Starting Zone", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 08842494..734c97c4 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.950422248707747, - "y": 8.23929007943486 + "x": 6.277134092729725, + "y": 7.271936851566012 }, "isLocked": false, "linkedName": "A2" }, { "anchor": { - "x": 7.671662009990687, - "y": 4.118900460023142 + "x": 8.063050741486098, + "y": 4.499325921728767 }, "prevControl": { - "x": 7.571662009990687, - "y": 4.118900460023142 + "x": 7.244094488188977, + "y": 5.4564193743772105 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -60.101098161385586, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index 3c47c9d1..ba09cef2 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": { - "x": 0.6499999999999999, - "y": 6.69 + "x": 0.6199999999999999, + "y": 6.67 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 60.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index 8355e8a0..e223f74a 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": { - "x": 1.27, + "x": 1.23, "y": 5.54412553269551 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path b/src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path deleted file mode 100644 index b34d46d1..00000000 --- a/src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.64, - "y": 6.11 - }, - "prevControl": null, - "nextControl": { - "x": 2.6586524036008736, - "y": 6.011754960227449 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3535296301361848, - "y": 5.545221859674489 - }, - "prevControl": { - "x": 1.2535296301361847, - "y": 5.545221859674489 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Speaker 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "Notes Near Starting Zone", - "previewStartingState": { - "rotation": -63.75999999999999, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index ace413ba..53a8ccf8 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.872088097956508, - "y": 0.9251560871167382 + "x": 7.826244114026691, + "y": 0.779488482053888 }, "prevControl": { - "x": 7.772088097956508, - "y": 0.9251560871167378 + "x": 7.726244114026691, + "y": 0.7794884820538875 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index eb4b1619..7c414a79 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": { - "x": 1.2699999999999998, + "x": 1.2299999999999998, "y": 5.54412553269551 }, "nextControl": null, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index 298ee4e1..d4888b68 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 1.25, - "y": 4.118900460023142 + "x": 0.72, + "y": 4.44 }, "prevControl": { - "x": 1.1499999999999997, - "y": 4.118900460023142 + "x": 0.6199999999999997, + "y": 4.44 }, "nextControl": null, "isLocked": false, - "linkedName": "S3" + "linkedName": null } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -55.0, + "rotation": -60.0, "rotateFast": false }, "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index eb86f035..b754e91f 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 2" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 7b8c98aa..72cbddd4 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting from Mid Field Notes", "previewStartingState": { - "rotation": 0, + "rotation": -18.565352613122048, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index 235d9441..dbdc1533 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": { - "x": 2.7228859892812496, - "y": 2.6633911139970734 + "x": 2.6396529577652563, + "y": 2.663266849133159 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.0, + "rotation": -60.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index f77b35a3..8d8e5eb6 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.872088097956508, - "y": 0.9251560871167382 + "x": 7.826244114026691, + "y": 0.779488482053888 }, "prevControl": null, "nextControl": { - "x": 7.782137282098908, - "y": 0.9688464833904297 + "x": 7.736293298169091, + "y": 0.8231788783275794 }, "isLocked": false, "linkedName": "C5" }, { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.72, + "y": 4.44 }, "prevControl": { - "x": 2.1958983447210816, - "y": 2.7336561332717624 + "x": 2.1158983447210815, + "y": 2.733531868407848 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": "S3" } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.0, + "rotation": -60.0, "rotateFast": false }, "reversed": false, "folder": "Starting from Mid Field Notes", "previewStartingState": { - "rotation": -20.0, + "rotation": -2.726310993906272, "velocity": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index a1288074..cf9935a9 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 0.8500000000000002, - "y": 6.69 + "x": 0.8200000000000002, + "y": 6.67 }, "isLocked": false, "linkedName": "S1" @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": 59.0, + "rotation": 60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index 87c26895..0ab17111 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 0.8500000000000001, - "y": 6.69 + "x": 0.8200000000000001, + "y": 6.67 }, "isLocked": false, "linkedName": "S1" @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index 813f1686..3bed5a03 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 0.8500000000000001, - "y": 6.69 + "x": 0.8200000000000001, + "y": 6.67 }, "isLocked": false, "linkedName": "S1" @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index 75969f47..ea4c7bae 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -12,7 +12,7 @@ "y": 7.187749496967226 }, "isLocked": false, - "linkedName": "Speaker 1" + "linkedName": null }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index 3d620310..1d95cffc 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index 53b97235..d1af5fc8 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index ba30eeb5..85966e85 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index 3896dbbd..3572d908 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index 71ae9221..0b63b706 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 1.4535688381373444, - "y": 4.731177964495728 + "x": 1.370335806621351, + "y": 4.731053699631814 }, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index 786025a6..0a89bc1b 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.72, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 2.378724925254083, - "y": 2.365588982247864 + "x": 2.298724925254083, + "y": 2.36546471738395 }, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index 76f149a8..29864531 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 2.535512604548745, - "y": 2.241800998348939 + "x": 2.452279573032752, + "y": 2.2416767334850247 }, "isLocked": true, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { - "x": 7.872088097956508, - "y": 0.9251560871167382 + "x": 7.826244114026691, + "y": 0.779488482053888 }, "prevControl": { - "x": 7.300080185895035, - "y": 1.10373384732504 + "x": 7.254236201965218, + "y": 0.9580662422621897 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -20.0, + "rotation": -2.3532968661083107, "rotateFast": false }, "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index c76a8461..a509e863 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 0.7284778050297884, - "y": 0.9719994332998654 + "x": 0.6452447735137951, + "y": 0.971875168435951 }, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A2-Speaker2.path b/src/main/deploy/pathplanner/paths/center.path similarity index 69% rename from src/main/deploy/pathplanner/paths/A2-Speaker2.path rename to src/main/deploy/pathplanner/paths/center.path index 340ac675..8985c86a 100644 --- a/src/main/deploy/pathplanner/paths/A2-Speaker2.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.45, - "y": 5.55 + "x": 8.340521559787769, + "y": 1.428722058585343 }, "prevControl": null, "nextControl": { - "x": 2.5500000000000003, - "y": 5.55 + "x": 8.340521559787769, + "y": 2.4287220585853433 }, "isLocked": false, - "linkedName": "A2" + "linkedName": null }, { "anchor": { - "x": 1.3535296301361848, - "y": 5.545221859674489 + "x": 8.340521559787769, + "y": 7.412959533479694 }, "prevControl": { - "x": 1.2535296301361847, - "y": 5.545221859674489 + "x": 8.340521559787769, + "y": 6.412959533479694 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 2" + "linkedName": null } ], "rotationTargets": [], @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Notes Near Starting Zone", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/A3-Speaker3.path b/src/main/deploy/pathplanner/paths/go to center.path similarity index 69% rename from src/main/deploy/pathplanner/paths/A3-Speaker3.path rename to src/main/deploy/pathplanner/paths/go to center.path index 01c43233..54460539 100644 --- a/src/main/deploy/pathplanner/paths/A3-Speaker3.path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.5, - "y": 4.3 + "x": 0.7167669684840067, + "y": 4.414985377759629 }, "prevControl": null, "nextControl": { - "x": 2.6, - "y": 4.3 + "x": 3.503946066380006, + "y": 1.0656861256661172 }, "isLocked": false, - "linkedName": "A3" + "linkedName": null }, { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 8.34, + "y": 1.43 }, "prevControl": { - "x": 0.7, - "y": 4.440124264863915 + "x": 7.34, + "y": 1.4299999999999997 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -58.49573328079582, + "rotation": 0, "rotateFast": false }, "reversed": false, - "folder": "Notes Near Starting Zone", + "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 1af67ff9..45480e77 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -126,6 +127,7 @@ private void configureBindings() { operatorController.rightBumper().onTrue(new ArmEmergencyStop(m_ArmSubsystem, m_PizzaBoxSubsystem)); operatorController.leftBumper().onTrue(new IntakeEmergencyStop(m_IntakeSubsystem)); operatorController.leftStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 240).getEntry().getDouble(245))); + operatorController.rightStick().whileTrue(new SpinArmCommand(m_ArmSubsystem)); } diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java b/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java index dc00b8d6..8fe5426c 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java @@ -34,7 +34,7 @@ public ScoreInTrapStutter(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armS addCommands( new StopNoteContainerMotor(pizzaBoxSubsystem), - new SpinToArmAngle(armSubsystem, 260).withTimeout(1), + new SpinToArmAngle(armSubsystem, 250).withTimeout(1), Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = false; }), diff --git a/src/main/java/frc/robot/commands/Arm/SpinArmCommand.java b/src/main/java/frc/robot/commands/Arm/SpinArmCommand.java new file mode 100644 index 00000000..70545c53 --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/SpinArmCommand.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.Arm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ArmSubsystem; + +public class SpinArmCommand extends Command { + /** Creates a new SpinArmCommand. */ + private final ArmSubsystem m_ArmSubsystem; + public SpinArmCommand(ArmSubsystem m_ArmSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.m_ArmSubsystem = m_ArmSubsystem; + addRequirements(m_ArmSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_ArmSubsystem.booster = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_ArmSubsystem.booster = false; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java index 6ef08d87..13cbadb5 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java @@ -27,7 +27,7 @@ public ClimbAndShoot(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, PizzaB // Add your commands in the addCommands() call, e.g. addCommands ( Commands.runOnce(()->DataLogManager.log("Command Start: ClimbAndShoot")), - new SpinToArmAngle(a, 260).withTimeout(1), + new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c).alongWith(new Extend(r)).withTimeout(3), new ScoreInTrapStutter(p, a), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index 6d3e1d6b..d4729c98 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -24,7 +24,7 @@ public class PrepClimb extends SequentialCommandGroup { public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), - new SpinToArmAngle(a, 260).withTimeout(1), + new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorUp(c).withTimeout(5), Commands.runOnce(()->DataLogManager.log("Command End: PrepClimb")) diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java index ecf3f3bf..fdd4a37a 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java @@ -27,9 +27,10 @@ public UnClimb(Climbsubsystem c, ArmSubsystem a, ReactionSubsystem r) { // addCommands(new FooCommand(), new BarCommand()); addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimb")), - new SpinToArmAngle(a, 260).withTimeout(1), + new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorUp(c).alongWith(new Retract(r)).withTimeout(5), + new MotorUp(c).withTimeout(5), + new Retract(r).withTimeout(1), Commands.runOnce(()->DataLogManager.log("Command End: UnClimb")) ); } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index 6b0021bd..36c9ff85 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -14,6 +14,7 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.ReactionSubsystem; +import frc.robot.subsystems.ArmSubsystem.Arm; import frc.robot.subsystems.swervedrive.SwerveSubsystem; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -25,9 +26,10 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimbPartTwo")), - new SpinToArmAngle(a, 135).withTimeout(1), + new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c).withTimeout(3), + new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) ); } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 9260c445..53b06496 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -36,15 +36,16 @@ public static final class Arm { public static final double ENCODER_RAW_TO_ROTATION = 8132.; public static final double ENCODER_OFFSET = 21.8; public static final int ARM_ID = 18; + // - public static final double KP = 6; + public static final double KP = 2.4; public static final double KI = 0.1; - public static final double KD = 0.15; + public static final double KD = 0; public static final double KSVOLTS = 1.5; public static final double KGVOLTS = .0; - public static final double KVVOLTS = 2; - public static final double KAVOLTS = 0; - public static final double VEL = 100 * Math.PI / 180; + public static final double KVVOLTS = 1.5; + public static final double KAVOLTS = 0.10; + public static final double VEL = 150 * Math.PI / 180; public static final double ACC = 180 * Math.PI / 180; // //Arm ID Jalen Tolbert @@ -62,6 +63,7 @@ public static final class Arm { private ArmFeedforward armFeedForward; public boolean emergencyStop = false; private double prevArmAngle; + public boolean booster = false; public ArmSubsystem(int armId, String armCanbus, int channel1) { @@ -106,6 +108,11 @@ public void spinArm(double speed) else if (speed > 15) { speed = 15; } + + if(booster) + { + speed = -15; + } VoltageOut armSpinRequest = new VoltageOut(speed, true, false, false, false); m_arm.setControl(armSpinRequest); } @@ -150,8 +157,8 @@ public void useOutput(double output, State setPoint) SmartDashboard.putNumber("Arm PID output", output); SmartDashboard.putNumber("Arm feed forward", feedForward); SmartDashboard.putNumber("Arm speed", output + feedForward); - SmartDashboard.putNumber("Arm FF setPoint Position", setPoint.position); - SmartDashboard.putNumber("Arm FF setPoint velocity", setPoint.velocity); + SmartDashboard.putNumber("Arm FF setPoint Position", setPoint.position * 180 / Math.PI); + SmartDashboard.putNumber("Arm FF setPoint velocity", setPoint.velocity * 180 / Math.PI); spinArm(output + feedForward); } else @@ -171,11 +178,12 @@ public double getMeasurement() return encoderGetAngle() * Math.PI/180; } - @Override - public void periodic() { - double currentArmAngle = encoderGetAngle(); - if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { - emergencyStop = true; - } - } + // @Override + // public void periodic() { + // double currentArmAngle = encoderGetAngle(); + // if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { + // emergencyStop = true; + // } + // prevArmAngle = currentArmAngle; + // } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java b/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java index dfcc027c..5fa557da 100644 --- a/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java +++ b/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java @@ -66,7 +66,7 @@ public class LimeLightSub extends SubsystemBase { NetworkTableEntry tid = networkTable.getEntry("tid"); boolean verbose = true;//If we want to print values - public boolean wantData = true;//If we want to accept limelight post esitmator + public boolean wantData = false;//If we want to accept limelight post esitmator // may be useful later private double kCameraHeight = From 5360b9a69d4fbc6b82ade9dbd8718e8c26d1ee0e Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 12:45:36 -0700 Subject: [PATCH 02/59] changed intake encoder offset --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f01e898b..667d139f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -95,7 +95,7 @@ public static final class IntakeConstants { public static final double MAX_ARM_ANGLE = 85; //need to check max arm angle public static final double ROTATION_TO_DEGREES = 360; public static final double ENCODER_RAW_TO_ROTATION = 8132.; - public static double ENCODER_OFFSET = 1; //need to check the encoder value + public static double ENCODER_OFFSET = 58.7; //need to check the encoder value public static final double UP_POSITION = 79; public static final double SOURCE_INTAKE_ANGLE = 160; From e8af97e11f09de69a755f1bd38f89e9086692f36 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 12:47:25 -0700 Subject: [PATCH 03/59] changed intake encoder offset --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 667d139f..9480cfb3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -95,7 +95,7 @@ public static final class IntakeConstants { public static final double MAX_ARM_ANGLE = 85; //need to check max arm angle public static final double ROTATION_TO_DEGREES = 360; public static final double ENCODER_RAW_TO_ROTATION = 8132.; - public static double ENCODER_OFFSET = 58.7; //need to check the encoder value + public static double ENCODER_OFFSET = 59.7; //need to check the encoder value public static final double UP_POSITION = 79; public static final double SOURCE_INTAKE_ANGLE = 160; From b41bcbc41a4270d778997ca67aa50fc949fc78bc Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 14:06:22 -0700 Subject: [PATCH 04/59] done tuning arm pid --- .../java/frc/robot/subsystems/ArmSubsystem.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 53b06496..820f6dc7 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -38,15 +38,15 @@ public static final class Arm { public static final int ARM_ID = 18; // - public static final double KP = 2.4; - public static final double KI = 0.1; + public static final double KP = 5.8; + public static final double KI = 0; public static final double KD = 0; public static final double KSVOLTS = 1.5; - public static final double KGVOLTS = .0; - public static final double KVVOLTS = 1.5; - public static final double KAVOLTS = 0.10; - public static final double VEL = 150 * Math.PI / 180; - public static final double ACC = 180 * Math.PI / 180; + public static final double KGVOLTS = 0; + public static final double KVVOLTS = 1.8; + public static final double KAVOLTS = 0; + public static final double VEL = 230 * Math.PI / 180; + public static final double ACC = 360 * Math.PI / 180; // //Arm ID Jalen Tolbert public static final int ENCODER_DIO_SLOT = 0; From a3cd95d6021f5a5776fea9ea36a1eae99f5282d9 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 14:47:41 -0700 Subject: [PATCH 05/59] dynamically change arm angle functionality; shuffleboard works --- src/main/java/frc/robot/GameRobotContainer.java | 6 ++++-- .../commands/Arm/ScoreInSpeakerAdjustable.java | 17 +++++++++++++++++ .../robot/commands/Arm/SpinArmAndPizzaBox.java | 14 ++++++++++++++ 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index f8cf5d13..2b4dd8f1 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -3,6 +3,7 @@ import java.io.File; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -84,7 +85,7 @@ public GameRobotContainer() { m_drivebase.setDefaultCommand(closedFieldRel); - SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); + //SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); configureBindings(); @@ -120,7 +121,8 @@ private void configureBindings() { operatorController.rightBumper().onTrue(new ArmEmergencyStop(m_ArmSubsystem, m_PizzaBoxSubsystem)); operatorController.leftBumper().onTrue(new IntakeEmergencyStop(m_IntakeSubsystem)); - operatorController.leftStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 240).getEntry().getDouble(245))); + GenericEntry s = Shuffleboard.getTab("Arm").add("Angle", 240).getEntry(); + operatorController.leftStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->s.getDouble(245))); operatorController.rightStick().whileTrue(new SpinArmCommand(m_ArmSubsystem)); } diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java b/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java index efec504c..d2acd8f8 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java @@ -32,4 +32,21 @@ public ScoreInSpeakerAdjustable(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsyste }) ); } + + public ScoreInSpeakerAdjustable(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem, DoubleSupplier angleSupplier) { + addCommands( + new SpinArmAndPizzaBox(pizzaBoxSubsystem, armSubsystem, angleSupplier, 100).withTimeout(2), + new SpinNoteContainerMotor(pizzaBoxSubsystem, 100, 100), + Commands.waitUntil(()->pizzaBoxSubsystem.isAtVelocity(90)).withTimeout(0.5), + new SwingForwardServo(pizzaBoxSubsystem), + Commands.waitSeconds(.2), + new SwingBackServo(pizzaBoxSubsystem), + Commands.waitSeconds(0.2), + new StopNoteContainerMotor(pizzaBoxSubsystem), + new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).withTimeout(0.1), + Commands.runOnce(() -> { + pizzaBoxSubsystem.hasNote = false; + }) + ); + } } diff --git a/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java b/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java index e8ef9080..c058976c 100644 --- a/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java +++ b/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java @@ -7,6 +7,8 @@ import frc.robot.subsystems.PizzaBoxSubsystem; import frc.robot.subsystems.ArmSubsystem; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; public class SpinArmAndPizzaBox extends Command { @@ -14,6 +16,7 @@ public class SpinArmAndPizzaBox extends Command { private ArmSubsystem armSubsystem; private double angle; private double vel; + private DoubleSupplier angleUpdate; @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) @@ -25,6 +28,14 @@ public SpinArmAndPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armS this.vel = vel; } + public SpinArmAndPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem, DoubleSupplier angle, double vel) { + this.pizzaBoxSubsystem = pizzaBoxSubsystem; + this.armSubsystem = armSubsystem; + this.angle = angle.getAsDouble(); + this.vel = vel; + angleUpdate = angle; + } + // Called when the command is initially scheduled. @Override public void initialize() { @@ -34,6 +45,9 @@ public void initialize() { @Override public void execute() { + if (angleUpdate != null) { + armSubsystem.targetArmAngle(angleUpdate.getAsDouble()); + } if(armSubsystem.encoderGetAngle() > PizzaBoxSubsystem.PizzaBox.START_SPIN_DEGREE) { pizzaBoxSubsystem.spinPizzaBoxMotor(vel, 500); From 4152945f4ba600a39703b03e7ea22267be91a854 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 14:57:17 -0700 Subject: [PATCH 06/59] rotate in place ang was flipped --- .pathplanner/settings.json | 4 ++-- src/main/deploy/pathplanner/paths/A1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C2.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C3.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-S1.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C2.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C3.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-S1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-C5.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/C4-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/C5-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-Leave.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-C4.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-C5.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-Leave.path | 4 ++-- src/main/deploy/pathplanner/paths/center.path | 4 ++-- src/main/deploy/pathplanner/paths/go to center.path | 4 ++-- .../java/frc/robot/commands/driveCommands/rotateinPlace.java | 4 +++- 40 files changed, 81 insertions(+), 79 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 911efa2a..f13d57fa 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -16,8 +16,8 @@ "3. A+A+C", "Hajel" ], - "defaultMaxVel": 1.5, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 2.0, "defaultMaxAngVel": 30.0, "defaultMaxAngAccel": 30.0, "maxModuleSpeed": 4.0 diff --git a/src/main/deploy/pathplanner/paths/A1-A2.path b/src/main/deploy/pathplanner/paths/A1-A2.path index 6b715bc5..9c32e859 100644 --- a/src/main/deploy/pathplanner/paths/A1-A2.path +++ b/src/main/deploy/pathplanner/paths/A1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index ca7af168..0df238b3 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index 417095c4..f460eec4 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 0a98cf02..146d8d36 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index 8c91f4c6..9c6a51b4 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index a84a6bc6..97a84844 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-A1.path b/src/main/deploy/pathplanner/paths/A2-A1.path index be9b85a4..555dd640 100644 --- a/src/main/deploy/pathplanner/paths/A2-A1.path +++ b/src/main/deploy/pathplanner/paths/A2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-A3.path b/src/main/deploy/pathplanner/paths/A2-A3.path index 6ebe8d7e..3e0be72b 100644 --- a/src/main/deploy/pathplanner/paths/A2-A3.path +++ b/src/main/deploy/pathplanner/paths/A2-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C1.path b/src/main/deploy/pathplanner/paths/A2-C1.path index f71bdb3f..3229bac9 100644 --- a/src/main/deploy/pathplanner/paths/A2-C1.path +++ b/src/main/deploy/pathplanner/paths/A2-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C2.path b/src/main/deploy/pathplanner/paths/A2-C2.path index 16b96f07..0ef54f06 100644 --- a/src/main/deploy/pathplanner/paths/A2-C2.path +++ b/src/main/deploy/pathplanner/paths/A2-C2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 734c97c4..1191b1fa 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index ba09cef2..4626bdb5 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index e223f74a..92b24e7a 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-A2.path b/src/main/deploy/pathplanner/paths/A3-A2.path index 28afbc2f..8487ba12 100644 --- a/src/main/deploy/pathplanner/paths/A3-A2.path +++ b/src/main/deploy/pathplanner/paths/A3-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index 53a8ccf8..ee200242 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index 7c414a79..92919f04 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index d4888b68..e689d87a 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-A1.path b/src/main/deploy/pathplanner/paths/C1-A1.path index b07be142..93ae4b16 100644 --- a/src/main/deploy/pathplanner/paths/C1-A1.path +++ b/src/main/deploy/pathplanner/paths/C1-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-A2.path b/src/main/deploy/pathplanner/paths/C1-A2.path index 1c778d45..a1e813ef 100644 --- a/src/main/deploy/pathplanner/paths/C1-A2.path +++ b/src/main/deploy/pathplanner/paths/C1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index b754e91f..5020644e 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 72cbddd4..2ddd7eb4 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C2-A2.path b/src/main/deploy/pathplanner/paths/C2-A2.path index 48a448c5..083aa8e8 100644 --- a/src/main/deploy/pathplanner/paths/C2-A2.path +++ b/src/main/deploy/pathplanner/paths/C2-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index dbdc1533..2d79d811 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index 8d8e5eb6..a68b153b 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index cf9935a9..07c37645 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index 0ab17111..f0f64a37 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index 3bed5a03..e87ce5ba 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index ea4c7bae..fe084b72 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index 1d95cffc..cad243b5 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index d1af5fc8..4af276db 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index 85966e85..91273800 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index 3572d908..b6063674 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index 0b63b706..c9329ca7 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index 0a89bc1b..e9f231c8 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index 29864531..7331e77a 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index a509e863..2c93c7af 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/center.path b/src/main/deploy/pathplanner/paths/center.path index 8985c86a..d152aaa4 100644 --- a/src/main/deploy/pathplanner/paths/center.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/go to center.path b/src/main/deploy/pathplanner/paths/go to center.path index 54460539..fd7ca704 100644 --- a/src/main/deploy/pathplanner/paths/go to center.path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java index a2b7f7b1..0d45ab53 100644 --- a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java +++ b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java @@ -57,9 +57,11 @@ public void execute() { currTheta = m_Subsystem.getHeading().getDegrees(); angleRate = PID.calculate(currTheta) / 4; - m_Subsystem.drive(pose, -angleRate, true); + m_Subsystem.drive(pose, angleRate, true); SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate / 4); + SmartDashboard.putNumber("Rotate In Place Angle curr angle", currTheta); + SmartDashboard.putNumber("Rotate In Place Angle goal", PID.getSetpoint()); } From 24efbe6a1fffd5e754a0f158673d9935603a6ec7 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 15:17:46 -0700 Subject: [PATCH 07/59] want limelight data --- src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java b/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java index 5fa557da..dfcc027c 100644 --- a/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java +++ b/src/main/java/frc/robot/subsystems/LimeVision/LimeLightSub.java @@ -66,7 +66,7 @@ public class LimeLightSub extends SubsystemBase { NetworkTableEntry tid = networkTable.getEntry("tid"); boolean verbose = true;//If we want to print values - public boolean wantData = false;//If we want to accept limelight post esitmator + public boolean wantData = true;//If we want to accept limelight post esitmator // may be useful later private double kCameraHeight = From 54574c5ce332878d80645df3779d57bd4ce301a6 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Mon, 1 Apr 2024 16:42:02 -0700 Subject: [PATCH 08/59] =?UTF-8?q?climb=20=E0=B2=A5=5F=E0=B2=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/deploy/pathplanner/autos/2N-S1-A1.auto | 2 +- src/main/deploy/pathplanner/autos/3N-S1-C1.auto | 2 +- src/main/deploy/pathplanner/autos/3N-S1-C2.auto | 2 +- src/main/deploy/pathplanner/paths/A1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C2.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C3.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-S1.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C2.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C3.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-S1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-C5.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/C4-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/C5-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-Leave.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-C4.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-C5.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-Leave.path | 4 ++-- src/main/deploy/pathplanner/paths/center.path | 4 ++-- src/main/deploy/pathplanner/paths/go to center.path | 4 ++-- src/main/java/frc/robot/GameRobotContainer.java | 10 +++++----- src/main/java/frc/robot/SetupShuffleboard.java | 4 ++-- .../ActuallyMovesMotors/MotorDown.java | 9 +++++++-- .../ClimberCommands/ActuallyMovesMotors/MotorUp.java | 6 +++++- .../commands/ClimberCommands/AutoClimb/ClimbDown.java | 4 ++-- .../commands/ClimberCommands/AutoClimb/ClimbUp.java | 4 ++-- .../ClimberCommands/ClimbParts/ClimbAndShoot.java | 2 +- .../ClimberCommands/ClimbParts/PrepClimb.java | 5 +++-- .../commands/ClimberCommands/ClimbParts/UnClimb.java | 2 +- .../UnClimbPartTwoThatWillBringDownTheMotor.java | 2 +- .../java/frc/robot/subsystems/Climbsubsystem.java | 11 ++++++----- 52 files changed, 114 insertions(+), 103 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto b/src/main/deploy/pathplanner/autos/2N-S1-A1.auto index 9c541187..c8fc7743 100644 --- a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S1-A1.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S1-C1.auto b/src/main/deploy/pathplanner/autos/3N-S1-C1.auto index a04f29cb..d0133c37 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-C1.auto +++ b/src/main/deploy/pathplanner/autos/3N-S1-C1.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S1-C2.auto b/src/main/deploy/pathplanner/autos/3N-S1-C2.auto index cebd19df..0247fc0d 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-C2.auto +++ b/src/main/deploy/pathplanner/autos/3N-S1-C2.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/paths/A1-A2.path b/src/main/deploy/pathplanner/paths/A1-A2.path index 9c32e859..6b715bc5 100644 --- a/src/main/deploy/pathplanner/paths/A1-A2.path +++ b/src/main/deploy/pathplanner/paths/A1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index 0df238b3..ca7af168 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index f460eec4..417095c4 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 146d8d36..0a98cf02 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index 9c6a51b4..8c91f4c6 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index 97a84844..a84a6bc6 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-A1.path b/src/main/deploy/pathplanner/paths/A2-A1.path index 555dd640..be9b85a4 100644 --- a/src/main/deploy/pathplanner/paths/A2-A1.path +++ b/src/main/deploy/pathplanner/paths/A2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-A3.path b/src/main/deploy/pathplanner/paths/A2-A3.path index 3e0be72b..6ebe8d7e 100644 --- a/src/main/deploy/pathplanner/paths/A2-A3.path +++ b/src/main/deploy/pathplanner/paths/A2-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C1.path b/src/main/deploy/pathplanner/paths/A2-C1.path index 3229bac9..f71bdb3f 100644 --- a/src/main/deploy/pathplanner/paths/A2-C1.path +++ b/src/main/deploy/pathplanner/paths/A2-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C2.path b/src/main/deploy/pathplanner/paths/A2-C2.path index 0ef54f06..16b96f07 100644 --- a/src/main/deploy/pathplanner/paths/A2-C2.path +++ b/src/main/deploy/pathplanner/paths/A2-C2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 1191b1fa..734c97c4 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index 4626bdb5..ba09cef2 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index 92b24e7a..e223f74a 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-A2.path b/src/main/deploy/pathplanner/paths/A3-A2.path index 8487ba12..28afbc2f 100644 --- a/src/main/deploy/pathplanner/paths/A3-A2.path +++ b/src/main/deploy/pathplanner/paths/A3-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index ee200242..53a8ccf8 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index 92919f04..7c414a79 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index e689d87a..d4888b68 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-A1.path b/src/main/deploy/pathplanner/paths/C1-A1.path index 93ae4b16..b07be142 100644 --- a/src/main/deploy/pathplanner/paths/C1-A1.path +++ b/src/main/deploy/pathplanner/paths/C1-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-A2.path b/src/main/deploy/pathplanner/paths/C1-A2.path index a1e813ef..1c778d45 100644 --- a/src/main/deploy/pathplanner/paths/C1-A2.path +++ b/src/main/deploy/pathplanner/paths/C1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index 5020644e..b754e91f 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 2ddd7eb4..72cbddd4 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C2-A2.path b/src/main/deploy/pathplanner/paths/C2-A2.path index 083aa8e8..48a448c5 100644 --- a/src/main/deploy/pathplanner/paths/C2-A2.path +++ b/src/main/deploy/pathplanner/paths/C2-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index 2d79d811..dbdc1533 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index a68b153b..8d8e5eb6 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index 07c37645..cf9935a9 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index f0f64a37..0ab17111 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index e87ce5ba..3bed5a03 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index fe084b72..ea4c7bae 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index cad243b5..1d95cffc 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index 4af276db..d1af5fc8 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index 91273800..85966e85 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index b6063674..3572d908 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index c9329ca7..0b63b706 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index e9f231c8..0a89bc1b 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index 7331e77a..29864531 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index 2c93c7af..a509e863 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/center.path b/src/main/deploy/pathplanner/paths/center.path index d152aaa4..8985c86a 100644 --- a/src/main/deploy/pathplanner/paths/center.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/go to center.path b/src/main/deploy/pathplanner/paths/go to center.path index fd7ca704..54460539 100644 --- a/src/main/deploy/pathplanner/paths/go to center.path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 2b4dd8f1..40fffab8 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -3,7 +3,6 @@ import java.io.File; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -85,7 +84,7 @@ public GameRobotContainer() { m_drivebase.setDefaultCommand(closedFieldRel); - //SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); + SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); configureBindings(); @@ -121,9 +120,10 @@ private void configureBindings() { operatorController.rightBumper().onTrue(new ArmEmergencyStop(m_ArmSubsystem, m_PizzaBoxSubsystem)); operatorController.leftBumper().onTrue(new IntakeEmergencyStop(m_IntakeSubsystem)); - GenericEntry s = Shuffleboard.getTab("Arm").add("Angle", 240).getEntry(); - operatorController.leftStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->s.getDouble(245))); - operatorController.rightStick().whileTrue(new SpinArmCommand(m_ArmSubsystem)); + + operatorController.leftStick().whileTrue(new LockHeadingToSourceForIntake(closedFieldRel, m_ArmSubsystem, m_PizzaBoxSubsystem)); + // operatorController. ?? ().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 242).getEntry().getDouble(245))); + operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 240).getEntry().getDouble(245))); } diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 7e0b50e2..90d76f66 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -111,8 +111,8 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); - Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem)).withPosition(1, 1); - Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem)).withPosition(0, 1); + Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem, armSubsystem)).withPosition(1, 1); + Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem, armSubsystem)).withPosition(0, 1); Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem)).withPosition(4, 0); Shuffleboard.getTab("Climb").add("climb & shoot", new ClimbAndShoot(climbSubsystem, swerve, armSubsystem, pizzaBoxSubsystem, reactionSubsystem)).withPosition(5, 0); diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java index f292de27..07ec6a19 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java @@ -5,6 +5,7 @@ package frc.robot.commands.ClimberCommands.ActuallyMovesMotors; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -12,10 +13,13 @@ public class MotorDown extends Command { /** Creates a new MotorDown. */ private Climbsubsystem climbersubsystem; + private ArmSubsystem armsubsystem; //constructor that takes in a Climbsubsystem object and a SwerveSubsystem obj - public MotorDown(Climbsubsystem c) { + public MotorDown(Climbsubsystem c, ArmSubsystem a) { + climbersubsystem = c; + armsubsystem = a; addRequirements(this.climbersubsystem); } @@ -27,7 +31,8 @@ public void initialize() { } @Override - public void execute() { + public void execute() { + climbersubsystem.armAngleCheck = armsubsystem.checkEncoderAngleForClimb(); } @Override diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java index e340654b..0a00914a 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ClimbConstants; +import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -13,11 +14,13 @@ public class MotorUp extends Command { /** Creates a new MotorUp. */ private final Climbsubsystem climbersubsystem; + private final ArmSubsystem armsubsystem; //constructor that takes in a Climbsubsystem object and a SwerveSubsystem obj - public MotorUp(Climbsubsystem c) { + public MotorUp(Climbsubsystem c, ArmSubsystem a) { climbersubsystem = c; + armsubsystem = a; addRequirements(this.climbersubsystem); } @@ -30,6 +33,7 @@ public void initialize() { @Override public void execute() { + climbersubsystem.armAngleCheck = armsubsystem.checkEncoderAngleForClimb(); } @Override diff --git a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java index 3e865798..48b6bd49 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java @@ -19,9 +19,9 @@ public class ClimbDown extends SequentialCommandGroup { public ClimbDown(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a) { addCommands( - new MotorUp(c), + new MotorUp(c, a), new ParallelCommandGroup(new DriveForClimb(s, 0.4)/* , new Retract(r)*/), - new MotorDown(c) + new MotorDown(c, a) ); // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java index 0b338ab3..826edd17 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java @@ -23,9 +23,9 @@ public class ClimbUp extends SequentialCommandGroup { public ClimbUp(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { addCommands( new ParallelCommandGroup(new SpinToArmAngle(a, 135)/* , new Extend(r)*/), - new SequentialCommandGroup(new WaitCommand(0.5), new MotorUp(c)), + new SequentialCommandGroup(new WaitCommand(0.5), new MotorUp(c, a)), new DriveForClimb(s, -0.4), - new MotorDown(c) + new MotorDown(c, a) ); // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java index 13cbadb5..152d0ac7 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java @@ -29,7 +29,7 @@ public ClimbAndShoot(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, PizzaB Commands.runOnce(()->DataLogManager.log("Command Start: ClimbAndShoot")), new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorDown(c).alongWith(new Extend(r)).withTimeout(3), + new MotorDown(c, a).alongWith(new Extend(r)).withTimeout(3), new ScoreInTrapStutter(p, a), Commands.runOnce(()->DataLogManager.log("Command End: ClimbAndShoot")) ); diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index d4729c98..9e5cdcd9 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.Arm.SpinToArmAngle; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; -import frc.robot.commands.ReactionArmCommands.Extend; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.ReactionSubsystem; @@ -24,9 +23,11 @@ public class PrepClimb extends SequentialCommandGroup { public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), + new SpinToArmAngle(a, ArmSubsystem.Arm.AMP_ANGLE).withTimeout(2), + //servo stuff new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorUp(c).withTimeout(5), + new MotorUp(c, a).withTimeout(5), Commands.runOnce(()->DataLogManager.log("Command End: PrepClimb")) ); } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java index fdd4a37a..094562a5 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java @@ -29,7 +29,7 @@ public UnClimb(Climbsubsystem c, ArmSubsystem a, ReactionSubsystem r) { Commands.runOnce(()->DataLogManager.log("Command Start: UnClimb")), new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorUp(c).withTimeout(5), + new MotorUp(c, a).withTimeout(5), new Retract(r).withTimeout(1), Commands.runOnce(()->DataLogManager.log("Command End: UnClimb")) ); diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index 36c9ff85..fc7b1229 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -28,7 +28,7 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem Commands.runOnce(()->DataLogManager.log("Command Start: UnClimbPartTwo")), new SpinToArmAngle(a, 250).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorDown(c).withTimeout(3), + new MotorDown(c, a).withTimeout(3), new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) ); diff --git a/src/main/java/frc/robot/subsystems/Climbsubsystem.java b/src/main/java/frc/robot/subsystems/Climbsubsystem.java index 1b1c5334..30f631a0 100644 --- a/src/main/java/frc/robot/subsystems/Climbsubsystem.java +++ b/src/main/java/frc/robot/subsystems/Climbsubsystem.java @@ -36,6 +36,7 @@ public class Climbsubsystem extends SubsystemBase { private boolean checkForUp = false; private boolean emergencyStop = false; + public boolean armAngleCheck = true; DigitalInput bottomLeft = new DigitalInput(ClimbConstants.BOT_LEFT_LIMIT_ID); DigitalInput bottomRight = new DigitalInput(ClimbConstants.BOT_RIGHT_LIMIT_ID); @@ -155,16 +156,16 @@ public void downMotor() { rightPIDcontroller.setGoal(.3); //0.3 } - public void downMotorHarmony() { - checkForUp = false; - - } - @Override public void periodic() { double leftPIDvalue = leftPIDcontroller.calculate(getPositionLeft()); double rightPIDvalue = rightPIDcontroller.calculate(getPositionRight()); + if (!armAngleCheck) { + leftPIDvalue = 0; + rightPIDvalue = 0; + } + if (checkForUp && getTopLeftLimit()) { leftPIDvalue = 0; } From da70f192f18b3ad2cbdee81bf5866b9bbd85034b Mon Sep 17 00:00:00 2001 From: chvoong24 Date: Mon, 1 Apr 2024 17:07:51 -0700 Subject: [PATCH 09/59] added code for second Servo --- .../java/frc/robot/GameRobotContainer.java | 2 +- .../java/frc/robot/SetupShuffleboard.java | 5 +++ .../commands/Arm/SwingBackServoTheSecond.java | 35 +++++++++++++++++++ .../Arm/SwingForwardServoTheSecond.java | 33 +++++++++++++++++ .../robot/subsystems/PizzaBoxSubsystem.java | 12 +++++-- .../robot/testcontainers/ArmContainer.java | 8 ++--- 6 files changed, 88 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java create mode 100644 src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 1af67ff9..e4bbcde3 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -61,7 +61,7 @@ public GameRobotContainer() { ArmSubsystem.Arm.ENCODER_DIO_SLOT); m_PizzaBoxSubsystem = new PizzaBoxSubsystem(PizzaBoxSubsystem.PizzaBox.PIZZABOX_ID, - "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT); + "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT, PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT_THE_SECOND); //m_ledsubsystem = new LEDSubsystem(Constants.LEDConstants.ledPortNumber); diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index de2992b1..eac69fa0 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -14,6 +14,8 @@ import frc.robot.commands.CancelAllCommands; import frc.robot.commands.Arm.ResetArm; import frc.robot.commands.Arm.ScoreInTrapStutter; +import frc.robot.commands.Arm.SwingBackServoTheSecond; +import frc.robot.commands.Arm.SwingForwardServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; import frc.robot.commands.ClimberCommands.ClimbParts.ClimbAndShoot; @@ -126,6 +128,9 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + DataLogManager.log("rotate in place P: " + Constants.DriveConstants.kP); DataLogManager.log("rotate in place I: " + Constants.DriveConstants.kI); DataLogManager.log("rotate in place D: " + Constants.DriveConstants.kD); diff --git a/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java new file mode 100644 index 00000000..3b46a9ab --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java @@ -0,0 +1,35 @@ +package frc.robot.commands.Arm; + +import frc.robot.subsystems.PizzaBoxSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwingBackServoTheSecond extends Command{ + + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final PizzaBoxSubsystem pizzaBoxSubsystem; + + public SwingBackServoTheSecond(PizzaBoxSubsystem pizzaBoxSubsystem) + { + this.pizzaBoxSubsystem = pizzaBoxSubsystem; + addRequirements(pizzaBoxSubsystem); + } + + public void initialize() { + pizzaBoxSubsystem.setServoAngleTheSecond(50); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + double motorAng = pizzaBoxSubsystem.getServoAngleTheSecond(); + + return Math.abs(motorAng - 50) < .001; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java new file mode 100644 index 00000000..2088b7d2 --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java @@ -0,0 +1,33 @@ +package frc.robot.commands.Arm; +import frc.robot.subsystems.PizzaBoxSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + + +public class SwingForwardServoTheSecond extends Command{ + + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final PizzaBoxSubsystem pizzaBoxSubsystem; + + public SwingForwardServoTheSecond(PizzaBoxSubsystem pizzaBoxSubsystem) + { + this.pizzaBoxSubsystem = pizzaBoxSubsystem; + addRequirements(pizzaBoxSubsystem); + } + public void initialize() { + pizzaBoxSubsystem.setServoAngleTheSecond(180); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + double motorAng = pizzaBoxSubsystem.getServoAngle(); + return Math.abs(motorAng - 180) < .001; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java b/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java index ed3575f4..273bf695 100644 --- a/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java @@ -23,6 +23,7 @@ public static final class PizzaBox { public static final int PIZZABOX_ID = 23; public static final int SERVO_PWN_SLOT = 0; + public static final int SERVO_PWN_SLOT_THE_SECOND = 1; public static final int START_SPIN_DEGREE = 180; } @@ -30,12 +31,13 @@ public static final class PizzaBox { private TalonFX m_pizzaBox; private Servo m_servo; + private Servo m_servo_the_second; - public PizzaBoxSubsystem(int pizzaBoxId, String pizzaBoxCanbus, int channelServo) + public PizzaBoxSubsystem(int pizzaBoxId, String pizzaBoxCanbus, int channelServo, int channelServoTheSecond) { m_pizzaBox = new TalonFX(pizzaBoxId, pizzaBoxCanbus); + m_servo_the_second = new Servo(channelServoTheSecond); m_servo = new Servo(channelServo); - UtilMotor.configMotor(m_pizzaBox, .5, 0, 0, .12, 15, 80, false); //UtilMotor.configMotorStatorCurrent(m_pizzaBox, 80); @@ -54,11 +56,17 @@ public void spinPizzaBoxMotor(double velocity, double acceleration) { public void setServoAngle(double angle) { m_servo.setAngle(angle); } + public void setServoAngleTheSecond(double angle) { + m_servo_the_second.setAngle(angle); + } //Returns the servo postion from 0.0 to 1.0 (0 degrees to 180 degrees) public double getServoAngle() { return m_servo.getAngle(); } + public double getServoAngleTheSecond() { + return m_servo_the_second.getAngle(); + } //Stops pizzaBox motor public void stopPizzaBoxMotor() { diff --git a/src/main/java/frc/robot/testcontainers/ArmContainer.java b/src/main/java/frc/robot/testcontainers/ArmContainer.java index 30d0fd3c..9e3fb4f7 100644 --- a/src/main/java/frc/robot/testcontainers/ArmContainer.java +++ b/src/main/java/frc/robot/testcontainers/ArmContainer.java @@ -20,8 +20,8 @@ public class ArmContainer implements BaseContainer { ArmSubsystem arm = new ArmSubsystem(ArmSubsystem.Arm.ARM_ID, "CAN_Network", ArmSubsystem.Arm.ENCODER_DIO_SLOT); - PizzaBoxSubsystem pizzaBox = new PizzaBoxSubsystem(PizzaBoxSubsystem.PizzaBox.PIZZABOX_ID, - "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT); + // PizzaBoxSubsystem pizzaBox = new PizzaBoxSubsystem(PizzaBoxSubsystem.PizzaBox.PIZZABOX_ID, + // "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT); private IntakeSubsystem intakeSubsystem = new IntakeSubsystem(Constants.IntakeConstants.INTAKE_LOWER_INTAKE_ID,Constants.IntakeConstants.INTAKE_SPIN_MOTOR_ID, "rio"); @@ -36,8 +36,8 @@ private void configureBindings() { arm.enable(); //Pick Up and Put in PizzaBox Command - m_driverController.a().onTrue(new PickUpFromGroundAndPassToPizzaBox(pizzaBox, arm, intakeSubsystem)); - m_driverController.x().whileTrue(new SpinNoteContainerMotor(pizzaBox, 400, 100)); + // m_driverController.a().onTrue(new PickUpFromGroundAndPassToPizzaBox(pizzaBox, arm, intakeSubsystem)); + // m_driverController.x().whileTrue(new SpinNoteContainerMotor(pizzaBox, 400, 100)); } From d7f72c25b741e196a6d2b0730eec27f39afa33e6 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 17:33:30 -0700 Subject: [PATCH 10/59] test servo --- src/main/java/frc/robot/GameRobotContainer.java | 7 ++++++- .../frc/robot/commands/Arm/SwingBackServoTheSecond.java | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index d24df1e1..05b2e9eb 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -84,7 +84,12 @@ public GameRobotContainer() { m_drivebase.setDefaultCommand(closedFieldRel); - SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); + //SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); + + Shuffleboard.getTab("Arm").add("270", new SpinToArmAngle(m_ArmSubsystem, 270)); + Shuffleboard.getTab("Arm").add("2nd forward", new SwingForwardServoTheSecond(m_PizzaBoxSubsystem)); + Shuffleboard.getTab("Arm").add("2nd backward", new SwingBackServoTheSecond(m_PizzaBoxSubsystem)); + configureBindings(); diff --git a/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java index 3b46a9ab..fa8a0c36 100644 --- a/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java +++ b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java @@ -15,7 +15,7 @@ public SwingBackServoTheSecond(PizzaBoxSubsystem pizzaBoxSubsystem) } public void initialize() { - pizzaBoxSubsystem.setServoAngleTheSecond(50); + pizzaBoxSubsystem.setServoAngleTheSecond(0); } @Override From 4333345a7e3e5f17468bba8495e650f59b14d221 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 18:31:15 -0700 Subject: [PATCH 11/59] servo works. changed intake encoder offset --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/GameRobotContainer.java | 3 ++- .../frc/robot/commands/Arm/SwingForwardServoTheSecond.java | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9480cfb3..b4fa73fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -91,11 +91,11 @@ public static final class IntakeConstants { public static final int INTAKE_SPIN_MOTOR_ID = 20; public static final int INTAKE_ENCODER_CHANNEL_ID = 9; public static final int INTAKE_NOTESENSOR_CHANNEL_ID = 8; - public static final double TOLERANCE = 4; // in degrees--- TO-DO: Tune Tolerance, intake is not lowering enough. + public static final double TOLERANCE = 2; // in degrees--- TO-DO: Tune Tolerance, intake is not lowering enough. public static final double MAX_ARM_ANGLE = 85; //need to check max arm angle public static final double ROTATION_TO_DEGREES = 360; public static final double ENCODER_RAW_TO_ROTATION = 8132.; - public static double ENCODER_OFFSET = 59.7; //need to check the encoder value + public static double ENCODER_OFFSET = 55.5; //need to check the encoder value public static final double UP_POSITION = 79; public static final double SOURCE_INTAKE_ANGLE = 160; diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 05b2e9eb..c6bcddd2 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -195,7 +195,8 @@ public void setMotorBrake(boolean brake) } public Command teleopInitReset() { - return new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem) + return new SwingBackServoTheSecond(m_PizzaBoxSubsystem) + .andThen(new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem)) .andThen(new IntakeResetArm(m_IntakeSubsystem)); } diff --git a/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java index 2088b7d2..114f65ba 100644 --- a/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java +++ b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java @@ -14,7 +14,7 @@ public SwingForwardServoTheSecond(PizzaBoxSubsystem pizzaBoxSubsystem) addRequirements(pizzaBoxSubsystem); } public void initialize() { - pizzaBoxSubsystem.setServoAngleTheSecond(180); + pizzaBoxSubsystem.setServoAngleTheSecond(200); } @Override @@ -28,6 +28,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { double motorAng = pizzaBoxSubsystem.getServoAngle(); - return Math.abs(motorAng - 180) < .001; + return Math.abs(motorAng - 200) < .001; } } \ No newline at end of file From b57047d6b136f0c1a3de7ac9127453883ed2f703 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 18:42:49 -0700 Subject: [PATCH 12/59] servo return true --- src/main/java/frc/robot/GameRobotContainer.java | 5 ----- .../java/frc/robot/commands/Arm/SwingBackServoTheSecond.java | 4 +--- .../frc/robot/commands/Arm/SwingForwardServoTheSecond.java | 3 +-- 3 files changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index c6bcddd2..2674d88e 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -86,11 +86,6 @@ public GameRobotContainer() { //SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); - Shuffleboard.getTab("Arm").add("270", new SpinToArmAngle(m_ArmSubsystem, 270)); - Shuffleboard.getTab("Arm").add("2nd forward", new SwingForwardServoTheSecond(m_PizzaBoxSubsystem)); - Shuffleboard.getTab("Arm").add("2nd backward", new SwingBackServoTheSecond(m_PizzaBoxSubsystem)); - - configureBindings(); } diff --git a/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java index fa8a0c36..d8d931b2 100644 --- a/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java +++ b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java @@ -28,8 +28,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - double motorAng = pizzaBoxSubsystem.getServoAngleTheSecond(); - - return Math.abs(motorAng - 50) < .001; + return true; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java index 114f65ba..66015480 100644 --- a/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java +++ b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java @@ -27,7 +27,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - double motorAng = pizzaBoxSubsystem.getServoAngle(); - return Math.abs(motorAng - 200) < .001; + return true; } } \ No newline at end of file From fdfeeca657030a6b73f6b19f6177a4ef03fade60 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Mon, 1 Apr 2024 18:47:22 -0700 Subject: [PATCH 13/59] cloim --- src/main/java/frc/robot/Constants.java | 1 + .../ClimberCommands/ClimbParts/ClimbAndShoot.java | 3 ++- .../ClimberCommands/ClimbParts/PrepClimb.java | 12 ++++++++---- .../UnClimbPartTwoThatWillBringDownTheMotor.java | 9 +++++++-- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b4fa73fb..dc16a4c2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -83,6 +83,7 @@ public static final class ClimbConstants { public static final int TOP_RIGHT_LIMIT_ID = 4; public static final int TOP_LEFT_LIMIT_ID = 5; + public static final int CLIMB_ARM_ANGLE = 355; } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java index 152d0ac7..916c974d 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; import frc.robot.commands.Arm.*; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; import frc.robot.commands.ReactionArmCommands.Extend; @@ -27,7 +28,7 @@ public ClimbAndShoot(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, PizzaB // Add your commands in the addCommands() call, e.g. addCommands ( Commands.runOnce(()->DataLogManager.log("Command Start: ClimbAndShoot")), - new SpinToArmAngle(a, 250).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).alongWith(new Extend(r)).withTimeout(3), new ScoreInTrapStutter(p, a), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index 9e5cdcd9..03b0241e 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -11,21 +11,25 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; +import frc.robot.commands.Arm.SwingForwardServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem; import frc.robot.subsystems.ReactionSubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem.PizzaBox; import frc.robot.subsystems.swervedrive.SwerveSubsystem; public class PrepClimb extends SequentialCommandGroup { /** Creates a new PrepClimb. */ - public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { + public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), - new SpinToArmAngle(a, ArmSubsystem.Arm.AMP_ANGLE).withTimeout(2), - //servo stuff - new SpinToArmAngle(a, 250).withTimeout(1), + new SpinToArmAngle(a, 330).withTimeout(2), + new SwingForwardServoTheSecond(p), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorUp(c, a).withTimeout(5), Commands.runOnce(()->DataLogManager.log("Command End: PrepClimb")) diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index fc7b1229..a7328ee1 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -8,11 +8,14 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; +import frc.robot.commands.Arm.SwingBackServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; import frc.robot.commands.ReactionArmCommands.Retract; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem; import frc.robot.subsystems.ReactionSubsystem; import frc.robot.subsystems.ArmSubsystem.Arm; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -22,13 +25,15 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class UnClimbPartTwoThatWillBringDownTheMotor extends SequentialCommandGroup { /** Creates a new UnClimbPartTwoThatWillBringDownTheMotor. */ - public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { + public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimbPartTwo")), - new SpinToArmAngle(a, 250).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).withTimeout(3), + new SpinToArmAngle(a, 330), + new SwingBackServoTheSecond(p), new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) ); From def9fd29e8a66a640897ec89dfb9a1af9a29774f Mon Sep 17 00:00:00 2001 From: CatonQ Date: Mon, 1 Apr 2024 18:48:24 -0700 Subject: [PATCH 14/59] fices??? --- src/main/java/frc/robot/GameRobotContainer.java | 4 ++-- src/main/java/frc/robot/SetupShuffleboard.java | 4 ++-- src/main/java/frc/robot/commands/SystemCheck.java | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 2674d88e..580242d4 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -112,10 +112,10 @@ private void configureBindings() { /* Operator Controllers */ - operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem)); + operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); operatorController.b().onTrue(new ClimbAndShoot(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_PizzaBoxSubsystem, m_ReactionSubsystem)); operatorController.a().onTrue(new UnClimb(m_ClimbSubsystem, m_ArmSubsystem, m_ReactionSubsystem)); - operatorController.x().onTrue(new UnClimbPartTwoThatWillBringDownTheMotor(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem)); + operatorController.x().onTrue(new UnClimbPartTwoThatWillBringDownTheMotor(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); operatorController.start().onTrue(new StopClimb(m_ClimbSubsystem)); operatorController.rightBumper().onTrue(new ArmEmergencyStop(m_ArmSubsystem, m_PizzaBoxSubsystem)); diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 069cf716..69d075d6 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -116,10 +116,10 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem, armSubsystem)).withPosition(1, 1); Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem, armSubsystem)).withPosition(0, 1); - Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem)).withPosition(4, 0); + Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(4, 0); Shuffleboard.getTab("Climb").add("climb & shoot", new ClimbAndShoot(climbSubsystem, swerve, armSubsystem, pizzaBoxSubsystem, reactionSubsystem)).withPosition(5, 0); Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); - Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem)).withPosition(5, 1); + Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); diff --git a/src/main/java/frc/robot/commands/SystemCheck.java b/src/main/java/frc/robot/commands/SystemCheck.java index 89381e74..1776dd5c 100644 --- a/src/main/java/frc/robot/commands/SystemCheck.java +++ b/src/main/java/frc/robot/commands/SystemCheck.java @@ -32,10 +32,10 @@ public SystemCheck(ArmSubsystem a, Climbsubsystem c, IntakeSubsystem i, PizzaBox addCommands( //TO-DO: Add condition for different systemcheck if arm/intake is broken - new PrepClimb(c, s, a, r), new WaitCommand(2), + new PrepClimb(c, s, a, r, p), new WaitCommand(2), new ClimbAndShoot(c, s, a, p, r), new WaitCommand(2), new UnClimb(c, a, r), new WaitCommand(2), - new UnClimbPartTwoThatWillBringDownTheMotor(c, s, a, r), new WaitCommand(2), + new UnClimbPartTwoThatWillBringDownTheMotor(c, s, a, r, p), new WaitCommand(2), new PickUpFromGroundAndPassToPizzaBox(p, a, i), new WaitCommand(2), new PickUpFromGroundAndPassToPizzaBox(p, a, i), new WaitCommand(2), new ScoreInAmp(p, a), new WaitCommand(2), From 9dd737ce0474189422b68038446c71b1a16d8ae8 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 18:50:30 -0700 Subject: [PATCH 15/59] teleop init with timeout --- src/main/java/frc/robot/GameRobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 2674d88e..91947d64 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -190,13 +190,13 @@ public void setMotorBrake(boolean brake) } public Command teleopInitReset() { - return new SwingBackServoTheSecond(m_PizzaBoxSubsystem) - .andThen(new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem)) - .andThen(new IntakeResetArm(m_IntakeSubsystem)); + return new SwingBackServoTheSecond(m_PizzaBoxSubsystem).withTimeout(1) + .andThen(new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem)).withTimeout(1) + .andThen(new IntakeResetArm(m_IntakeSubsystem)).withTimeout(1); } public Command autoInitReset() { - return new IntakeResetArm(m_IntakeSubsystem); + return new IntakeResetArm(m_IntakeSubsystem).withTimeout(1); } } From dacecc6850de6ee46b23715b83d9d65364b81686 Mon Sep 17 00:00:00 2001 From: chvoong24 Date: Mon, 1 Apr 2024 18:57:03 -0700 Subject: [PATCH 16/59] made command --- .../java/frc/robot/SetupShuffleboard.java | 2 + .../driveCommands/StraightenWheelCommand.java | 42 +++++++++++++++++++ 2 files changed, 44 insertions(+) create mode 100644 src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 069cf716..da30e5cf 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -33,6 +33,7 @@ import frc.robot.commands.ReactionArmCommands.Extend; import frc.robot.commands.ReactionArmCommands.Retract; import frc.robot.commands.driveCommands.ResetTeleopDrive; +import frc.robot.commands.driveCommands.StraightenWheelCommand; import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.commands.driveCommands.LockHeadingToSourceForIntake; import frc.robot.subsystems.ArmSubsystem; @@ -101,6 +102,7 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p .withPosition(5,3); Shuffleboard.getTab("GameTab").add("CLEAR ALL COMMANDS", new CancelAllCommands()).withPosition(7,3); Shuffleboard.getTab("GameTab").add("RESET TELEOP DRIVE", new ResetTeleopDrive(teleopDrive)).withPosition(9,0); + Shuffleboard.getTab("GameTab").add("Straighten", new StraightenWheelCommand(swerve)).withPosition(9, 1); Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); diff --git a/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java new file mode 100644 index 00000000..9de0ae63 --- /dev/null +++ b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.driveCommands; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +public class StraightenWheelCommand extends Command { + /** Creates a new StraightenWheelCommand. */ + private final SwerveSubsystem m_Subsystem; + public StraightenWheelCommand(SwerveSubsystem m_Subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.m_Subsystem = m_Subsystem; + addRequirements(m_Subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + ChassisSpeeds speed = new ChassisSpeeds(1,1,0); + m_Subsystem.driveFieldOriented(speed); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} From ff46ed7d20985892459aaa3c9a98f1ac9b7775f0 Mon Sep 17 00:00:00 2001 From: chvoong24 Date: Mon, 1 Apr 2024 19:15:10 -0700 Subject: [PATCH 17/59] check pls --- .../java/frc/robot/SetupShuffleboard.java | 65 ++++++++++--------- .../driveCommands/StraightenWheelCommand.java | 2 +- 2 files changed, 37 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 79fcf905..7c280174 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.*; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -60,6 +61,39 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p // LimelightHelpers.setStreamMode_PiPSecondary("limelight"); // Shuffleboard.getTab("GameTab").addCamera("Vision", "limelight", "http://limelight.local:5800").withSize(4,3).withPosition(5, 0); + //Climb stuff (all in climb tab) + + if(DriverStation.isTest()) + { + Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(4, 0); + Shuffleboard.getTab("Climb").add("climb & shoot", new ClimbAndShoot(climbSubsystem, swerve, armSubsystem, pizzaBoxSubsystem, reactionSubsystem)).withPosition(5, 0); + Shuffleboard.getTab("Climb").addDouble("climb distance left", () -> climbSubsystem.getPositionLeft()).withPosition(0, 0); + Shuffleboard.getTab("Climb").addDouble("climb distance right", () -> climbSubsystem.getPositionRight()).withPosition(1, 0); + Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem, armSubsystem)).withPosition(1, 1); + Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem, armSubsystem)).withPosition(0, 1); + Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); + Shuffleboard.getTab("Climb").addBoolean("bot right limit", () -> climbSubsystem.getBotRightLimit()).withPosition(3, 1); + Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); + Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); + + + Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); + Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); + + Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); + + Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); + + Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); + + Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); + + Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + + + } + Shuffleboard.getTab("GameTab").add("Field", swerve.getField2d()) .withSize(3, 2) .withPosition(0, 0); @@ -107,36 +141,9 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); - //Climb stuff (all in climb tab) - Shuffleboard.getTab("Climb").addDouble("climb distance left", () -> climbSubsystem.getPositionLeft()).withPosition(0, 0); - Shuffleboard.getTab("Climb").addDouble("climb distance right", () -> climbSubsystem.getPositionRight()).withPosition(1, 0); - Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); - Shuffleboard.getTab("Climb").addBoolean("bot right limit", () -> climbSubsystem.getBotRightLimit()).withPosition(3, 1); - Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); - Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); - - Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem, armSubsystem)).withPosition(1, 1); - Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem, armSubsystem)).withPosition(0, 1); - - Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(4, 0); - Shuffleboard.getTab("Climb").add("climb & shoot", new ClimbAndShoot(climbSubsystem, swerve, armSubsystem, pizzaBoxSubsystem, reactionSubsystem)).withPosition(5, 0); - Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); - Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); - - Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); - - Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); - - - Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve)); - Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); - - Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); - - Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); - + Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve)); + DataLogManager.log("rotate in place P: " + Constants.DriveConstants.kP); DataLogManager.log("rotate in place I: " + Constants.DriveConstants.kI); DataLogManager.log("rotate in place D: " + Constants.DriveConstants.kD); diff --git a/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java index 9de0ae63..a926e613 100644 --- a/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java +++ b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java @@ -20,7 +20,7 @@ public StraightenWheelCommand(SwerveSubsystem m_Subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - ChassisSpeeds speed = new ChassisSpeeds(1,1,0); + ChassisSpeeds speed = new ChassisSpeeds(1,0,0); m_Subsystem.driveFieldOriented(speed); } From 60bd1708337ed23a55b3b62f45a4b5dcdb0917f2 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 19:43:45 -0700 Subject: [PATCH 18/59] bug arm and intake arm not moving --- src/main/java/frc/robot/Constants.java | 2 +- .../ClimberCommands/ClimbParts/PrepClimb.java | 2 +- ...ClimbPartTwoThatWillBringDownTheMotor.java | 2 +- .../frc/robot/subsystems/ArmSubsystem.java | 24 +++++++++++-------- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- 5 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc16a4c2..3db07837 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -83,7 +83,7 @@ public static final class ClimbConstants { public static final int TOP_RIGHT_LIMIT_ID = 4; public static final int TOP_LEFT_LIMIT_ID = 5; - public static final int CLIMB_ARM_ANGLE = 355; + public static final int CLIMB_ARM_ANGLE = 255; } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index 03b0241e..c2b71468 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -27,7 +27,7 @@ public class PrepClimb extends SequentialCommandGroup { public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), - new SpinToArmAngle(a, 330).withTimeout(2), + new SpinToArmAngle(a, 270).withTimeout(2), new SwingForwardServoTheSecond(p), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index a7328ee1..50078c84 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -32,7 +32,7 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).withTimeout(3), - new SpinToArmAngle(a, 330), + new SpinToArmAngle(a, 270), new SwingBackServoTheSecond(p), new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 820f6dc7..429a2485 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -51,7 +51,7 @@ public static final class Arm { //Arm ID Jalen Tolbert public static final int ENCODER_DIO_SLOT = 0; public static final int AMP_ANGLE = 300; - public static final int TRAP_ANGLE = 290; + public static final int TRAP_ANGLE = 270; public static final int SPEAKER_LOW_ANGLE = 165; public static final int SPEAKER_HIGH_ANGLE = 238; public static final int INTAKE_ANGLE = 60; @@ -152,7 +152,7 @@ public void useOutput(double output, State setPoint) { //Comment out for testing purposes double feedForward = armFeedForward.calculate(setPoint.position, setPoint.velocity); - if(!isEmergencyStop()) + if(true || !isEmergencyStop()) { SmartDashboard.putNumber("Arm PID output", output); SmartDashboard.putNumber("Arm feed forward", feedForward); @@ -178,12 +178,16 @@ public double getMeasurement() return encoderGetAngle() * Math.PI/180; } - // @Override - // public void periodic() { - // double currentArmAngle = encoderGetAngle(); - // if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { - // emergencyStop = true; - // } - // prevArmAngle = currentArmAngle; - // } + @Override + public void periodic() { + double currentArmAngle = encoderGetAngle(); + // if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { + // emergencyStop = true; + // } + // prevArmAngle = currentArmAngle; + + // if (currentArmAngle <= Arm.ARM_MIN_ANGLE || currentArmAngle > Arm.ARM_MAX_ANGLE) { + // emergencyStop = true; + // } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 0fab06f4..e83e89aa 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -109,7 +109,7 @@ public void spinIntakeArm(double speed) { } else if (speed > 1) { // Will not be greater than maximum angle speed = 1; } - if (!isEmergencyStop()) { + if (true || !isEmergencyStop()) { SmartDashboard.putNumber("Intake Arm speed", speed); m_moveIntakeArm.set(speed); } From 5ee7d907d0294e04b42d2b078dc52d72bcbf90ab Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 19:52:53 -0700 Subject: [PATCH 19/59] arm intake still not spinning --- src/main/java/frc/robot/GameRobotContainer.java | 8 ++++---- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 0f80c7fa..580242d4 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -190,13 +190,13 @@ public void setMotorBrake(boolean brake) } public Command teleopInitReset() { - return new SwingBackServoTheSecond(m_PizzaBoxSubsystem).withTimeout(1) - .andThen(new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem)).withTimeout(1) - .andThen(new IntakeResetArm(m_IntakeSubsystem)).withTimeout(1); + return new SwingBackServoTheSecond(m_PizzaBoxSubsystem) + .andThen(new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem)) + .andThen(new IntakeResetArm(m_IntakeSubsystem)); } public Command autoInitReset() { - return new IntakeResetArm(m_IntakeSubsystem).withTimeout(1); + return new IntakeResetArm(m_IntakeSubsystem); } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 6ed85cfa..b70fe796 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -155,7 +155,7 @@ public void useOutput(double output, State setPoint) { //Comment out for testing purposes double feedForward = armFeedForward.calculate(setPoint.position, setPoint.velocity); - if(true || !isEmergencyStop()) + if(!isEmergencyStop()) { SmartDashboard.putNumber("Arm PID output", output); SmartDashboard.putNumber("Arm feed forward", feedForward); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c5a96a53..87031bfc 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -108,7 +108,7 @@ public void spinIntakeArm(double speed) { } else if (speed > 1) { // Will not be greater than maximum angle speed = 1; } - if (true || !isEmergencyStop()) { + if (!isEmergencyStop()) { SmartDashboard.putNumber("Intake Arm speed", speed); m_moveIntakeArm.set(speed); } From bbc11cd9ed5c0c7aa4c1aae32501187c1bbb93d1 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Mon, 1 Apr 2024 19:54:42 -0700 Subject: [PATCH 20/59] reaction bar --- .../ActuallyMovesMotors/MotorDown.java | 1 - .../commands/ReactionArmCommands/Extend.java | 19 +++++++--------- .../commands/ReactionArmCommands/Retract.java | 20 +++++++---------- .../robot/subsystems/ReactionSubsystem.java | 22 +++++++++++++++---- 4 files changed, 34 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java index 07ec6a19..9ef2e9a3 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; public class MotorDown extends Command { /** Creates a new MotorDown. */ diff --git a/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java b/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java index 5d5f3bee..f7794641 100644 --- a/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java +++ b/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java @@ -10,8 +10,8 @@ public class Extend extends Command { private final ReactionSubsystem m_Subsystem; - private double currPos; //position in rotation - private double endPos; + //private double currPos; //position in rotation + //private double endPos; /** Creates a new Extend. */ public Extend(ReactionSubsystem subsystem) { // Use addRequirements() here to declare subsystem dependencies. @@ -22,30 +22,27 @@ public Extend(ReactionSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - currPos = m_Subsystem.getPos(); - endPos = Constants.ReactionConstants.extendedPosition; //adds the current position to the final which is the end pos. + //currPos = m_Subsystem.getPos(); + //endPos = Constants.ReactionConstants.extendedPosition; //adds the current position to the final which is the end pos. m_Subsystem.spinForward(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - currPos = m_Subsystem.getPos(); + //currPos = m_Subsystem.getPos(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_Subsystem.stop(); + // m_Subsystem.stop(); } // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance) - { - return true; - } - return false; + //return (Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance); + return m_Subsystem.PIDcontroller.atSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java b/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java index 3281aa6b..dc78f375 100644 --- a/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java +++ b/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java @@ -11,8 +11,8 @@ public class Retract extends Command { /** Creates a new Retract. */ private final ReactionSubsystem m_Subsystem; -private double currPos; //position in rotation -private double endPos; +//private double currPos; //position in rotation +//private double endPos; public Retract(ReactionSubsystem subsystem) { // Use addRequirements() here to declare subsystem dependencies. this.m_Subsystem = subsystem; @@ -22,32 +22,28 @@ public Retract(ReactionSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - currPos = m_Subsystem.getPos(); - endPos = Constants.ReactionConstants.retractedPosition; //adds the current position to the final which is the end pos. + // currPos = m_Subsystem.getPos(); + // endPos = Constants.ReactionConstants.retractedPosition; //adds the current position to the final which is the end pos. m_Subsystem.spinBackward(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - currPos = m_Subsystem.getPos(); + // currPos = m_Subsystem.getPos(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_Subsystem.stop(); + //m_Subsystem.stop(); } // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance || currPos >0) - { - return true; - } - return false; - + // Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance || currPos >0); + return m_Subsystem.PIDcontroller.atSetpoint(); } } diff --git a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java index af5a8635..cf8ff509 100644 --- a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java @@ -6,11 +6,18 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix6.signals.NeutralModeValue; public class ReactionSubsystem extends SubsystemBase { private TalonFX m_reactionArm; + private final double REACTION_BAR_PID_KP = 0.8; + private final double REACTION_BAR_PID_KI = 0; + private final double REACTION_BAR_PID_KD = 0; + public PIDController PIDcontroller = new PIDController(REACTION_BAR_PID_KP, REACTION_BAR_PID_KI, REACTION_BAR_PID_KD); + /** Creates a new ReactionSubsystem. */ public ReactionSubsystem(int armID, String canbus) { m_reactionArm = new TalonFX(armID, canbus); @@ -23,15 +30,23 @@ public ReactionSubsystem(int armID, String canbus) { @Override public void periodic() { // This method will be called once per scheduler run + double PIDvalue = PIDcontroller.calculate(getPos()); + if (PIDvalue > 1) + PIDvalue = 1; + + if (PIDvalue < -1) + PIDvalue = -1; + + } public void spinForward() { - m_reactionArm.set(-0.1); + PIDcontroller.setSetpoint(-2.5); } public void spinBackward() { - m_reactionArm.set(0.1); + PIDcontroller.setSetpoint(0); } public void stop() { @@ -40,8 +55,7 @@ public void stop() public double getPos() { - double posVal = m_reactionArm.getRotorPosition().getValue(); - return posVal; + return m_reactionArm.getRotorPosition().getValue(); } From 3adbd63733aeebd5b43b46ef04393e9f7a79727b Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 19:58:05 -0700 Subject: [PATCH 21/59] revert arm angle from shuffleboard --- src/main/java/frc/robot/GameRobotContainer.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 580242d4..f4a2f4b3 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -3,6 +3,7 @@ import java.io.File; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -122,8 +123,8 @@ private void configureBindings() { operatorController.leftBumper().onTrue(new IntakeEmergencyStop(m_IntakeSubsystem)); operatorController.leftStick().whileTrue(new LockHeadingToSourceForIntake(closedFieldRel, m_ArmSubsystem, m_PizzaBoxSubsystem)); - // operatorController. ?? ().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 242).getEntry().getDouble(245))); - operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 240).getEntry().getDouble(245))); + GenericEntry s = Shuffleboard.getTab("Arm").add("Angle", 236).getEntry(); + operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->s.getDouble(236))); } From b2caba28ab1a44d00cba0ed1a013170f0ca689ef Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 20:03:44 -0700 Subject: [PATCH 22/59] score in trap stutter use climb arm angle constant --- .../java/frc/robot/commands/Arm/ScoreInTrapStutter.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java b/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java index 8fe5426c..534c83eb 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java @@ -5,6 +5,7 @@ package frc.robot.commands.Arm; import frc.robot.subsystems.PizzaBoxSubsystem; +import frc.robot.Constants; import frc.robot.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -17,7 +18,7 @@ public class ScoreInTrapStutter extends SequentialCommandGroup { public ScoreInTrapStutter(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem) { addCommands( new PrintCommand("Score in trp stutter initialize"), - new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.TRAP_ANGLE).withTimeout(3)); + new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.TRAP_ANGLE).withTimeout(3)); for(int index = 0; index < 8; index++) @@ -34,11 +35,11 @@ public ScoreInTrapStutter(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armS addCommands( new StopNoteContainerMotor(pizzaBoxSubsystem), - new SpinToArmAngle(armSubsystem, 250).withTimeout(1), + new SpinToArmAngle(armSubsystem, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = false; }), - new PrintCommand("scorein trap stutter finished") + new PrintCommand("scorein trap stutter finished") ); } From d41a1409af46b75ed90b17bdba3bff379733e33b Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 1 Apr 2024 20:10:36 -0700 Subject: [PATCH 23/59] arm works --- .../frc/robot/subsystems/ArmSubsystem.java | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index b70fe796..0aabc74a 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -153,7 +153,17 @@ public boolean checkEncoderAngleForClimb() { @Override public void useOutput(double output, State setPoint) { - //Comment out for testing purposes + double currentArmAngle = encoderGetAngle(); + if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { + emergencyStop = true; + } + prevArmAngle = currentArmAngle; + + if (currentArmAngle <= Arm.ARM_MIN_ANGLE || currentArmAngle > Arm.ARM_MAX_ANGLE) { + emergencyStop = true; + } + + double feedForward = armFeedForward.calculate(setPoint.position, setPoint.velocity); if(!isEmergencyStop()) { @@ -180,17 +190,4 @@ public double getMeasurement() { return encoderGetAngle() * Math.PI/180; } - - @Override - public void periodic() { - double currentArmAngle = encoderGetAngle(); - // if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { - // emergencyStop = true; - // } - // prevArmAngle = currentArmAngle; - - // if (currentArmAngle <= Arm.ARM_MIN_ANGLE || currentArmAngle > Arm.ARM_MAX_ANGLE) { - // emergencyStop = true; - // } - } } \ No newline at end of file From d13172b508f9615cf381c34b144faf890c48a064 Mon Sep 17 00:00:00 2001 From: isaacngkh Date: Mon, 1 Apr 2024 21:01:45 -0700 Subject: [PATCH 24/59] adjusted arm speed safety guards --- .../frc/robot/subsystems/ArmSubsystem.java | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 0aabc74a..86d82a4f 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -30,13 +30,14 @@ public static final class Arm { public static final int kSlotIdx = 0; public static final int kPIDLoopIdx = 0; public static final int kTimeoutMs = 30; - public static final int ARM_MAX_ANGLE = 335; + public static final int ARM_MAX_ANGLE = 330; public static final int ARM_MIN_ANGLE = 10; public static final int ROTATION_TO_DEGREES = 360; public static final double GEAR_RATIO = 118.587767088; public static final double ENCODER_RAW_TO_ROTATION = 8132.; public static final double ENCODER_OFFSET = 21.8; public static final int ARM_ID = 18; + public static final int MAX_VOLT = 12; // public static final double KP = 5.8; @@ -105,17 +106,28 @@ public ArmSubsystem(int armId, String armCanbus, int channel1) //Looking at the left of the robot, counterclockwise arm spin is positive public void spinArm(double speed) { - if(speed < -15) { - speed = -15; + if(speed < -Arm.MAX_VOLT) { + speed = -Arm.MAX_VOLT; } - else if (speed > 15) { - speed = 15; + else if (speed > Arm.MAX_VOLT) { + speed = Arm.MAX_VOLT; } if(booster) { - speed = -15; + speed = Arm.MAX_VOLT; } + + if(encoderGetAngle() < 0) { + speed = -5; + } + + if(encoderGetAngle() >= 300) { + if (speed > 0.5) { + speed = 0.5; + } + } + VoltageOut armSpinRequest = new VoltageOut(speed, true, false, false, false); m_arm.setControl(armSpinRequest); } @@ -159,10 +171,6 @@ public void useOutput(double output, State setPoint) } prevArmAngle = currentArmAngle; - if (currentArmAngle <= Arm.ARM_MIN_ANGLE || currentArmAngle > Arm.ARM_MAX_ANGLE) { - emergencyStop = true; - } - double feedForward = armFeedForward.calculate(setPoint.position, setPoint.velocity); if(!isEmergencyStop()) From 590eb1e785f3a67a06aa197c074253c4966c6677 Mon Sep 17 00:00:00 2001 From: isaacngkh Date: Mon, 1 Apr 2024 21:10:39 -0700 Subject: [PATCH 25/59] adjusted set up shuffleboard --- .../java/frc/robot/SetupShuffleboard.java | 38 +++++++++---------- .../driveCommands/StraightenWheelCommand.java | 5 +-- 2 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 7c280174..2f2b1ac4 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -77,19 +77,19 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); - Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); - Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); + Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); + Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); - Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); + Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); - Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); - - Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); + Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); + + Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); - Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); + Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); } @@ -100,15 +100,15 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("GameTab").add("Autonomous Chooser", chooser) .withSize(2, 1) .withPosition(0, 2); - Shuffleboard.getTab("GameTab").add("Disable Pose Estimator", new toggleLimelightPoseEstimation(limelightSubsystem)) - .withSize(1,1) - .withPosition(2,2); - Shuffleboard.getTab("GameTab").add("Toggle Camera Mode", new toggleCameraMode(limelightSubsystem)) - .withSize(1,1) - .withPosition(1,3); + // Shuffleboard.getTab("GameTab").add("Disable Pose Estimator", new toggleLimelightPoseEstimation(limelightSubsystem)) + // .withSize(1,1) + // .withPosition(2,2); + // Shuffleboard.getTab("GameTab").add("Toggle Camera Mode", new toggleCameraMode(limelightSubsystem)) + // .withSize(1,1) + // .withPosition(1,3); - Shuffleboard.getTab("GameTab").addBoolean("Note In Pizza Box", ()-> pizzaBoxSubsystem.hasNote) - .withPosition(0,3); + // Shuffleboard.getTab("GameTab").addBoolean("Note In Pizza Box", ()-> pizzaBoxSubsystem.hasNote) + // .withPosition(0,3); Shuffleboard.getTab("GameTab").addBoolean("Arm Running", ()-> !armSubsystem.isEmergencyStop()) .withSize(1, 1) @@ -124,7 +124,7 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("GameTab").add("Reset Intake", new IntakeResetArm(intakeSubsystem)) .withSize(1,1) .withPosition(4,1); - Shuffleboard.getTab("GameTab").add("IntakeRejectNote", new IntakeRejectNote(intakeSubsystem)) + Shuffleboard.getTab("GameTab").add("Intake Reject Note", new IntakeRejectNote(intakeSubsystem)) .withPosition(4,2); Shuffleboard.getTab("GameTab").add("Extend Reaction Bar", new Extend(reactionSubsystem)) .withPosition(3, 3); @@ -138,7 +138,7 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("GameTab").add("RESET TELEOP DRIVE", new ResetTeleopDrive(teleopDrive)).withPosition(9,0); Shuffleboard.getTab("GameTab").add("Straighten", new StraightenWheelCommand(swerve)).withPosition(9, 1); - Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); + // Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); diff --git a/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java index a926e613..6c09ba09 100644 --- a/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java +++ b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java @@ -4,7 +4,7 @@ package frc.robot.commands.driveCommands; -import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -20,8 +20,7 @@ public StraightenWheelCommand(SwerveSubsystem m_Subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - ChassisSpeeds speed = new ChassisSpeeds(1,0,0); - m_Subsystem.driveFieldOriented(speed); + m_Subsystem.drive(new Translation2d(1, 0), 0, false); } // Called every time the scheduler runs while the command is scheduled. From 2de258911e335cd725bc1a3fab08e462f2029847 Mon Sep 17 00:00:00 2001 From: Isaac Ng Date: Mon, 1 Apr 2024 21:36:53 -0700 Subject: [PATCH 26/59] change arm volt contraints --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 86d82a4f..a0f41f07 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -117,10 +117,6 @@ else if (speed > Arm.MAX_VOLT) { { speed = Arm.MAX_VOLT; } - - if(encoderGetAngle() < 0) { - speed = -5; - } if(encoderGetAngle() >= 300) { if (speed > 0.5) { @@ -128,6 +124,12 @@ else if (speed > Arm.MAX_VOLT) { } } + if(encoderGetAngle() >= 332) { + speed = -1; + } + + SmartDashboard.putNumber("Arm speed", speed); + VoltageOut armSpinRequest = new VoltageOut(speed, true, false, false, false); m_arm.setControl(armSpinRequest); } @@ -177,7 +179,6 @@ public void useOutput(double output, State setPoint) { SmartDashboard.putNumber("Arm PID output", output); SmartDashboard.putNumber("Arm feed forward", feedForward); - SmartDashboard.putNumber("Arm speed", output + feedForward); SmartDashboard.putNumber("Arm FF setPoint Position", setPoint.position * 180 / Math.PI); SmartDashboard.putNumber("Arm FF setPoint velocity", setPoint.velocity * 180 / Math.PI); spinArm(output + feedForward); From e054e1b24da41d165d180bdb6abda3b0c8039a72 Mon Sep 17 00:00:00 2001 From: Isaac Ng Date: Mon, 1 Apr 2024 21:43:40 -0700 Subject: [PATCH 27/59] reaction bar fixed --- src/main/java/frc/robot/subsystems/ReactionSubsystem.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java index cf8ff509..d207b316 100644 --- a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + import com.ctre.phoenix6.signals.NeutralModeValue; public class ReactionSubsystem extends SubsystemBase { @@ -24,7 +26,7 @@ public ReactionSubsystem(int armID, String canbus) { MotorOutputConfigs motorOutput = new MotorOutputConfigs(); motorOutput.NeutralMode = NeutralModeValue.Brake; m_reactionArm.getConfigurator().apply(motorOutput); - + PIDcontroller.setTolerance(Constants.ReactionConstants.tolerance); } @Override @@ -37,7 +39,7 @@ public void periodic() { if (PIDvalue < -1) PIDvalue = -1; - + m_reactionArm.set(PIDvalue); } public void spinForward() From d3015bd92ce98bb0a2e9d3d550eaa82d75df1f9f Mon Sep 17 00:00:00 2001 From: Isaac Ng Date: Mon, 1 Apr 2024 22:06:45 -0700 Subject: [PATCH 28/59] fix servo --- src/main/java/frc/robot/Constants.java | 1 + .../robot/commands/ClimberCommands/ClimbParts/PrepClimb.java | 4 +++- .../robot/commands/ClimberCommands/ClimbParts/UnClimb.java | 3 ++- .../ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java | 4 +++- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3db07837..e48bd530 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -84,6 +84,7 @@ public static final class ClimbConstants { public static final int TOP_LEFT_LIMIT_ID = 5; public static final int CLIMB_ARM_ANGLE = 255; + public static final int CLIMB_ARM_ARNGLE_FOR_SERVO = 270; } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index c2b71468..f71c3055 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; import frc.robot.commands.Arm.SwingForwardServoTheSecond; @@ -27,8 +28,9 @@ public class PrepClimb extends SequentialCommandGroup { public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), - new SpinToArmAngle(a, 270).withTimeout(2), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(2), new SwingForwardServoTheSecond(p), + new WaitCommand(0.5), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorUp(c, a).withTimeout(5), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java index 094562a5..f2d5c8ed 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; import frc.robot.commands.ReactionArmCommands.Retract; @@ -27,7 +28,7 @@ public UnClimb(Climbsubsystem c, ArmSubsystem a, ReactionSubsystem r) { // addCommands(new FooCommand(), new BarCommand()); addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimb")), - new SpinToArmAngle(a, 250).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorUp(c, a).withTimeout(5), new Retract(r).withTimeout(1), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index 50078c84..b3d6fd4b 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; import frc.robot.commands.Arm.SwingBackServoTheSecond; @@ -32,8 +33,9 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).withTimeout(3), - new SpinToArmAngle(a, 270), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO), new SwingBackServoTheSecond(p), + new WaitCommand(0.5), new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) ); From ac0c15a797e6fc33b7a6c7c012a0baa4623576a5 Mon Sep 17 00:00:00 2001 From: Isaac Ng Date: Tue, 2 Apr 2024 00:15:25 -0700 Subject: [PATCH 29/59] fix intake not moving --- src/main/java/frc/robot/GameRobotContainer.java | 7 +++---- src/main/java/frc/robot/commands/Arm/ResetArm.java | 8 ++++++-- .../frc/robot/commands/IntakeCommands/IntakeResetArm.java | 6 ++++-- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index f4a2f4b3..45c9f600 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -191,13 +191,12 @@ public void setMotorBrake(boolean brake) } public Command teleopInitReset() { - return new SwingBackServoTheSecond(m_PizzaBoxSubsystem) - .andThen(new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem)) - .andThen(new IntakeResetArm(m_IntakeSubsystem)); + return new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem).withTimeout(3) + .alongWith(new IntakeResetArm(m_IntakeSubsystem)).withTimeout(3); } public Command autoInitReset() { - return new IntakeResetArm(m_IntakeSubsystem); + return new IntakeResetArm(m_IntakeSubsystem).withTimeout(3); } } diff --git a/src/main/java/frc/robot/commands/Arm/ResetArm.java b/src/main/java/frc/robot/commands/Arm/ResetArm.java index 850464c8..f81a24eb 100644 --- a/src/main/java/frc/robot/commands/Arm/ResetArm.java +++ b/src/main/java/frc/robot/commands/Arm/ResetArm.java @@ -2,6 +2,7 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.PizzaBoxSubsystem; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -19,7 +20,8 @@ public ResetArm(ArmSubsystem armSubsystem, PizzaBoxSubsystem pizzaBoxSubsystem) } - public void initialize() { + public void initialize() { + DataLogManager.log("Command Start: Reset Arm"); Command c = CommandScheduler.getInstance().requiring(armSubsystem); if(c != null) { c.cancel(); @@ -32,8 +34,9 @@ public void initialize() { armSubsystem.stopArmMotor(); pizzaBoxSubsystem.stopPizzaBoxMotor(); - new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).schedule(); + armSubsystem.targetArmAngle(ArmSubsystem.Arm.INTAKE_ANGLE); pizzaBoxSubsystem.setServoAngle(50); + pizzaBoxSubsystem.setServoAngleTheSecond(0); } @@ -46,6 +49,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + DataLogManager.log("Command End: Reset Arm"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java index c3a14767..3da6be15 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java @@ -6,6 +6,7 @@ import frc.robot.Constants; import frc.robot.subsystems.IntakeSubsystem; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -21,6 +22,7 @@ public IntakeResetArm(IntakeSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { + DataLogManager.log("Command Start: Reset Intake Arm"); Command i = CommandScheduler.getInstance().requiring(intakeSubsystem); if(i != null) { @@ -29,7 +31,7 @@ public void initialize() { intakeSubsystem.stopArmMotor(); intakeSubsystem.stopIntakeMotors(); - new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION).schedule(); + new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION).withTimeout(3).schedule(); } // Called every time the scheduler runs while the command is scheduled. @@ -42,7 +44,7 @@ public void execute() { // runs once when isFinished is called @Override public void end(boolean interrupted) { - + DataLogManager.log("Command End: Reset Intake Arm"); } // Returns true when the command should end. From 1a41b18e0423842a8996439c754d1611892e4090 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Tue, 2 Apr 2024 12:38:17 -0700 Subject: [PATCH 30/59] tested and almost work except swinging servo back --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/GameRobotContainer.java | 5 +++++ .../robot/commands/ClimberCommands/ClimbParts/PrepClimb.java | 2 +- src/main/java/frc/robot/subsystems/ReactionSubsystem.java | 4 ++-- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e48bd530..21942ced 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -84,7 +84,7 @@ public static final class ClimbConstants { public static final int TOP_LEFT_LIMIT_ID = 5; public static final int CLIMB_ARM_ANGLE = 255; - public static final int CLIMB_ARM_ARNGLE_FOR_SERVO = 270; + public static final int CLIMB_ARM_ARNGLE_FOR_SERVO = 330; } diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 45c9f600..2f77b68c 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -14,6 +14,8 @@ import frc.robot.commands.Arm.*; import frc.robot.commands.ClimberCommands.ClimbParts.*; import frc.robot.commands.IntakeCommands.*; +import frc.robot.commands.ReactionArmCommands.Extend; +import frc.robot.commands.ReactionArmCommands.Retract; import frc.robot.commands.driveCommands.*; import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.*; @@ -126,6 +128,9 @@ private void configureBindings() { GenericEntry s = Shuffleboard.getTab("Arm").add("Angle", 236).getEntry(); operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->s.getDouble(236))); + Shuffleboard.getTab("Arm").add("Extend", new Extend(m_ReactionSubsystem)); + Shuffleboard.getTab("Arm").add("Retract", new Retract(m_ReactionSubsystem)); + Shuffleboard.getTab("Arm").addDouble("Reaction Bar Angle", ()-> m_ReactionSubsystem.getPos()).withPosition(9, 4); } private void configurePathPlannerCommands() { diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index f71c3055..a7d598f7 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -28,7 +28,7 @@ public class PrepClimb extends SequentialCommandGroup { public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), - new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(2), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(4), new SwingForwardServoTheSecond(p), new WaitCommand(0.5), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), diff --git a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java index d207b316..ccf4bb27 100644 --- a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java @@ -15,7 +15,7 @@ public class ReactionSubsystem extends SubsystemBase { private TalonFX m_reactionArm; - private final double REACTION_BAR_PID_KP = 0.8; + private final double REACTION_BAR_PID_KP = 0.09; private final double REACTION_BAR_PID_KI = 0; private final double REACTION_BAR_PID_KD = 0; public PIDController PIDcontroller = new PIDController(REACTION_BAR_PID_KP, REACTION_BAR_PID_KI, REACTION_BAR_PID_KD); @@ -48,7 +48,7 @@ public void spinForward() } public void spinBackward() { - PIDcontroller.setSetpoint(0); + PIDcontroller.setSetpoint(-0.1); } public void stop() { From 37918b1a85e4544e87415c3a22d372f3addd8a18 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Tue, 2 Apr 2024 15:45:12 -0700 Subject: [PATCH 31/59] slight changes --- .../UnClimbPartTwoThatWillBringDownTheMotor.java | 2 +- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index b3d6fd4b..b3b60aab 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -33,7 +33,7 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).withTimeout(3), - new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(2.5), new SwingBackServoTheSecond(p), new WaitCommand(0.5), new SpinToArmAngle(a, Arm.INTAKE_ANGLE), diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index a0f41f07..79ecfb92 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -113,11 +113,6 @@ else if (speed > Arm.MAX_VOLT) { speed = Arm.MAX_VOLT; } - if(booster) - { - speed = Arm.MAX_VOLT; - } - if(encoderGetAngle() >= 300) { if (speed > 0.5) { speed = 0.5; @@ -128,6 +123,11 @@ else if (speed > Arm.MAX_VOLT) { speed = -1; } + if(booster) + { + speed = Arm.MAX_VOLT; + } + SmartDashboard.putNumber("Arm speed", speed); VoltageOut armSpinRequest = new VoltageOut(speed, true, false, false, false); From f7a53aa75ad29edc955baa43da3cf20abe8d3001 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Tue, 2 Apr 2024 15:45:44 -0700 Subject: [PATCH 32/59] timing --- .../ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index b3d6fd4b..4aea035b 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -30,7 +30,7 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimbPartTwo")), - new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(0.5), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).withTimeout(3), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO), From e5145a468c1dde5a3855d1cf77aaf1052c0dd73e Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Tue, 2 Apr 2024 16:00:28 -0700 Subject: [PATCH 33/59] . --- src/main/java/frc/robot/GameRobotContainer.java | 8 +------- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 2f77b68c..af89b2fd 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -49,8 +49,6 @@ public String getDriveTrainName(){ public GameRobotContainer() { - // LimelightHelpers.setStreamMode_PiPSecondary("limelight"); - // Shuffleboard.getTab("GameTab").addCamera("Vision", "limelight", "http://limelight.local:5800").withSize(4,3).withPosition(5, 0); m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), getDriveTrainName())); m_IntakeSubsystem = new IntakeSubsystem(Constants.IntakeConstants.INTAKE_LOWER_INTAKE_ID,Constants.IntakeConstants.INTAKE_SPIN_MOTOR_ID, "rio"); @@ -73,7 +71,7 @@ public GameRobotContainer() { m_ReactionSubsystem = new ReactionSubsystem(Constants.ReactionConstants.reactionID, Constants.ReactionConstants.reactionCAN); - closedFieldRel = new TeleopDrive( + closedFieldRel = new TeleopDrive( m_drivebase, () -> MathUtil.applyDeadband(-driverController.getRawAxis(1), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(-driverController.getRawAxis(0), OperatorConstants.LEFT_X_DEADBAND), @@ -127,10 +125,6 @@ private void configureBindings() { operatorController.leftStick().whileTrue(new LockHeadingToSourceForIntake(closedFieldRel, m_ArmSubsystem, m_PizzaBoxSubsystem)); GenericEntry s = Shuffleboard.getTab("Arm").add("Angle", 236).getEntry(); operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->s.getDouble(236))); - - Shuffleboard.getTab("Arm").add("Extend", new Extend(m_ReactionSubsystem)); - Shuffleboard.getTab("Arm").add("Retract", new Retract(m_ReactionSubsystem)); - Shuffleboard.getTab("Arm").addDouble("Reaction Bar Angle", ()-> m_ReactionSubsystem.getPos()).withPosition(9, 4); } private void configurePathPlannerCommands() { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 79ecfb92..af9d2893 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -114,8 +114,8 @@ else if (speed > Arm.MAX_VOLT) { } if(encoderGetAngle() >= 300) { - if (speed > 0.5) { - speed = 0.5; + if (speed > 0.8) { + speed = 0.8; } } From 807d4a2fd0c3014fd5ef7ef910483a837d1065f3 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Tue, 2 Apr 2024 16:02:35 -0700 Subject: [PATCH 34/59] clean up --- src/main/java/frc/robot/SetupShuffleboard.java | 10 +++------- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 4 ++++ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 2f2b1ac4..7188aaf7 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -94,9 +94,9 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p } - Shuffleboard.getTab("GameTab").add("Field", swerve.getField2d()) - .withSize(3, 2) - .withPosition(0, 0); + // Shuffleboard.getTab("GameTab").add("Field", swerve.getField2d()) + // .withSize(3, 2) + // .withPosition(0, 0); Shuffleboard.getTab("GameTab").add("Autonomous Chooser", chooser) .withSize(2, 1) .withPosition(0, 2); @@ -111,18 +111,14 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p // .withPosition(0,3); Shuffleboard.getTab("GameTab").addBoolean("Arm Running", ()-> !armSubsystem.isEmergencyStop()) - .withSize(1, 1) .withPosition(3, 0); Shuffleboard.getTab("GameTab").addBoolean("Intake Running", ()-> !intakeSubsystem.isEmergencyStop()) - .withSize(1, 1) .withPosition(4, 0); Shuffleboard.getTab("GameTab").add("Reset Arm", new ResetArm(armSubsystem, pizzaBoxSubsystem) ) - .withSize(1,1) .withPosition(3,1); Shuffleboard.getTab("GameTab").add("Reset Intake", new IntakeResetArm(intakeSubsystem)) - .withSize(1,1) .withPosition(4,1); Shuffleboard.getTab("GameTab").add("Intake Reject Note", new IntakeRejectNote(intakeSubsystem)) .withPosition(4,2); diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index af9d2893..866bded6 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -123,6 +123,10 @@ else if (speed > Arm.MAX_VOLT) { speed = -1; } + if(isEmergencyStop()) { + speed = 0; + } + if(booster) { speed = Arm.MAX_VOLT; From 8a17b764dbff8cacc96163504031115c725f4a41 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Tue, 2 Apr 2024 16:06:13 -0700 Subject: [PATCH 35/59] cpu high --- .pathplanner/settings.json | 4 ++-- src/main/deploy/pathplanner/paths/A1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C2.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-C3.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-S1.path | 4 ++-- src/main/deploy/pathplanner/paths/A1-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C2.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-C3.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-S1.path | 4 ++-- src/main/deploy/pathplanner/paths/A2-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-C5.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/A3-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-S2.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/C4-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/C5-S3.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/S1-Leave.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A1.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A2.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/S2-C1.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-A3.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-C4.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-C5.path | 4 ++-- src/main/deploy/pathplanner/paths/S3-Leave.path | 4 ++-- src/main/deploy/pathplanner/paths/center.path | 4 ++-- src/main/deploy/pathplanner/paths/go to center.path | 4 ++-- 39 files changed, 78 insertions(+), 78 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f13d57fa..f032e175 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -16,8 +16,8 @@ "3. A+A+C", "Hajel" ], - "defaultMaxVel": 1.0, - "defaultMaxAccel": 2.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 30.0, "defaultMaxAngAccel": 30.0, "maxModuleSpeed": 4.0 diff --git a/src/main/deploy/pathplanner/paths/A1-A2.path b/src/main/deploy/pathplanner/paths/A1-A2.path index 6b715bc5..005ab262 100644 --- a/src/main/deploy/pathplanner/paths/A1-A2.path +++ b/src/main/deploy/pathplanner/paths/A1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index ca7af168..c7e17f37 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index 417095c4..ab283d91 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 0a98cf02..8d760412 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index 8c91f4c6..0fe39615 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index a84a6bc6..22000094 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-A1.path b/src/main/deploy/pathplanner/paths/A2-A1.path index be9b85a4..47ce32a1 100644 --- a/src/main/deploy/pathplanner/paths/A2-A1.path +++ b/src/main/deploy/pathplanner/paths/A2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-A3.path b/src/main/deploy/pathplanner/paths/A2-A3.path index 6ebe8d7e..5400ae87 100644 --- a/src/main/deploy/pathplanner/paths/A2-A3.path +++ b/src/main/deploy/pathplanner/paths/A2-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C1.path b/src/main/deploy/pathplanner/paths/A2-C1.path index f71bdb3f..d2f61569 100644 --- a/src/main/deploy/pathplanner/paths/A2-C1.path +++ b/src/main/deploy/pathplanner/paths/A2-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C2.path b/src/main/deploy/pathplanner/paths/A2-C2.path index 16b96f07..bfc91177 100644 --- a/src/main/deploy/pathplanner/paths/A2-C2.path +++ b/src/main/deploy/pathplanner/paths/A2-C2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 734c97c4..181f30f1 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index ba09cef2..5abbc2e5 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index e223f74a..edd15666 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-A2.path b/src/main/deploy/pathplanner/paths/A3-A2.path index 28afbc2f..8103b935 100644 --- a/src/main/deploy/pathplanner/paths/A3-A2.path +++ b/src/main/deploy/pathplanner/paths/A3-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index 53a8ccf8..c32f7d26 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index 7c414a79..38ff00f6 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index d4888b68..db503b3e 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-A1.path b/src/main/deploy/pathplanner/paths/C1-A1.path index b07be142..19f43e0d 100644 --- a/src/main/deploy/pathplanner/paths/C1-A1.path +++ b/src/main/deploy/pathplanner/paths/C1-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-A2.path b/src/main/deploy/pathplanner/paths/C1-A2.path index 1c778d45..2a2d64e6 100644 --- a/src/main/deploy/pathplanner/paths/C1-A2.path +++ b/src/main/deploy/pathplanner/paths/C1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index b754e91f..07f1ce5a 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 72cbddd4..087bce8e 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C2-A2.path b/src/main/deploy/pathplanner/paths/C2-A2.path index 48a448c5..d0272801 100644 --- a/src/main/deploy/pathplanner/paths/C2-A2.path +++ b/src/main/deploy/pathplanner/paths/C2-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index dbdc1533..83706600 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index 8d8e5eb6..7e91716e 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index cf9935a9..0ba10c2a 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index 0ab17111..b4c2a2a6 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index 3bed5a03..55651a77 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index ea4c7bae..d50f07d7 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index 1d95cffc..58155519 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index d1af5fc8..06805062 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index 85966e85..a17e6e40 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index 3572d908..84e95bf2 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index 0b63b706..0b358e4f 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index 0a89bc1b..3da78039 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index 29864531..87b65c6f 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index a509e863..dcfaa151 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/center.path b/src/main/deploy/pathplanner/paths/center.path index 8985c86a..9853c26f 100644 --- a/src/main/deploy/pathplanner/paths/center.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, diff --git a/src/main/deploy/pathplanner/paths/go to center.path b/src/main/deploy/pathplanner/paths/go to center.path index 54460539..0e629d7c 100644 --- a/src/main/deploy/pathplanner/paths/go to center.path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 30.0, "maxAngularAcceleration": 30.0 }, From 4921a4a9076826d65da8ce7ae4253936db4616a2 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Tue, 2 Apr 2024 16:30:03 -0700 Subject: [PATCH 36/59] . --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 6 +++--- src/main/java/frc/robot/subsystems/Climbsubsystem.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 866bded6..e469bb89 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -45,7 +45,7 @@ public static final class Arm { public static final double KD = 0; public static final double KSVOLTS = 1.5; public static final double KGVOLTS = 0; - public static final double KVVOLTS = 1.8; + public static final double KVVOLTS = 1.5; public static final double KAVOLTS = 0; public static final double VEL = 230 * Math.PI / 180; public static final double ACC = 360 * Math.PI / 180; @@ -114,8 +114,8 @@ else if (speed > Arm.MAX_VOLT) { } if(encoderGetAngle() >= 300) { - if (speed > 0.8) { - speed = 0.8; + if (speed > 1) { + speed = 1; } } diff --git a/src/main/java/frc/robot/subsystems/Climbsubsystem.java b/src/main/java/frc/robot/subsystems/Climbsubsystem.java index 30f631a0..ddc483ab 100644 --- a/src/main/java/frc/robot/subsystems/Climbsubsystem.java +++ b/src/main/java/frc/robot/subsystems/Climbsubsystem.java @@ -173,10 +173,10 @@ public void periodic() { rightPIDvalue = 0; } - if (!checkForUp && getBotLeftLimit()) { + if (!checkForUp && getBotLeftLimit() && getPositionLeft() > -15) { leftPIDvalue = 0; } - if (!checkForUp && getBotRightLimit()) { + if (!checkForUp && getBotRightLimit() && getPositionRight() < 15) { rightPIDvalue = 0; } From 78d61c14f9631ef757f51a4f76d60a58df69a106 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Wed, 3 Apr 2024 08:39:13 -0700 Subject: [PATCH 37/59] testing --- .pathplanner/settings.json | 5 +- .../deploy/pathplanner/autos/2N-S1-A1.auto | 82 ----------- .../deploy/pathplanner/autos/2N-S2-A1.auto | 82 ----------- .../deploy/pathplanner/autos/2N-S3-A3.auto | 82 ----------- .../deploy/pathplanner/autos/3N-S1-allA.auto | 4 +- .../deploy/pathplanner/autos/3N-S2-C1.auto | 2 +- .../deploy/pathplanner/autos/3N-S2-C2.auto | 2 +- .../deploy/pathplanner/autos/3N-S3-allA.auto | 4 +- ...Copy of Copy of Hajel middle bottom 2.auto | 139 ++++++++++++++++++ ...uto => Copy of Hajel middle bottom 2.auto} | 31 +++- .../autos/Hajel middle bottom 2.auto | 21 ++- .../pathplanner/autos/Hajel middle top 2.auto | 21 ++- src/main/deploy/pathplanner/paths/A1-A2.path | 12 +- src/main/deploy/pathplanner/paths/A1-C1.path | 12 +- src/main/deploy/pathplanner/paths/A1-C2.path | 12 +- src/main/deploy/pathplanner/paths/A1-C3.path | 12 +- src/main/deploy/pathplanner/paths/A1-S1.path | 12 +- src/main/deploy/pathplanner/paths/A1-S2.path | 4 +- src/main/deploy/pathplanner/paths/A2-A1.path | 12 +- src/main/deploy/pathplanner/paths/A2-A3.path | 4 +- src/main/deploy/pathplanner/paths/A2-C1.path | 4 +- src/main/deploy/pathplanner/paths/A2-C2.path | 4 +- src/main/deploy/pathplanner/paths/A2-C3.path | 4 +- src/main/deploy/pathplanner/paths/A2-S1.path | 4 +- src/main/deploy/pathplanner/paths/A2-S2.path | 4 +- src/main/deploy/pathplanner/paths/A3-A1.path | 52 +++++++ src/main/deploy/pathplanner/paths/A3-A2.path | 4 +- src/main/deploy/pathplanner/paths/A3-C5.path | 4 +- src/main/deploy/pathplanner/paths/A3-S2.path | 4 +- src/main/deploy/pathplanner/paths/A3-S3.path | 4 +- src/main/deploy/pathplanner/paths/C1-A1.path | 12 +- src/main/deploy/pathplanner/paths/C1-A2.path | 4 +- src/main/deploy/pathplanner/paths/C1-S2.path | 4 +- src/main/deploy/pathplanner/paths/C2-A1.path | 12 +- src/main/deploy/pathplanner/paths/C2-A2.path | 4 +- src/main/deploy/pathplanner/paths/C4-S3.path | 4 +- src/main/deploy/pathplanner/paths/C5-S3.path | 4 +- src/main/deploy/pathplanner/paths/S1-A1.path | 12 +- src/main/deploy/pathplanner/paths/S1-A2.path | 4 +- src/main/deploy/pathplanner/paths/S1-C1.path | 4 +- .../deploy/pathplanner/paths/S1-Leave.path | 4 +- src/main/deploy/pathplanner/paths/S2-A1.path | 12 +- src/main/deploy/pathplanner/paths/S2-A2.path | 4 +- src/main/deploy/pathplanner/paths/S2-A3.path | 4 +- src/main/deploy/pathplanner/paths/S2-C1.path | 4 +- src/main/deploy/pathplanner/paths/S3-A3.path | 4 +- src/main/deploy/pathplanner/paths/S3-C4.path | 4 +- src/main/deploy/pathplanner/paths/S3-C5.path | 4 +- .../deploy/pathplanner/paths/S3-Leave.path | 4 +- src/main/deploy/pathplanner/paths/center.path | 4 +- .../pathplanner/paths/go to center.path | 4 +- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/GameRobotContainer.java | 63 +++----- .../ClimbParts/ClimbAndShoot.java | 2 +- .../ClimberCommands/ClimbParts/UnClimb.java | 2 +- .../PickUpFromGroundAndPassToPizzaBox.java | 2 +- .../commands/driveCommands/rotateinPlace.java | 4 +- .../frc/robot/subsystems/ArmSubsystem.java | 4 +- .../frc/robot/subsystems/IntakeSubsystem.java | 8 +- .../robot/subsystems/ReactionSubsystem.java | 2 +- .../swervedrive/SwerveSubsystem.java | 2 +- 61 files changed, 410 insertions(+), 440 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/2N-S1-A1.auto delete mode 100644 src/main/deploy/pathplanner/autos/2N-S2-A1.auto delete mode 100644 src/main/deploy/pathplanner/autos/2N-S3-A3.auto create mode 100644 src/main/deploy/pathplanner/autos/Copy of Copy of Hajel middle bottom 2.auto rename src/main/deploy/pathplanner/autos/{2N-S2-A3.auto => Copy of Hajel middle bottom 2.auto} (68%) create mode 100644 src/main/deploy/pathplanner/paths/A3-A1.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f032e175..77bb3e68 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -11,14 +11,13 @@ "0.", "1. A", "1. C", - "2. A+A", "2. A+C", "3. A+A+C", "Hajel" ], "defaultMaxVel": 2.0, - "defaultMaxAccel": 1.0, + "defaultMaxAccel": 1.5, "defaultMaxAngVel": 30.0, - "defaultMaxAngAccel": 30.0, + "defaultMaxAngAccel": 50.0, "maxModuleSpeed": 4.0 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto b/src/main/deploy/pathplanner/autos/2N-S1-A1.auto deleted file mode 100644 index c8fc7743..00000000 --- a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 6.67 - }, - "rotation": 60.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S1)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S1-A1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A1-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A2)" - } - } - ] - } - }, - "folder": "2. A+A", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto deleted file mode 100644 index ace2e107..00000000 --- a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.33, - "y": 5.54 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S2) then Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "S2-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A2)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A2-A1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A1)" - } - } - ] - } - }, - "folder": "2. A+A", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S3-A3.auto b/src/main/deploy/pathplanner/autos/2N-S3-A3.auto deleted file mode 100644 index 1bf46ea7..00000000 --- a/src/main/deploy/pathplanner/autos/2N-S3-A3.auto +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 4.44 - }, - "rotation": -60.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S3)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S3-A3" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (A3-A2) then Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A3-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A2)" - } - } - ] - } - }, - "folder": "2. A+A", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-S1-allA.auto b/src/main/deploy/pathplanner/autos/3N-S1-allA.auto index 20c75cc7..5051e4f9 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-allA.auto +++ b/src/main/deploy/pathplanner/autos/3N-S1-allA.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": null } }, { @@ -75,7 +75,7 @@ { "type": "named", "data": { - "name": "Speaker (A2-A3) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S2-C1.auto b/src/main/deploy/pathplanner/autos/3N-S2-C1.auto index a6ffe5a6..063f1cbe 100644 --- a/src/main/deploy/pathplanner/autos/3N-S2-C1.auto +++ b/src/main/deploy/pathplanner/autos/3N-S2-C1.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A2-A1) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S2-C2.auto b/src/main/deploy/pathplanner/autos/3N-S2-C2.auto index 7e6e3c32..0640d011 100644 --- a/src/main/deploy/pathplanner/autos/3N-S2-C2.auto +++ b/src/main/deploy/pathplanner/autos/3N-S2-C2.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A2-A1) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S3-allA.auto b/src/main/deploy/pathplanner/autos/3N-S3-allA.auto index a7c3fcd8..dcdcfdf6 100644 --- a/src/main/deploy/pathplanner/autos/3N-S3-allA.auto +++ b/src/main/deploy/pathplanner/autos/3N-S3-allA.auto @@ -50,7 +50,7 @@ { "type": "named", "data": { - "name": "Speaker (A3-A2) then Intake" + "name": null } }, { @@ -82,7 +82,7 @@ { "type": "named", "data": { - "name": "Speaker (A2-A1) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel middle bottom 2.auto b/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel middle bottom 2.auto new file mode 100644 index 00000000..020b5077 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel middle bottom 2.auto @@ -0,0 +1,139 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.54 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker (S2) then Intake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.75 + } + }, + { + "type": "path", + "data": { + "pathName": "S2-A2" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "A2-S2" + } + }, + { + "type": "named", + "data": { + "name": "Speaker (S2)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S2-A3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.65 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker (A3)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A3-A1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker (A1)" + } + } + ] + } + }, + "folder": "Hajel", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto b/src/main/deploy/pathplanner/autos/Copy of Hajel middle bottom 2.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/2N-S2-A3.auto rename to src/main/deploy/pathplanner/autos/Copy of Hajel middle bottom 2.auto index 3f12da71..d4af2fbf 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto +++ b/src/main/deploy/pathplanner/autos/Copy of Hajel middle bottom 2.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 2.75 } }, { @@ -43,10 +43,16 @@ ] } }, + { + "type": "path", + "data": { + "pathName": "A2-S2" + } + }, { "type": "named", "data": { - "name": "Speaker (A2)" + "name": "Speaker (S2)" } }, { @@ -56,13 +62,26 @@ { "type": "path", "data": { - "pathName": "A2-A3" + "pathName": "S2-A3" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "Intake" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.65 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] @@ -77,6 +96,6 @@ ] } }, - "folder": "2. A+A", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto b/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto index 92d684ec..82f8449a 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto @@ -60,15 +60,28 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Intake" + "pathName": "S2-A3" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "S2-A3" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.65 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto b/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto index d0758a7a..cb1259b8 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto @@ -60,15 +60,28 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Intake" + "pathName": "S2-A1" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "S2-A1" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.65 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/A1-A2.path b/src/main/deploy/pathplanner/paths/A1-A2.path index 005ab262..6e3818d2 100644 --- a/src/main/deploy/pathplanner/paths/A1-A2.path +++ b/src/main/deploy/pathplanner/paths/A1-A2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 1.6067905459634015, - "y": 6.710382806138379 + "x": 1.570508221490355, + "y": 6.940041387424158 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index c7e17f37..0891e5c7 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 + "x": 2.6, + "y": 6.979658581285779 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index ab283d91..5b1ca3ef 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 4.0362823244730475, - "y": 6.75 + "x": 4.000000000000001, + "y": 6.979658581285779 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 8d760412..7ad89d2b 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 + "x": 2.6, + "y": 6.979658581285779 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index 0fe39615..9d35937b 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 2.5220300737463233, - "y": 6.735747749273277 + "x": 2.4857477492732767, + "y": 6.965406330559056 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index 22000094..ad5324e5 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-A1.path b/src/main/deploy/pathplanner/paths/A2-A1.path index 47ce32a1..d9d7ffa8 100644 --- a/src/main/deploy/pathplanner/paths/A2-A1.path +++ b/src/main/deploy/pathplanner/paths/A2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": { - "x": 1.7101812358942634, - "y": 6.424116753996703 + "x": 1.6738989114212168, + "y": 6.653775335282482 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-A3.path b/src/main/deploy/pathplanner/paths/A2-A3.path index 5400ae87..dd621e09 100644 --- a/src/main/deploy/pathplanner/paths/A2-A3.path +++ b/src/main/deploy/pathplanner/paths/A2-A3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C1.path b/src/main/deploy/pathplanner/paths/A2-C1.path index d2f61569..079a5456 100644 --- a/src/main/deploy/pathplanner/paths/A2-C1.path +++ b/src/main/deploy/pathplanner/paths/A2-C1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C2.path b/src/main/deploy/pathplanner/paths/A2-C2.path index bfc91177..abc3e484 100644 --- a/src/main/deploy/pathplanner/paths/A2-C2.path +++ b/src/main/deploy/pathplanner/paths/A2-C2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 181f30f1..7e7757ee 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index 5abbc2e5..cf0a423b 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index edd15666..1332838e 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-A1.path b/src/main/deploy/pathplanner/paths/A3-A1.path new file mode 100644 index 00000000..c45c936d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A3-A1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.3 + }, + "prevControl": null, + "nextControl": { + "x": 1.548236363234494, + "y": 4.801442983770418 + }, + "isLocked": false, + "linkedName": "A3" + }, + { + "anchor": { + "x": 2.5, + "y": 6.979658581285779 + }, + "prevControl": { + "x": 1.83985746204333, + "y": 6.588642825645138 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "A1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 30.0, + "maxAngularAcceleration": 50.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": true + }, + "reversed": false, + "folder": "Notes Near Starting Zone", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A3-A2.path b/src/main/deploy/pathplanner/paths/A3-A2.path index 8103b935..ffbbb15a 100644 --- a/src/main/deploy/pathplanner/paths/A3-A2.path +++ b/src/main/deploy/pathplanner/paths/A3-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index c32f7d26..119340b4 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index 38ff00f6..f649a1e2 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index db503b3e..4e06e7d8 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-A1.path b/src/main/deploy/pathplanner/paths/C1-A1.path index 19f43e0d..45b1125c 100644 --- a/src/main/deploy/pathplanner/paths/C1-A1.path +++ b/src/main/deploy/pathplanner/paths/C1-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": { - "x": 2.6347630997742675, - "y": 6.767364817766693 + "x": 2.598480775301221, + "y": 6.997023399052472 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-A2.path b/src/main/deploy/pathplanner/paths/C1-A2.path index 2a2d64e6..73db4f8b 100644 --- a/src/main/deploy/pathplanner/paths/C1-A2.path +++ b/src/main/deploy/pathplanner/paths/C1-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index 07f1ce5a..56c836b4 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 087bce8e..48dd40e0 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": { - "x": 2.6347630997742675, - "y": 6.732635182233307 + "x": 2.598480775301221, + "y": 6.962293763519086 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2-A2.path b/src/main/deploy/pathplanner/paths/C2-A2.path index d0272801..2bbd6457 100644 --- a/src/main/deploy/pathplanner/paths/C2-A2.path +++ b/src/main/deploy/pathplanner/paths/C2-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index 83706600..4adb1d56 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index 7e91716e..550ac00c 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index 0ba10c2a..9422d857 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": { - "x": 2.155193024633961, - "y": 6.412405917450922 + "x": 2.118910700160914, + "y": 6.642064498736701 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index b4c2a2a6..693d6422 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index 55651a77..65ff9ffe 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index d50f07d7..45d5a355 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index 58155519..6d39f14f 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.5, + "y": 6.979658581285779 }, "prevControl": { - "x": 2.4362823244730465, - "y": 6.75 + "x": 2.4, + "y": 6.979658581285779 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index 06805062..1b6cdd17 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index a17e6e40..9bd21dc8 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index 84e95bf2..1b0ba2d7 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index 0b358e4f..a1abfd7e 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index 3da78039..35b0abc1 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index 87b65c6f..d1567e42 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index dcfaa151..1eeb7501 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/center.path b/src/main/deploy/pathplanner/paths/center.path index 9853c26f..8a0c55c8 100644 --- a/src/main/deploy/pathplanner/paths/center.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/go to center.path b/src/main/deploy/pathplanner/paths/go to center.path index 0e629d7c..2d3ae087 100644 --- a/src/main/deploy/pathplanner/paths/go to center.path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxAngularAcceleration": 50.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 21942ced..866926e1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -98,7 +98,7 @@ public static final class IntakeConstants { public static final double ROTATION_TO_DEGREES = 360; public static final double ENCODER_RAW_TO_ROTATION = 8132.; public static double ENCODER_OFFSET = 55.5; //need to check the encoder value - public static final double UP_POSITION = 79; + public static final double UP_POSITION = 80; public static final double SOURCE_INTAKE_ANGLE = 160; } diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index af89b2fd..b2246c58 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -80,7 +80,7 @@ public GameRobotContainer() { configurePathPlannerCommands(); - autoChooser = AutoBuilder.buildAutoChooser("2N-S2-A3"); + autoChooser = AutoBuilder.buildAutoChooser("Copy of Hajel middle bottom 2"); m_drivebase.setDefaultCommand(closedFieldRel); @@ -129,50 +129,30 @@ private void configureBindings() { private void configurePathPlannerCommands() { - NamedCommands.registerCommand("Wait (half a second)", new WaitCommand(0.5)); - NamedCommands.registerCommand("Wait (one second)", new WaitCommand(1)); + NamedCommands.registerCommand("Intake", new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem)); - NamedCommands.registerCommand("Intake", new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem)); + NamedCommands.registerCommand("Speaker (underhand)", new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); + NamedCommands.registerCommand("Speaker (subwoofer)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 227.22)); - NamedCommands.registerCommand("Amp", new ScoreInAmp(m_PizzaBoxSubsystem, m_ArmSubsystem)); + NamedCommands.registerCommand("Speaker (A1)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), + new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); + NamedCommands.registerCommand("Speaker (A2)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), + new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); + NamedCommands.registerCommand("Speaker (A3)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), + new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); - NamedCommands.registerCommand("Speaker (underhand)", new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); - NamedCommands.registerCommand("Speaker (subwoofer)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 227.22)); + NamedCommands.registerCommand("Speaker (S1)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); + NamedCommands.registerCommand("Speaker (S2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236)); + NamedCommands.registerCommand("Speaker (S3)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); - NamedCommands.registerCommand("Speaker (A1)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248)/*, - new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase)*/)); - NamedCommands.registerCommand("Speaker (A2)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248)/*, - new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase)*/)); - NamedCommands.registerCommand("Speaker (A3)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), - new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); + NamedCommands.registerCommand("Speaker (S1) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) + .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); + NamedCommands.registerCommand("Speaker (S2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236) + .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); + NamedCommands.registerCommand("Speaker (S3) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) + .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (A1-A2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); - NamedCommands.registerCommand("Speaker (A2-A3)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); - - NamedCommands.registerCommand("Speaker (A3-A2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); - NamedCommands.registerCommand("Speaker (A2-A1)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); - - NamedCommands.registerCommand("Speaker (S1)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); - NamedCommands.registerCommand("Speaker (S2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236)); - NamedCommands.registerCommand("Speaker (S3)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); - - NamedCommands.registerCommand("Speaker (S1) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (S2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (S3) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - - NamedCommands.registerCommand("Speaker (A1-A2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (A2-A3) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - - NamedCommands.registerCommand("Speaker (A3-A2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (A2-A1) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - } + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -191,7 +171,8 @@ public void setMotorBrake(boolean brake) public Command teleopInitReset() { return new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem).withTimeout(3) - .alongWith(new IntakeResetArm(m_IntakeSubsystem)).withTimeout(3); + .alongWith(new IntakeResetArm(m_IntakeSubsystem)).withTimeout(3) + .alongWith(new Retract(m_ReactionSubsystem)); } public Command autoInitReset() { diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java index 916c974d..691c8fd4 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java @@ -28,7 +28,7 @@ public ClimbAndShoot(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, PizzaB // Add your commands in the addCommands() call, e.g. addCommands ( Commands.runOnce(()->DataLogManager.log("Command Start: ClimbAndShoot")), - new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(0.5), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).alongWith(new Extend(r)).withTimeout(3), new ScoreInTrapStutter(p, a), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java index f2d5c8ed..465ae9f3 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java @@ -28,7 +28,7 @@ public UnClimb(Climbsubsystem c, ArmSubsystem a, ReactionSubsystem r) { // addCommands(new FooCommand(), new BarCommand()); addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimb")), - new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(0.5), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorUp(c, a).withTimeout(5), new Retract(r).withTimeout(1), diff --git a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java index 48c4d2be..9a9746a6 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java @@ -24,7 +24,7 @@ public PickUpFromGroundAndPassToPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, Ar addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PickUpFromGroundAndPassToPizzaBox")), new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).withTimeout(2) - .alongWith((new IntakePickUpFromGroundPID(intakeSubsystem, 0.8, 0.0).withTimeout(intakeTimeout)) + .alongWith((new IntakePickUpFromGroundPID(intakeSubsystem, 0.7, 0.0).withTimeout(intakeTimeout)) .andThen(new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION))) .andThen(new IntakePassNoteToPizzaBox(intakeSubsystem, pizzaBoxSubsystem).withTimeout(6)) .andThen(Commands.runOnce(() -> { diff --git a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java index 0d45ab53..3e4960de 100644 --- a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java +++ b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java @@ -56,10 +56,10 @@ public void initialize() { public void execute() { currTheta = m_Subsystem.getHeading().getDegrees(); - angleRate = PID.calculate(currTheta) / 4; + angleRate = PID.calculate(currTheta) / 3; m_Subsystem.drive(pose, angleRate, true); - SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate / 4); + SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate / 3); SmartDashboard.putNumber("Rotate In Place Angle curr angle", currTheta); SmartDashboard.putNumber("Rotate In Place Angle goal", PID.getSetpoint()); } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index e469bb89..5bc3b05b 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -40,7 +40,7 @@ public static final class Arm { public static final int MAX_VOLT = 12; // - public static final double KP = 5.8; + public static final double KP = 6; public static final double KI = 0; public static final double KD = 0; public static final double KSVOLTS = 1.5; @@ -56,7 +56,7 @@ public static final class Arm { public static final int TRAP_ANGLE = 270; public static final int SPEAKER_LOW_ANGLE = 165; public static final int SPEAKER_HIGH_ANGLE = 238; - public static final int INTAKE_ANGLE = 60; + public static final int INTAKE_ANGLE = 63; public static final int CLIMBING_ANGLE = 45; } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 87031bfc..7ddec14a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -84,11 +84,11 @@ else if (intakeMotorVelocity < -1) { } if (!emergencyStop) { - // m_spinIntake.set(-intakeMotorVelocity); + m_spinIntake.set(-intakeMotorVelocity); - intakeMotorVelocity *= 100; - spinRequest1 = new VelocityVoltage(-intakeMotorVelocity, 150, true, 0, 0,false, false, false); - m_spinIntake.setControl(spinRequest1); + // intakeMotorVelocity *= 100; + // spinRequest1 = new VelocityVoltage(-intakeMotorVelocity, 150, true, 0, 0,false, false, false); + // m_spinIntake.setControl(spinRequest1); SmartDashboard.putNumber("Intake spin motor speed", intakeMotorVelocity); } diff --git a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java index ccf4bb27..b202bb4e 100644 --- a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java @@ -48,7 +48,7 @@ public void spinForward() } public void spinBackward() { - PIDcontroller.setSetpoint(-0.1); + PIDcontroller.setSetpoint(-0.21); } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index c041e7eb..552ad9ec 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -130,7 +130,7 @@ public void setupPathPlanner() new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class new PIDConstants(1,0,0), // Translation PID constants - new PIDConstants(7.75,0,0), + new PIDConstants(8,0,0), // Rotation PID constants 4.5, // Max module speed, in m/s From 25b16861e5b6bb8e6f111f1c4d5b2c0420eb41b4 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Wed, 3 Apr 2024 15:05:57 -0700 Subject: [PATCH 38/59] more logs --- src/main/java/frc/robot/GameRobotContainer.java | 4 ++-- src/main/java/frc/robot/Robot.java | 5 +++-- src/main/java/frc/robot/SetupShuffleboard.java | 14 +++++++------- .../frc/robot/commands/Arm/SpinToArmAngle.java | 2 -- .../ActuallyMovesMotors/MotorDown.java | 2 -- .../ActuallyMovesMotors/MotorUp.java | 2 -- .../IntakeCommands/IntakePassNoteToPizzaBox.java | 6 ------ .../IntakeCommands/IntakePickUpFromGroundPID.java | 2 +- .../commands/IntakeCommands/SpinIntakePID.java | 2 +- .../commands/driveCommands/rotateinPlace.java | 6 +++--- .../commands/ledcommands/ChangeLEDToGreen.java | 1 - .../swervedrive/drivebase/TeleopDrive.java | 10 ++++++++-- .../java/frc/robot/subsystems/ArmSubsystem.java | 4 ++-- .../subsystems/swervedrive/SwerveSubsystem.java | 8 ++------ 14 files changed, 29 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index b2246c58..1386fa2b 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -132,11 +132,11 @@ private void configurePathPlannerCommands() { NamedCommands.registerCommand("Intake", new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem)); NamedCommands.registerCommand("Speaker (underhand)", new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); - NamedCommands.registerCommand("Speaker (subwoofer)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 227.22)); + NamedCommands.registerCommand("Speaker (subwoofer)", new ScoreInSpeakerHigh(m_PizzaBoxSubsystem, m_ArmSubsystem)); NamedCommands.registerCommand("Speaker (A1)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); - NamedCommands.registerCommand("Speaker (A2)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), + NamedCommands.registerCommand("Speaker (A2)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245), new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); NamedCommands.registerCommand("Speaker (A3)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b160542..c152d0d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -70,8 +70,9 @@ public void robotInit() { // Logger.addDataReceiver(new NT4Publisher()); // Logger.start(); - //SignalLogger.enableAutoLogging(true); - //SignalLogger.start(); + SignalLogger.setPath("/home/lvuser/log/"); + SignalLogger.enableAutoLogging(true); + SignalLogger.start(); DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 7188aaf7..1b3f5e07 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -71,11 +71,6 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Climb").addDouble("climb distance right", () -> climbSubsystem.getPositionRight()).withPosition(1, 0); Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem, armSubsystem)).withPosition(1, 1); Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem, armSubsystem)).withPosition(0, 1); - Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); - Shuffleboard.getTab("Climb").addBoolean("bot right limit", () -> climbSubsystem.getBotRightLimit()).withPosition(3, 1); - Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); - Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); - Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); @@ -83,8 +78,6 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); - - Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); @@ -93,6 +86,13 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p } + + Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); + + Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); + Shuffleboard.getTab("Climb").addBoolean("bot right limit", () -> climbSubsystem.getBotRightLimit()).withPosition(3, 1); + Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); + Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); // Shuffleboard.getTab("GameTab").add("Field", swerve.getField2d()) // .withSize(3, 2) diff --git a/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java b/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java index f0bb4385..69f136c5 100644 --- a/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java +++ b/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java @@ -23,13 +23,11 @@ public SpinToArmAngle(ArmSubsystem armSubsystem, double angle) { // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("Spin to arm angle initialize"); armSubsystem.targetArmAngle(angle); } @Override public void end(boolean interrupted){ - System.out.println("Spin to arm angle finished"); } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java index 9ef2e9a3..4a940233 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java @@ -25,7 +25,6 @@ public MotorDown(Climbsubsystem c, ArmSubsystem a) { @Override public void initialize() { - System.out.println("Initialize motor down"); climbersubsystem.downMotor(); } @@ -36,7 +35,6 @@ public void execute() { @Override public void end(boolean interrupted) { - System.out.println("finish motor down"); } @Override diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java index 0a00914a..0cc41b0a 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java @@ -27,7 +27,6 @@ public MotorUp(Climbsubsystem c, ArmSubsystem a) { @Override public void initialize() { - System.out.println("Initialize motor up"); climbersubsystem.upMotor(); } @@ -38,7 +37,6 @@ public void execute() { @Override public void end(boolean interrupted) { - System.out.println("finish motor up"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java index 49ac8731..e6c0a5c0 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java @@ -31,7 +31,6 @@ public IntakePassNoteToPizzaBox(IntakeSubsystem subsystem, PizzaBoxSubsystem piz // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("Intake pass note to pizzabox initialize"); currentSensorValue = true; intakeSubsystem.spinIntakeMotor(0.8, 100); pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); @@ -47,13 +46,8 @@ public void execute() { // runs once when isFinished is called @Override public void end(boolean interrupted) { - System.out.println("Intake pass note to pizzabox finished"); pizzaBoxSubsystem.stopPizzaBoxMotor(); intakeSubsystem.stopIntakeMotors(); - // if(interrupted) { - // System.out.println("************************************************************************************************************************************************************************************************************************************************************************************"); - // andThen(new IntakeRejectNote(intakeSubsystem)); - // } } // Returns true when the command should end; called every cycle diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java index 3042be6b..6243945a 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java @@ -12,7 +12,7 @@ public class IntakePickUpFromGroundPID extends PIDCommand { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeSubsystem intakeSubsystem; - private static PIDController intakeController = new PIDController(.025, .001, .0); + private static PIDController intakeController = new PIDController(.03, 0, .0); private double velocity; private double accleration; diff --git a/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java b/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java index 024c5a6c..b4a360fe 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java @@ -13,7 +13,7 @@ public class SpinIntakePID extends PIDCommand { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeSubsystem intakeSubsystem; - private static PIDController intakeController = new PIDController(.025, .001, .0); + private static PIDController intakeController = new PIDController(.03, 0, .0); public SpinIntakePID(IntakeSubsystem intakeSubsystem, final double targetAngle) { super(intakeController, ()-> intakeSubsystem.encoderGetAngle(), () -> targetAngle, diff --git a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java index 3e4960de..e9eb88a7 100644 --- a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java +++ b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java @@ -47,7 +47,7 @@ public void initialize() { currTheta = m_Subsystem.getHeading().getDegrees(); PID.setSetpoint(targetTheta); PID.setTolerance(Constants.DriveConstants.THETA_TOLERANCE, Constants.DriveConstants.STEADY_STATE_TOLERANCE); - PID.setPID(Constants.DriveConstants.kP, Constants.DriveConstants.kI, Constants.DriveConstants.kD); + PID.setPID(0.005, 0, 0); PID.enableContinuousInput(-180, 180); } @@ -56,10 +56,10 @@ public void initialize() { public void execute() { currTheta = m_Subsystem.getHeading().getDegrees(); - angleRate = PID.calculate(currTheta) / 3; + angleRate = PID.calculate(currTheta); m_Subsystem.drive(pose, angleRate, true); - SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate / 3); + SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate); SmartDashboard.putNumber("Rotate In Place Angle curr angle", currTheta); SmartDashboard.putNumber("Rotate In Place Angle goal", PID.getSetpoint()); } diff --git a/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java b/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java index 38d24d57..558ff3ae 100644 --- a/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java +++ b/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java @@ -37,7 +37,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - System.out.println("green command executed succesfully"); return ledSubsystem.getColor(1).equals(new Color(0, 255, 0)); } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java index 754503d0..f8abd0ee 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java @@ -93,13 +93,19 @@ public void execute() // Drive using raw values. if(isFaceSpeaker) { - PID.setSetpoint(UtilMath.FrontSpeakerTheta(swerve.getPose())); + double ang = UtilMath.FrontSpeakerTheta(swerve.getPose()); + PID.setSetpoint(ang); angVelocity = PID.calculate(currTheta); + SmartDashboard.putNumber("isFaceSpeaker Goal", ang); + SmartDashboard.putNumber("isFaceSpeaker Result", angVelocity); } else if(isBackSpeaker) { - PID.setSetpoint(UtilMath.BackSpeakerTheta(swerve.getPose())); + double ang = UtilMath.BackSpeakerTheta(swerve.getPose()); + PID.setSetpoint(ang); angVelocity = PID.calculate(currTheta); + SmartDashboard.putNumber("isBackSpeaker Goal", ang); + SmartDashboard.putNumber("isBackSpeaker Result", angVelocity); } if(isSlow) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 5bc3b05b..19e5849e 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -54,8 +54,8 @@ public static final class Arm { public static final int ENCODER_DIO_SLOT = 0; public static final int AMP_ANGLE = 300; public static final int TRAP_ANGLE = 270; - public static final int SPEAKER_LOW_ANGLE = 165; - public static final int SPEAKER_HIGH_ANGLE = 238; + public static final int SPEAKER_LOW_ANGLE = 160; + public static final int SPEAKER_HIGH_ANGLE = 236; public static final int INTAKE_ANGLE = 63; public static final int CLIMBING_ANGLE = 45; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 552ad9ec..c7b3d0c1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -45,9 +45,6 @@ public class SwerveSubsystem extends SubsystemBase */ public double maximumSpeed = Units.feetToMeters(14.5); //14.5 - private StructArrayPublisher swerveStatePublisher = NetworkTableInstance.getDefault() - .getStructArrayTopic("Swerve State", SwerveModuleState.struct).publish(); - /** * Initialize {@link SwerveDrive} with the directory provided. * @@ -70,7 +67,7 @@ public SwerveSubsystem(File directory) System.out.println("}"); // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.NONE; + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; // 3/8/2024 changed from HIGH to NONE to see if prevent loop overrun try { @@ -81,7 +78,7 @@ public SwerveSubsystem(File directory) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(true); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. setupPathPlanner(); @@ -326,7 +323,6 @@ public void drive(ChassisSpeeds velocity) @Override public void periodic() { - swerveStatePublisher.set(swerveDrive.getStates()); } @Override From 535547dece9825516e368d120af1f38515a0cdf3 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Wed, 3 Apr 2024 15:09:33 -0700 Subject: [PATCH 39/59] increase arm P --- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 19e5849e..b9111d90 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -40,7 +40,7 @@ public static final class Arm { public static final int MAX_VOLT = 12; // - public static final double KP = 6; + public static final double KP = 6.5; public static final double KI = 0; public static final double KD = 0; public static final double KSVOLTS = 1.5; From a80a36b902c1c624c34580b6f7932443abe8dfbd Mon Sep 17 00:00:00 2001 From: CatonQ Date: Wed, 3 Apr 2024 15:19:30 -0700 Subject: [PATCH 40/59] intake command change --- src/main/java/frc/robot/GameRobotContainer.java | 1 - .../commands/IntakeCommands/IntakePassNoteToPizzaBox.java | 3 ++- .../frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index b2246c58..f3a3a4f2 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -110,7 +110,6 @@ private void configureBindings() { driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); - /* Operator Controllers */ operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java index 49ac8731..251a84d6 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java @@ -31,10 +31,11 @@ public IntakePassNoteToPizzaBox(IntakeSubsystem subsystem, PizzaBoxSubsystem piz // Called when the command is initially scheduled. @Override public void initialize() { + pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); System.out.println("Intake pass note to pizzabox initialize"); currentSensorValue = true; intakeSubsystem.spinIntakeMotor(0.8, 100); - pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); + //pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); initTimer = Timer.getFPGATimestamp(); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 552ad9ec..626309bf 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -130,13 +130,13 @@ public void setupPathPlanner() new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class new PIDConstants(1,0,0), // Translation PID constants - new PIDConstants(8,0,0), + new PIDConstants(9,0,0), // Rotation PID constants 4.5, // Max module speed, in m/s swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() + new ReplanningConfig(true, true) // Default path replanning config. See the API for the options here ), () -> { From 868712fb345d4b44566b1e5742a5d2e6e3704285 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Wed, 3 Apr 2024 16:54:59 -0700 Subject: [PATCH 41/59] pushed --- .pathplanner/settings.json | 6 +- .../deploy/pathplanner/autos/3N-S1-allA.auto | 114 ----------------- .../deploy/pathplanner/autos/3N-S3-allA.auto | 121 ------------------ ...om 2.auto => Copy of Copy of Hajel 3.auto} | 0 .../deploy/pathplanner/autos/Test Auto.auto | 18 --- src/main/deploy/pathplanner/paths/A1-A2.path | 14 +- src/main/deploy/pathplanner/paths/A1-C1.path | 14 +- src/main/deploy/pathplanner/paths/A1-C2.path | 14 +- src/main/deploy/pathplanner/paths/A1-C3.path | 14 +- src/main/deploy/pathplanner/paths/A1-S1.path | 14 +- src/main/deploy/pathplanner/paths/A1-S2.path | 8 +- src/main/deploy/pathplanner/paths/A2-A1.path | 14 +- src/main/deploy/pathplanner/paths/A2-A3.path | 6 +- src/main/deploy/pathplanner/paths/A2-C1.path | 6 +- src/main/deploy/pathplanner/paths/A2-C2.path | 6 +- src/main/deploy/pathplanner/paths/A2-C3.path | 6 +- src/main/deploy/pathplanner/paths/A2-S1.path | 6 +- src/main/deploy/pathplanner/paths/A2-S2.path | 6 +- src/main/deploy/pathplanner/paths/A3-A1.path | 14 +- src/main/deploy/pathplanner/paths/A3-A2.path | 6 +- src/main/deploy/pathplanner/paths/A3-C5.path | 6 +- src/main/deploy/pathplanner/paths/A3-S2.path | 8 +- src/main/deploy/pathplanner/paths/A3-S3.path | 6 +- src/main/deploy/pathplanner/paths/C1-A1.path | 14 +- src/main/deploy/pathplanner/paths/C1-A2.path | 6 +- src/main/deploy/pathplanner/paths/C1-S2.path | 6 +- src/main/deploy/pathplanner/paths/C2-A1.path | 14 +- src/main/deploy/pathplanner/paths/C2-A2.path | 6 +- src/main/deploy/pathplanner/paths/C4-S3.path | 6 +- src/main/deploy/pathplanner/paths/C5-S3.path | 6 +- src/main/deploy/pathplanner/paths/S1-A1.path | 14 +- src/main/deploy/pathplanner/paths/S1-A2.path | 6 +- src/main/deploy/pathplanner/paths/S1-C1.path | 6 +- .../deploy/pathplanner/paths/S1-Leave.path | 6 +- src/main/deploy/pathplanner/paths/S2-A1.path | 18 +-- src/main/deploy/pathplanner/paths/S2-A2.path | 6 +- src/main/deploy/pathplanner/paths/S2-A3.path | 6 +- src/main/deploy/pathplanner/paths/S2-C1.path | 6 +- src/main/deploy/pathplanner/paths/S3-A3.path | 6 +- src/main/deploy/pathplanner/paths/S3-C4.path | 6 +- src/main/deploy/pathplanner/paths/S3-C5.path | 6 +- .../deploy/pathplanner/paths/S3-Leave.path | 6 +- src/main/deploy/pathplanner/paths/center.path | 6 +- .../pathplanner/paths/go to center.path | 6 +- .../swerve/hajel_kraken/modules/backleft.json | 8 +- .../hajel_kraken/modules/backright.json | 6 +- .../hajel_kraken/modules/frontleft.json | 8 +- .../hajel_kraken/modules/frontright.json | 6 +- .../hajel_kraken/modules/pidfproperties.json | 2 +- src/main/java/frc/robot/Constants.java | 4 +- .../java/frc/robot/GameRobotContainer.java | 2 +- .../java/frc/robot/SetupShuffleboard.java | 8 +- .../PickUpFromGroundAndPassToPizzaBox.java | 1 + .../frc/robot/subsystems/ArmSubsystem.java | 2 +- 54 files changed, 194 insertions(+), 442 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/3N-S1-allA.auto delete mode 100644 src/main/deploy/pathplanner/autos/3N-S3-allA.auto rename src/main/deploy/pathplanner/autos/{Copy of Copy of Hajel middle bottom 2.auto => Copy of Copy of Hajel 3.auto} (100%) delete mode 100644 src/main/deploy/pathplanner/autos/Test Auto.auto diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 77bb3e68..365b17ba 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -16,8 +16,8 @@ "Hajel" ], "defaultMaxVel": 2.0, - "defaultMaxAccel": 1.5, - "defaultMaxAngVel": 30.0, - "defaultMaxAngAccel": 50.0, + "defaultMaxAccel": 2.0, + "defaultMaxAngVel": 50.0, + "defaultMaxAngAccel": 100.0, "maxModuleSpeed": 4.0 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-S1-allA.auto b/src/main/deploy/pathplanner/autos/3N-S1-allA.auto deleted file mode 100644 index 5051e4f9..00000000 --- a/src/main/deploy/pathplanner/autos/3N-S1-allA.auto +++ /dev/null @@ -1,114 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.85, - "y": 6.62 - }, - "rotation": 59.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S1)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S1-A1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A1-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A2-A3" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A3)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-S3-allA.auto b/src/main/deploy/pathplanner/autos/3N-S3-allA.auto deleted file mode 100644 index dcdcfdf6..00000000 --- a/src/main/deploy/pathplanner/autos/3N-S3-allA.auto +++ /dev/null @@ -1,121 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 4.44 - }, - "rotation": -59.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S3) then Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "S3-A3" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A3-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A2-A1" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A1)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel middle bottom 2.auto b/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel 3.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Copy of Copy of Hajel middle bottom 2.auto rename to src/main/deploy/pathplanner/autos/Copy of Copy of Hajel 3.auto diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto deleted file mode 100644 index 07d4e332..00000000 --- a/src/main/deploy/pathplanner/autos/Test Auto.auto +++ /dev/null @@ -1,18 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 2 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-A2.path b/src/main/deploy/pathplanner/paths/A1-A2.path index 6e3818d2..0aff71f7 100644 --- a/src/main/deploy/pathplanner/paths/A1-A2.path +++ b/src/main/deploy/pathplanner/paths/A1-A2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 1.570508221490355, - "y": 6.940041387424158 + "x": 1.7312740565740923, + "y": 6.717535493054309 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index 0891e5c7..546379a3 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 2.6, - "y": 6.979658581285779 + "x": 2.7607658350837374, + "y": 6.75715268691593 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index 5b1ca3ef..49d23665 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 4.000000000000001, - "y": 6.979658581285779 + "x": 4.160765835083739, + "y": 6.75715268691593 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 7ad89d2b..77f5eabc 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 2.6, - "y": 6.979658581285779 + "x": 2.7607658350837374, + "y": 6.75715268691593 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index 9d35937b..aa7aa117 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 2.4857477492732767, - "y": 6.965406330559056 + "x": 2.646513584357014, + "y": 6.742900436189207 }, "isLocked": false, "linkedName": "A1" @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index ad5324e5..9ee6c62b 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -20,7 +20,7 @@ "y": 5.54412553269551 }, "prevControl": { - "x": 1.2299999999999998, + "x": 1.23, "y": 5.54412553269551 }, "nextControl": null, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-A1.path b/src/main/deploy/pathplanner/paths/A2-A1.path index d9d7ffa8..1244b1a0 100644 --- a/src/main/deploy/pathplanner/paths/A2-A1.path +++ b/src/main/deploy/pathplanner/paths/A2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 1.6738989114212168, - "y": 6.653775335282482 + "x": 1.8346647465049541, + "y": 6.431269440912633 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-A3.path b/src/main/deploy/pathplanner/paths/A2-A3.path index dd621e09..b9dce19d 100644 --- a/src/main/deploy/pathplanner/paths/A2-A3.path +++ b/src/main/deploy/pathplanner/paths/A2-A3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C1.path b/src/main/deploy/pathplanner/paths/A2-C1.path index 079a5456..a6d0760f 100644 --- a/src/main/deploy/pathplanner/paths/A2-C1.path +++ b/src/main/deploy/pathplanner/paths/A2-C1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C2.path b/src/main/deploy/pathplanner/paths/A2-C2.path index abc3e484..5fd2c034 100644 --- a/src/main/deploy/pathplanner/paths/A2-C2.path +++ b/src/main/deploy/pathplanner/paths/A2-C2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 7e7757ee..8e920439 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index cf0a423b..7fadf210 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index 1332838e..2f1508f7 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-A1.path b/src/main/deploy/pathplanner/paths/A3-A1.path index c45c936d..8086268a 100644 --- a/src/main/deploy/pathplanner/paths/A3-A1.path +++ b/src/main/deploy/pathplanner/paths/A3-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 1.83985746204333, - "y": 6.588642825645138 + "x": 2.000623297127067, + "y": 6.366136931275289 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-A2.path b/src/main/deploy/pathplanner/paths/A3-A2.path index ffbbb15a..9a611791 100644 --- a/src/main/deploy/pathplanner/paths/A3-A2.path +++ b/src/main/deploy/pathplanner/paths/A3-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index 119340b4..220297ca 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index f649a1e2..51521bd0 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -20,7 +20,7 @@ "y": 5.54412553269551 }, "prevControl": { - "x": 1.2299999999999998, + "x": 1.23, "y": 5.54412553269551 }, "nextControl": null, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index 4e06e7d8..e9a696ac 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-A1.path b/src/main/deploy/pathplanner/paths/C1-A1.path index 45b1125c..d775afb2 100644 --- a/src/main/deploy/pathplanner/paths/C1-A1.path +++ b/src/main/deploy/pathplanner/paths/C1-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.598480775301221, - "y": 6.997023399052472 + "x": 2.759246610384958, + "y": 6.774517504682623 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-A2.path b/src/main/deploy/pathplanner/paths/C1-A2.path index 73db4f8b..55d5b4c0 100644 --- a/src/main/deploy/pathplanner/paths/C1-A2.path +++ b/src/main/deploy/pathplanner/paths/C1-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index 56c836b4..1fd8d50f 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 48dd40e0..05f2554c 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.598480775301221, - "y": 6.962293763519086 + "x": 2.759246610384958, + "y": 6.739787869149237 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2-A2.path b/src/main/deploy/pathplanner/paths/C2-A2.path index 2bbd6457..15328d68 100644 --- a/src/main/deploy/pathplanner/paths/C2-A2.path +++ b/src/main/deploy/pathplanner/paths/C2-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index 4adb1d56..00120eb0 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index 550ac00c..fa20dc2a 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index 9422d857..77243f67 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.118910700160914, - "y": 6.642064498736701 + "x": 2.2796765352446515, + "y": 6.419558604366852 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index 693d6422..348e5954 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index 65ff9ffe..464373bc 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index 45d5a355..2ab76907 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index 6d39f14f..719ee9a0 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.4300000000000002, - "y": 5.54412553269551 + "x": 2.3914165945307624, + "y": 5.703177397795595 }, "isLocked": false, "linkedName": "S2" }, { "anchor": { - "x": 2.5, - "y": 6.979658581285779 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.4, - "y": 6.979658581285779 + "x": 1.6855724787889659, + "y": 6.2536022981555925 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index 1b6cdd17..224864d8 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index 9bd21dc8..ed9d4a22 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index 1b0ba2d7..283d8796 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index a1abfd7e..a9ecf3a6 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index 35b0abc1..6e44ef9e 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index d1567e42..346d1d34 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index 1eeb7501..09304e66 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/center.path b/src/main/deploy/pathplanner/paths/center.path index 8a0c55c8..63189897 100644 --- a/src/main/deploy/pathplanner/paths/center.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/go to center.path b/src/main/deploy/pathplanner/paths/go to center.path index 2d3ae087..61cff81a 100644 --- a/src/main/deploy/pathplanner/paths/go to center.path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 50.0 + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/swerve/hajel_kraken/modules/backleft.json b/src/main/deploy/swerve/hajel_kraken/modules/backleft.json index 031ff679..beefc08e 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/backleft.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/backleft.json @@ -1,9 +1,9 @@ { "location": { - "front": -13.779, - "left": 13.779 + "front": -13, + "left": 13 }, - "absoluteEncoderOffset": 301.816404, + "absoluteEncoderOffset": 122.871, "drive": { "type": "talonfx", "id": 3, @@ -24,7 +24,7 @@ "canbus": "CAN_Network" }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderInverted": false diff --git a/src/main/deploy/swerve/hajel_kraken/modules/backright.json b/src/main/deploy/swerve/hajel_kraken/modules/backright.json index b720d798..5fe94c84 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/backright.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/backright.json @@ -1,9 +1,9 @@ { "location": { - "front": -13.779, - "left": -13.779 + "front": -13, + "left": -13 }, - "absoluteEncoderOffset": 228.691404, + "absoluteEncoderOffset": 229.746, "drive": { "type": "talonfx", "id": 1, diff --git a/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json b/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json index 1589d98f..5a466fff 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json @@ -1,9 +1,9 @@ { "location": { - "front": 13.779, - "left": 13.779 + "front": 13, + "left": 13 }, - "absoluteEncoderOffset": 281.33789, + "absoluteEncoderOffset": 103.008, "drive": { "type": "talonfx", "id": 5, @@ -24,7 +24,7 @@ "canbus": "CAN_Network" }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderInverted": false diff --git a/src/main/deploy/swerve/hajel_kraken/modules/frontright.json b/src/main/deploy/swerve/hajel_kraken/modules/frontright.json index 3c71b12a..1e3ca78e 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/frontright.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/frontright.json @@ -1,9 +1,9 @@ { "location": { - "front": 13.779, - "left": -13.779 + "front": 13, + "left": -13 }, - "absoluteEncoderOffset": 7.1191404, + "absoluteEncoderOffset": 6.502, "drive": { "type": "talonfx", "id": 7, diff --git a/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json b/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json index 75d55289..cc3e36dd 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json @@ -9,7 +9,7 @@ "angle": { "p": 50, "i": 0, - "d": 0.32, + "d": 0, "f": 0, "iz": 0 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 866926e1..a45e481f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -94,11 +94,11 @@ public static final class IntakeConstants { public static final int INTAKE_ENCODER_CHANNEL_ID = 9; public static final int INTAKE_NOTESENSOR_CHANNEL_ID = 8; public static final double TOLERANCE = 2; // in degrees--- TO-DO: Tune Tolerance, intake is not lowering enough. - public static final double MAX_ARM_ANGLE = 85; //need to check max arm angle + public static final double MAX_ARM_ANGLE = 95; //need to check max arm angle public static final double ROTATION_TO_DEGREES = 360; public static final double ENCODER_RAW_TO_ROTATION = 8132.; public static double ENCODER_OFFSET = 55.5; //need to check the encoder value - public static final double UP_POSITION = 80; + public static final double UP_POSITION = 95; public static final double SOURCE_INTAKE_ANGLE = 160; } diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 15946770..48d187da 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -85,7 +85,7 @@ public GameRobotContainer() { m_drivebase.setDefaultCommand(closedFieldRel); - //SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); + SetupShuffleboard.setupShuffleboard(m_drivebase, m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem, m_LimelightSubsystem, m_ClimbSubsystem, m_ReactionSubsystem, autoChooser, closedFieldRel); configureBindings(); diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 1b3f5e07..f15d0cce 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -16,6 +16,7 @@ import frc.robot.commands.CancelAllCommands; import frc.robot.commands.Arm.ResetArm; import frc.robot.commands.Arm.ScoreInTrapStutter; +import frc.robot.commands.Arm.SpinToArmAngle; import frc.robot.commands.Arm.SwingBackServoTheSecond; import frc.robot.commands.Arm.SwingForwardServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; @@ -81,12 +82,15 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + } + Shuffleboard.getTab("Pizza Box").add("270", new SpinToArmAngle(armSubsystem, 270)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); + Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); diff --git a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java index 9a9746a6..e0e698f4 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java @@ -26,6 +26,7 @@ public PickUpFromGroundAndPassToPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, Ar new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).withTimeout(2) .alongWith((new IntakePickUpFromGroundPID(intakeSubsystem, 0.7, 0.0).withTimeout(intakeTimeout)) .andThen(new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION))) + .alongWith(new SpinNoteContainerMotor(pizzaBoxSubsystem, -60, 100)) .andThen(new IntakePassNoteToPizzaBox(intakeSubsystem, pizzaBoxSubsystem).withTimeout(6)) .andThen(Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = true; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index b9111d90..e54af164 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -53,7 +53,7 @@ public static final class Arm { //Arm ID Jalen Tolbert public static final int ENCODER_DIO_SLOT = 0; public static final int AMP_ANGLE = 300; - public static final int TRAP_ANGLE = 270; + public static final int TRAP_ANGLE = 275; public static final int SPEAKER_LOW_ANGLE = 160; public static final int SPEAKER_HIGH_ANGLE = 236; public static final int INTAKE_ANGLE = 63; From 39b8d63c158726adf2c3adf55bc6562be77c83f6 Mon Sep 17 00:00:00 2001 From: chvoong24 Date: Wed, 3 Apr 2024 16:56:10 -0700 Subject: [PATCH 42/59] paths --- .../deploy/pathplanner/autos/2N-S2-A1.auto | 82 +++++++++++++++++++ .../deploy/pathplanner/autos/2N-S2-A3.auto | 82 +++++++++++++++++++ 2 files changed, 164 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/2N-S2-A1.auto create mode 100644 src/main/deploy/pathplanner/autos/2N-S2-A3.auto diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto new file mode 100644 index 00000000..ace2e107 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto @@ -0,0 +1,82 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.54 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker (S2) then Intake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "S2-A2" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker (A2)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A2-A1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker (A1)" + } + } + ] + } + }, + "folder": "2. A+A", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto b/src/main/deploy/pathplanner/autos/2N-S2-A3.auto new file mode 100644 index 00000000..3f12da71 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2N-S2-A3.auto @@ -0,0 +1,82 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.54 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker (S2) then Intake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "S2-A2" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker (A2)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A2-A3" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker (A3)" + } + } + ] + } + }, + "folder": "2. A+A", + "choreoAuto": false +} \ No newline at end of file From 3b71c460bb9b1364a889ec0a3620d4bc071ef15a Mon Sep 17 00:00:00 2001 From: wilsomc Date: Wed, 3 Apr 2024 18:02:05 -0700 Subject: [PATCH 43/59] hi --- src/main/java/frc/robot/GameRobotContainer.java | 3 ++- .../commands/ClimberCommands/ClimbParts/ClimbAndShoot.java | 4 ++++ src/main/java/frc/robot/subsystems/ReactionSubsystem.java | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 48d187da..786b2f15 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -175,7 +175,8 @@ public Command teleopInitReset() { } public Command autoInitReset() { - return new IntakeResetArm(m_IntakeSubsystem).withTimeout(3); + return new IntakeResetArm(m_IntakeSubsystem).withTimeout(3) + .alongWith(new Retract(m_ReactionSubsystem)); } } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java index 691c8fd4..2773f19a 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import frc.robot.commands.Arm.*; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; @@ -32,6 +33,9 @@ public ClimbAndShoot(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, PizzaB Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorDown(c, a).alongWith(new Extend(r)).withTimeout(3), new ScoreInTrapStutter(p, a), + new SpinNoteContainerMotor(p, -5, 100), + new WaitCommand(1.6), + new StopNoteContainerMotor(p), Commands.runOnce(()->DataLogManager.log("Command End: ClimbAndShoot")) ); diff --git a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java index b202bb4e..bf0b1ed1 100644 --- a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java @@ -48,7 +48,7 @@ public void spinForward() } public void spinBackward() { - PIDcontroller.setSetpoint(-0.21); + PIDcontroller.setSetpoint(-0.8); } public void stop() { From 02c518991291b9326aef18a541eeaff458b0725a Mon Sep 17 00:00:00 2001 From: chvoong24 Date: Wed, 3 Apr 2024 18:34:31 -0700 Subject: [PATCH 44/59] reaction bar works and put back tree --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/Util/UtilMath.java | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c152d0d4..c184d931 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,7 @@ public void robotInit() { break; } - //UtilMath.treeOverHand.put(); + //UtilMath.overhand.put(); //UtilMath.treeUnderHand.put(); diff --git a/src/main/java/frc/robot/Util/UtilMath.java b/src/main/java/frc/robot/Util/UtilMath.java index cd374c5b..e93613d3 100644 --- a/src/main/java/frc/robot/Util/UtilMath.java +++ b/src/main/java/frc/robot/Util/UtilMath.java @@ -18,6 +18,8 @@ public class UtilMath { public static final int RED_SOURCE_HEADING_ANGLE = -120; public static final int BLUE_SOURCE_HEADING_ANGLE = -60; + public static InterpolatingDoubleTreeMap overhand = new InterpolatingDoubleTreeMap(); + public static double caclucateRotateTheta(Pose2d pose, double targetX, double targetY){ From fe4ae520c83ec0a9bfb1dd78ad2b5fe5b0448dab Mon Sep 17 00:00:00 2001 From: chvoong24 Date: Wed, 3 Apr 2024 18:52:36 -0700 Subject: [PATCH 45/59] tree --- src/main/java/frc/robot/GameRobotContainer.java | 2 +- src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/Util/UtilMath.java | 14 ++++++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 48d187da..899904be 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -105,7 +105,7 @@ private void configureBindings() { driverController.rightStick().onTrue(new ScoreInSpeakerHigh(m_PizzaBoxSubsystem, m_ArmSubsystem)); //driverController.leftStick().onTrue(new ChangeRobotOrientation(closedFieldRel)); - //driverController.rightBumper().onTrue(new BackSpeaker(closedFieldRel)); + driverController.rightBumper().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()->m_drivebase.getPose())))); //driverController.leftBumper().onTrue(new FaceSpeaker(closedFieldRel)); driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c184d931..bbaf9eff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,8 @@ public void robotInit() { break; } - //UtilMath.overhand.put(); + UtilMath.overhand.put(1.33, 236d); //S2 + UtilMath.overhand.put(2.45, 245d); //A2 //UtilMath.treeUnderHand.put(); diff --git a/src/main/java/frc/robot/Util/UtilMath.java b/src/main/java/frc/robot/Util/UtilMath.java index e93613d3..53262aff 100644 --- a/src/main/java/frc/robot/Util/UtilMath.java +++ b/src/main/java/frc/robot/Util/UtilMath.java @@ -4,6 +4,8 @@ package frc.robot.Util; +import java.util.function.Supplier; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.DriverStation; @@ -19,6 +21,7 @@ public class UtilMath { public static final int BLUE_SOURCE_HEADING_ANGLE = -60; public static InterpolatingDoubleTreeMap overhand = new InterpolatingDoubleTreeMap(); + @@ -111,4 +114,15 @@ public static double distanceFromSpeaker(Pose2d pose) { return Math.sqrt(Math.pow(RED_SPEAKER_X - robotX, 2) + Math.pow(RED_SPEAKER_Y - robotY, 2)); } } + + public static double distanceFromSpeaker(Supplier pose) { + double robotX = pose.get().getX(); + double robotY = pose.get().getY(); + + if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + return Math.sqrt(Math.pow(BLUE_SPEAKER_X - robotX, 2) + Math.pow(BLUE_SPEAKER_Y - robotY, 2)); + } else { + return Math.sqrt(Math.pow(RED_SPEAKER_X - robotX, 2) + Math.pow(RED_SPEAKER_Y - robotY, 2)); + } + } } From d3462424950738945a570c2dc0f66c5f6c20e6b1 Mon Sep 17 00:00:00 2001 From: Isaac Ng Date: Wed, 3 Apr 2024 22:03:17 -0700 Subject: [PATCH 46/59] clean up --- simgui-ds.json | 67 ++++++++++++------- simgui.json | 12 ++++ src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/Robot.java | 15 +---- .../java/frc/robot/SetupShuffleboard.java | 18 ++--- src/main/java/frc/robot/Util/UtilMath.java | 9 +-- .../ClimberCommands/ClimbParts/PrepClimb.java | 2 +- ...ClimbPartTwoThatWillBringDownTheMotor.java | 2 +- .../swervedrive/drivebase/TeleopDrive.java | 32 ++++----- .../frc/robot/subsystems/ArmSubsystem.java | 8 +-- .../frc/robot/subsystems/IntakeSubsystem.java | 13 ---- .../robot/subsystems/PizzaBoxSubsystem.java | 6 -- .../swervedrive/SwerveSubsystem.java | 5 -- 13 files changed, 83 insertions(+), 112 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index f4b6d226..43fec4e1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,7 +1,12 @@ { - "System Joysticks": { + "Keyboard 0 Settings": { "window": { - "visible": false + "visible": true + } + }, + "Keyboard 1 Settings": { + "window": { + "visible": true } }, "keyboardJoysticks": [ @@ -23,24 +28,23 @@ } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 14, "buttonKeys": [ - 90, - 88, - 67, - 86 - ], - "povConfig": [ - { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } + 321, + 322, + 323, + 324, + -1, + 326, + 327, + 328, + 329, + 320, + -1, + -1, + -1, + -1, + -1 ], "povCount": 1 }, @@ -56,12 +60,21 @@ } ], "axisCount": 2, - "buttonCount": 4, + "buttonCount": 13, "buttonKeys": [ - 77, - 44, - 46, - 47 + -1, + -1, + -1, + -1, + -1, + -1, + -1, + -1, + 325, + -1, + -1, + -1, + -1 ], "povCount": 0 }, @@ -96,8 +109,10 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index aff4524f..1c4e2db9 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,11 @@ { + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -18,7 +25,9 @@ "/Shuffleboard/GameTab/Field": "Field2d", "/Shuffleboard/GameTab/Filed": "Field2d", "/Shuffleboard/GameTab/Increase Intake Offset": "Command", + "/Shuffleboard/GameTab/Intake Reject Note": "Command", "/Shuffleboard/GameTab/IntakeRejectNote": "Command", + "/Shuffleboard/GameTab/RESET TELEOP DRIVE": "Command", "/Shuffleboard/GameTab/Reset Arm": "Command", "/Shuffleboard/GameTab/Reset Intake": "Command", "/Shuffleboard/GameTab/Retract Reaction Bar": "Command", @@ -55,5 +64,8 @@ }, "NetworkTables Info": { "visible": true + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a45e481f..f91069a2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -118,9 +118,9 @@ public static final class LEDConstants public static final int ledPortNumber = 9; } public static final class DriveConstants { - public static final double kP = 0.04; - public static final double kI = 0.055; - public static final double kD = 0.000; + public static final double kP = 0.02; + public static final double kI = 0; + public static final double kD = 0; public static final double THETA_TOLERANCE = 0.1; public static final double STEADY_STATE_TOLERANCE = 0.1; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bbaf9eff..e3346442 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -59,12 +59,6 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and use the subsystems needed - // for the specific robot - // LimelightHelpers.setStreamMode_PiPSecondary("limelight"); - - // Shuffleboard.getTab("GameTab").addCamera("Vision", "limelight", "http://limelight.local:5800").withSize(4,3).withPosition(5, 0); - // String logfolder = "/home/lvuser"; // Logger.addDataReceiver(new WPILOGWriter(logfolder)); // Logger.addDataReceiver(new NT4Publisher()); @@ -76,6 +70,9 @@ public void robotInit() { DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); + + UtilMath.overhand.put(1.33, 236d); //S2 + UtilMath.overhand.put(2.45, 245d); //A2 switch (container){ case GAME: @@ -104,12 +101,6 @@ public void robotInit() { m_baseContainer = new ReactionArmContainer(); break; } - - UtilMath.overhand.put(1.33, 236d); //S2 - UtilMath.overhand.put(2.45, 245d); //A2 - - //UtilMath.treeUnderHand.put(); - } /** diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index f15d0cce..3e540e39 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -81,15 +81,11 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); - - - - } - Shuffleboard.getTab("Pizza Box").add("270", new SpinToArmAngle(armSubsystem, 270)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); - Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + // Shuffleboard.getTab("Pizza Box").add("270", new SpinToArmAngle(armSubsystem, 270)); + // Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); + // Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); @@ -136,12 +132,10 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p .withPosition(5,3); Shuffleboard.getTab("GameTab").add("CLEAR ALL COMMANDS", new CancelAllCommands()).withPosition(7,3); Shuffleboard.getTab("GameTab").add("RESET TELEOP DRIVE", new ResetTeleopDrive(teleopDrive)).withPosition(9,0); - Shuffleboard.getTab("GameTab").add("Straighten", new StraightenWheelCommand(swerve)).withPosition(9, 1); + // Shuffleboard.getTab("GameTab").add("Straighten", new StraightenWheelCommand(swerve)).withPosition(9, 1); // Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); - - Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve)); DataLogManager.log("rotate in place P: " + Constants.DriveConstants.kP); @@ -150,8 +144,4 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p } - @Override - public void periodic() { - // This method will be called once per scheduler run - } } diff --git a/src/main/java/frc/robot/Util/UtilMath.java b/src/main/java/frc/robot/Util/UtilMath.java index 53262aff..452ebf32 100644 --- a/src/main/java/frc/robot/Util/UtilMath.java +++ b/src/main/java/frc/robot/Util/UtilMath.java @@ -116,13 +116,6 @@ public static double distanceFromSpeaker(Pose2d pose) { } public static double distanceFromSpeaker(Supplier pose) { - double robotX = pose.get().getX(); - double robotY = pose.get().getY(); - - if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - return Math.sqrt(Math.pow(BLUE_SPEAKER_X - robotX, 2) + Math.pow(BLUE_SPEAKER_Y - robotY, 2)); - } else { - return Math.sqrt(Math.pow(RED_SPEAKER_X - robotX, 2) + Math.pow(RED_SPEAKER_Y - robotY, 2)); - } + return distanceFromSpeaker(pose.get()); } } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index a7d598f7..ef434943 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -30,7 +30,7 @@ public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSu Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(4), new SwingForwardServoTheSecond(p), - new WaitCommand(0.5), + new WaitCommand(0.7), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), new MotorUp(c, a).withTimeout(5), diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index f178a982..e956a4c7 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -35,7 +35,7 @@ public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem new MotorDown(c, a).withTimeout(3), new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(2.5), new SwingBackServoTheSecond(p), - new WaitCommand(0.5), + new WaitCommand(0.7), new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) ); diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java index f8abd0ee..f6336227 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java @@ -55,7 +55,6 @@ public TeleopDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, this.PID = new PIDController(Constants.DriveConstants.kP, Constants.DriveConstants.kI, Constants.DriveConstants.kD); this.PID.setTolerance(Constants.FaceSpeakerConstants.THETA_TOLERANCE, Constants.FaceSpeakerConstants.STEADY_STATE_TOLERANCE); - this.PID.setPID(Constants.FaceSpeakerConstants.kP, Constants.FaceSpeakerConstants.kI, Constants.FaceSpeakerConstants.kD); this.PID.enableContinuousInput(-180, 180); // Use addRequirements() here to declare subsystem dependencies. @@ -79,6 +78,7 @@ public void execute() double xVelocity = Math.pow(vX.getAsDouble(), 3); double yVelocity = Math.pow(vY.getAsDouble(), 3); double angVelocity = Math.pow(omega.getAsDouble(), 3); + if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { @@ -86,11 +86,17 @@ public void execute() yVelocity *= -1; } - - currTheta = swerve.getHeading().getDegrees(); + SmartDashboard.putNumber("Robot Rotation", currTheta); + + if(isSlow) + { + double slowFactor = 0.25; + xVelocity *= slowFactor; + yVelocity *= slowFactor; + angVelocity *= slowFactor; + } - // Drive using raw values. if(isFaceSpeaker) { double ang = UtilMath.FrontSpeakerTheta(swerve.getPose()); @@ -108,20 +114,14 @@ else if(isBackSpeaker) SmartDashboard.putNumber("isBackSpeaker Result", angVelocity); } - if(isSlow) - { - double slowFactor = 0.25; - xVelocity *= slowFactor; - yVelocity *= slowFactor; - angVelocity *= slowFactor; - } - if(isHeadingLock) { - PID.setSetpoint(UtilMath.SourceIntakeHeading(swerve.getPose())); - double result = PID.calculate(currTheta)/4; - angVelocity += result; - SmartDashboard.putNumber("heading Lock Result", result); + double theta = UtilMath.SourceIntakeHeading(swerve.getPose()); + PID.setSetpoint(theta); + double result = PID.calculate(currTheta); + angVelocity += result; + SmartDashboard.putNumber("heading Lock Goal", theta); + SmartDashboard.putNumber("heading Lock Result", result); } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index e54af164..47d24b1b 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -40,7 +40,7 @@ public static final class Arm { public static final int MAX_VOLT = 12; // - public static final double KP = 6.5; + public static final double KP = 8; public static final double KI = 0; public static final double KD = 0; public static final double KSVOLTS = 1.5; @@ -85,12 +85,6 @@ public ArmSubsystem(int armId, String armCanbus, int channel1) Shuffleboard.getTab("Arm").addDouble("Encoder Angle", ()->encoderGetAngle()); Shuffleboard.getTab("Arm").addDouble("Goal in degrees", ()->getController().getGoal().position * (180/Math.PI)); - - if(DriverStation.isTest()) { - Shuffleboard.getTab("Arm").addDouble("Arm Stator Current", () -> m_arm.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Arm").addDouble("Arm Rotor Velocity", () -> m_arm.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Arm").addDouble("Arm Temperature", () -> m_arm.getDeviceTemp().getValueAsDouble()); - } DataLogManager.log("Arm P: " + Arm.KP); DataLogManager.log("Arm I: " + Arm.KI); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7ddec14a..57cfeeec 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -47,20 +47,7 @@ public IntakeSubsystem(int lowerIntakeId, int spinIntakeId, String can) { UtilMotor.configMotorSupplyCurrent(m_spinIntake, 80); Shuffleboard.getTab("Intake").addDouble("Encoder Angle", () -> encoderGetAngle()); - Shuffleboard.getTab("Intake").addBoolean("Sensor value", () -> isNotePresent()); - - if(DriverStation.isTest()) { - Shuffleboard.getTab("Intake").addDouble("Intake Arm Stator Current", () -> m_moveIntakeArm.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Intake Arm Rotor Velocity", () -> m_moveIntakeArm.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Intake Arm Temperature", () -> m_moveIntakeArm.getDeviceTemp().getValueAsDouble()); - - Shuffleboard.getTab("Intake").addDouble("Spin Intake Stator Current", () -> m_spinIntake.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Spin Intake Rotor Velocity", () -> m_spinIntake.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Spin Intake Temperature", () -> m_spinIntake.getDeviceTemp().getValueAsDouble()); - } - - //m_Encoder.reset(); // Logger.recordOutput("Intake/EncoderAngle", encoderGetAngle()); // Logger.recordOutput("Intake/SensorValue", isNotePresent()); diff --git a/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java b/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java index 7b080d5c..bb7f68ac 100644 --- a/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java @@ -42,12 +42,6 @@ public PizzaBoxSubsystem(int pizzaBoxId, String pizzaBoxCanbus, int channelServo m_servo = new Servo(channelServo); UtilMotor.configMotor(m_pizzaBox, .5, 0, 0, .12, 15, 80, false); //UtilMotor.configMotorStatorCurrent(m_pizzaBox, 80); - - if(DriverStation.isTest()) { - Shuffleboard.getTab("Pizza").addDouble("Pizzabox Stator Current", () -> m_pizzaBox.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Pizza").addDouble("Pizzabox Rotor Velocity", () -> m_pizzaBox.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Pizza").addDouble("Pizzabox Temperature", () -> m_pizzaBox.getDeviceTemp().getValueAsDouble()); - } } //Spins "Pizzabox" motor: velocity in rotations/sec and acceleration in rotations/sec^2 diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 258b23d1..5688364a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -94,11 +94,6 @@ public SwerveSubsystem(File directory) TalonFX driveMotorTalon = (TalonFX)driveMotor.getMotor(); TalonFX angleMotorTalon = (TalonFX) angleMotor.getMotor(); - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Drive Motor StatorCurrent", () -> driveMotorTalon.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Angle Motor StatorCurrent", () -> angleMotorTalon.getStatorCurrent().getValueAsDouble()); - - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Drive Motor Temp", () -> driveMotorTalon.getDeviceTemp().getValueAsDouble()); - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Angle Motor Temp", () -> angleMotorTalon.getDeviceTemp().getValueAsDouble()); } } } From 79351b7940bc4e98d908e93728b029d161152f5e Mon Sep 17 00:00:00 2001 From: Isaac Ng Date: Wed, 3 Apr 2024 22:55:50 -0700 Subject: [PATCH 47/59] clean up --- simgui.json | 5 +++++ .../IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java | 2 +- .../java/frc/robot/commands/driveCommands/rotateinPlace.java | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/simgui.json b/simgui.json index 1c4e2db9..5a131bdc 100644 --- a/simgui.json +++ b/simgui.json @@ -38,6 +38,11 @@ "/SmartDashboard/Pigeon 2 (v6) [30]": "Gyro" }, "windows": { + "/Shuffleboard/GameTab/Autonomous Chooser": { + "window": { + "visible": true + } + }, "/Shuffleboard/GameTab/Field": { "bottom": 1476, "height": 8.210550308227539, diff --git a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java index e0e698f4..9b8faaad 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java @@ -25,7 +25,7 @@ public PickUpFromGroundAndPassToPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, Ar Commands.runOnce(()->DataLogManager.log("Command Start: PickUpFromGroundAndPassToPizzaBox")), new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).withTimeout(2) .alongWith((new IntakePickUpFromGroundPID(intakeSubsystem, 0.7, 0.0).withTimeout(intakeTimeout)) - .andThen(new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION))) + .andThen(new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION)).withTimeout(5)) .alongWith(new SpinNoteContainerMotor(pizzaBoxSubsystem, -60, 100)) .andThen(new IntakePassNoteToPizzaBox(intakeSubsystem, pizzaBoxSubsystem).withTimeout(6)) .andThen(Commands.runOnce(() -> { diff --git a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java index e9eb88a7..d3d6de96 100644 --- a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java +++ b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java @@ -47,7 +47,7 @@ public void initialize() { currTheta = m_Subsystem.getHeading().getDegrees(); PID.setSetpoint(targetTheta); PID.setTolerance(Constants.DriveConstants.THETA_TOLERANCE, Constants.DriveConstants.STEADY_STATE_TOLERANCE); - PID.setPID(0.005, 0, 0); + // PID.setPID(0.005, 0, 0); PID.enableContinuousInput(-180, 180); } From 1f0b8b2d61cd9ce5122ba19e96881f622ead04bc Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 15:27:11 -0700 Subject: [PATCH 48/59] faceamp --- .../robot/commands/driveCommands/FaceAmp.java | 37 +++++++++++++++++++ .../swervedrive/drivebase/TeleopDrive.java | 9 ++++- 2 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/driveCommands/FaceAmp.java diff --git a/src/main/java/frc/robot/commands/driveCommands/FaceAmp.java b/src/main/java/frc/robot/commands/driveCommands/FaceAmp.java new file mode 100644 index 00000000..985ab6a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/driveCommands/FaceAmp.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.driveCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; + +public class FaceAmp extends Command { + /** Creates a new FaceAmp. */ + private final TeleopDrive drive; + public FaceAmp(TeleopDrive driveBase) { + drive = driveBase; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drive.faceAmp = !drive.faceAmp; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java index f8abd0ee..e8c27570 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java @@ -33,6 +33,7 @@ public class TeleopDrive extends Command private final SwerveController controller; public boolean isFaceSpeaker = false; public boolean isBackSpeaker = false; + public boolean faceAmp = false; public boolean isSlow; public boolean isHeadingLock; private final PIDController PID; @@ -108,6 +109,13 @@ else if(isBackSpeaker) SmartDashboard.putNumber("isBackSpeaker Result", angVelocity); } + if (faceAmp) { + PID.setSetpoint(90); + angVelocity = PID.calculate(currTheta); + SmartDashboard.putNumber("faceAmp Goal", 90); + SmartDashboard.putNumber("faceAmp Result", angVelocity); + } + if(isSlow) { double slowFactor = 0.25; @@ -124,7 +132,6 @@ else if(isBackSpeaker) SmartDashboard.putNumber("heading Lock Result", result); } - swerve.drive(new Translation2d(xVelocity * swerve.maximumSpeed, yVelocity * swerve.maximumSpeed), angVelocity * controller.config.maxAngularVelocity, driveMode.getAsBoolean()); From 28f6cc93519e22427f9b14fc6ac05161d8c4aa53 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 15:37:53 -0700 Subject: [PATCH 49/59] uh --- src/main/java/frc/robot/GameRobotContainer.java | 2 +- src/main/java/frc/robot/commands/Arm/ScoreInAmp.java | 4 +++- .../java/frc/robot/commands/driveCommands/FaceSpeaker.java | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 8b87a728..6c9d4eed 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -98,7 +98,7 @@ private void configureBindings() { //RE-ENABLE BUTTONS; driverController.y().onTrue(new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); - driverController.a().onTrue(new ScoreInAmp(m_PizzaBoxSubsystem, m_ArmSubsystem)); + driverController.a().onTrue(new ScoreInAmp(m_PizzaBoxSubsystem, m_ArmSubsystem, closedFieldRel)); driverController.b().onTrue(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem, 10)); driverController.x().whileTrue(new DecreaseSpeed(closedFieldRel)); diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java b/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java index 3b25c79e..64c41131 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java @@ -5,6 +5,7 @@ package frc.robot.commands.Arm; import frc.robot.subsystems.PizzaBoxSubsystem; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -14,7 +15,7 @@ public class ScoreInAmp extends SequentialCommandGroup { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) //velocity = 100 for testing shooting - public ScoreInAmp(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem) { + public ScoreInAmp(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem, TeleopDrive drive) { addCommands( new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.AMP_ANGLE).withTimeout(3), new SpinNoteContainerMotor(pizzaBoxSubsystem, 40, 100), @@ -26,6 +27,7 @@ public ScoreInAmp(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE), Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = false; + drive.faceAmp = false; }) ); } diff --git a/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java b/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java index 6cb99b72..ecc1b788 100644 --- a/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java +++ b/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java @@ -18,7 +18,7 @@ public FaceSpeaker(TeleopDrive drivebase) { // Called when the command is initially scheduled. @Override public void initialize() { - drive.isFaceSpeaker = !drive.isFaceSpeaker; + drive.isFaceSpeaker = true; } // Called every time the scheduler runs while the command is scheduled. From 34d3638e0e8f099b4ed8ee9f68f583295c5d2c01 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 15:39:29 -0700 Subject: [PATCH 50/59] u --- src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java b/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java index ecc1b788..6cb99b72 100644 --- a/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java +++ b/src/main/java/frc/robot/commands/driveCommands/FaceSpeaker.java @@ -18,7 +18,7 @@ public FaceSpeaker(TeleopDrive drivebase) { // Called when the command is initially scheduled. @Override public void initialize() { - drive.isFaceSpeaker = true; + drive.isFaceSpeaker = !drive.isFaceSpeaker; } // Called every time the scheduler runs while the command is scheduled. From b38ce34d8fb4160af1c8189ce873b4ae4a7941aa Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 15:44:28 -0700 Subject: [PATCH 51/59] a --- src/main/java/frc/robot/SetupShuffleboard.java | 2 +- src/main/java/frc/robot/commands/SystemCheck.java | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 3e540e39..97bbffd9 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -136,7 +136,7 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p // Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); - Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve)); + Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve, teleopDrive)); DataLogManager.log("rotate in place P: " + Constants.DriveConstants.kP); DataLogManager.log("rotate in place I: " + Constants.DriveConstants.kI); diff --git a/src/main/java/frc/robot/commands/SystemCheck.java b/src/main/java/frc/robot/commands/SystemCheck.java index 1776dd5c..c784ee33 100644 --- a/src/main/java/frc/robot/commands/SystemCheck.java +++ b/src/main/java/frc/robot/commands/SystemCheck.java @@ -15,6 +15,7 @@ import frc.robot.commands.ClimberCommands.ClimbParts.UnClimbPartTwoThatWillBringDownTheMotor; import frc.robot.commands.IntakeCommands.PickUpFromGroundAndPassToPizzaBox; import frc.robot.commands.ReactionArmCommands.*; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -28,7 +29,7 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class SystemCheck extends SequentialCommandGroup { /** Creates a new SystemCheck. */ - public SystemCheck(ArmSubsystem a, Climbsubsystem c, IntakeSubsystem i, PizzaBoxSubsystem p, ReactionSubsystem r, SwerveSubsystem s) { + public SystemCheck(ArmSubsystem a, Climbsubsystem c, IntakeSubsystem i, PizzaBoxSubsystem p, ReactionSubsystem r, SwerveSubsystem s, TeleopDrive t) { addCommands( //TO-DO: Add condition for different systemcheck if arm/intake is broken @@ -38,7 +39,7 @@ public SystemCheck(ArmSubsystem a, Climbsubsystem c, IntakeSubsystem i, PizzaBox new UnClimbPartTwoThatWillBringDownTheMotor(c, s, a, r, p), new WaitCommand(2), new PickUpFromGroundAndPassToPizzaBox(p, a, i), new WaitCommand(2), new PickUpFromGroundAndPassToPizzaBox(p, a, i), new WaitCommand(2), - new ScoreInAmp(p, a), new WaitCommand(2), + new ScoreInAmp(p, a, t), new WaitCommand(2), new ScoreInSpeakerUnderHand(p, a), new WaitCommand(2) ); From 39f23980016f70465e8c52166d1d5bc24829c3ef Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 16:04:32 -0700 Subject: [PATCH 52/59] aimbot --- .../java/frc/robot/commands/Arm/Aimbot.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/main/java/frc/robot/commands/Arm/Aimbot.java diff --git a/src/main/java/frc/robot/commands/Arm/Aimbot.java b/src/main/java/frc/robot/commands/Arm/Aimbot.java new file mode 100644 index 00000000..e052ec29 --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/Aimbot.java @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.Arm; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Util.UtilMath; +import frc.robot.commands.driveCommands.DecreaseSpeed; +import frc.robot.commands.driveCommands.FaceAmp; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Aimbot extends SequentialCommandGroup { + /** Creates a new Aimbot. */ + public Aimbot(TeleopDrive t, ArmSubsystem a, PizzaBoxSubsystem p, SwerveSubsystem s) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + Commands.runOnce(()-> {t.isSlow = true; t.isFaceSpeaker = true;}), + new ScoreInSpeakerAdjustable(p, a, ()-> UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()-> s.getPose()))), + Commands.runOnce(()-> {t.isSlow = false; t.isFaceSpeaker = false;}) + ); + } +} From 77bafefbb70eb6cacf19cdf10b7baa3b17511e92 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 16:28:00 -0700 Subject: [PATCH 53/59] gyro --- src/main/java/frc/robot/GameRobotContainer.java | 3 ++- .../subsystems/swervedrive/SwerveSubsystem.java | 14 +++++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 6c9d4eed..458cf8b7 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -108,7 +108,8 @@ private void configureBindings() { driverController.rightBumper().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()->m_drivebase.getPose())))); //driverController.leftBumper().onTrue(new FaceSpeaker(closedFieldRel)); - driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); + //driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); + driverController.start().onTrue(new InstantCommand(m_drivebase::setHeading)); /* Operator Controllers */ diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 5688364a..0b2f6c00 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -540,7 +540,19 @@ public Field2d getField2d() public void setHeading() { - resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(Math.PI))); + double xPos = getPose().getX(); + double yPos = getPose().getY(); + + if(DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + { + resetOdometry(new Pose2d(xPos, yPos, new Rotation2d(Math.PI))); + } else if (DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) + { + resetOdometry(new Pose2d(xPos, yPos, new Rotation2d(0))); + } + //resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(Math.PI))); } } \ No newline at end of file From 544cea0464e20e6eb6f9724ce93b993cc9ff47c4 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 16:34:13 -0700 Subject: [PATCH 54/59] back --- src/main/java/frc/robot/GameRobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 458cf8b7..10b55408 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -103,7 +103,7 @@ private void configureBindings() { driverController.x().whileTrue(new DecreaseSpeed(closedFieldRel)); driverController.rightStick().onTrue(new ScoreInSpeakerHigh(m_PizzaBoxSubsystem, m_ArmSubsystem)); - //driverController.leftStick().onTrue(new ChangeRobotOrientation(closedFieldRel)); + driverController.back().onTrue(new ChangeRobotOrientation(closedFieldRel)); driverController.rightBumper().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()->m_drivebase.getPose())))); //driverController.leftBumper().onTrue(new FaceSpeaker(closedFieldRel)); @@ -112,7 +112,7 @@ private void configureBindings() { driverController.start().onTrue(new InstantCommand(m_drivebase::setHeading)); /* Operator Controllers */ - +print"Hello World!" operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); operatorController.b().onTrue(new ClimbAndShoot(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_PizzaBoxSubsystem, m_ReactionSubsystem)); operatorController.a().onTrue(new UnClimb(m_ClimbSubsystem, m_ArmSubsystem, m_ReactionSubsystem)); From 43f740604b3e92e868f9376e58423f1c810d7cef Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 16:40:06 -0700 Subject: [PATCH 55/59] binded buttons --- src/main/java/frc/robot/GameRobotContainer.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 10b55408..cc3004bc 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -105,14 +105,17 @@ private void configureBindings() { driverController.rightStick().onTrue(new ScoreInSpeakerHigh(m_PizzaBoxSubsystem, m_ArmSubsystem)); driverController.back().onTrue(new ChangeRobotOrientation(closedFieldRel)); - driverController.rightBumper().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()->m_drivebase.getPose())))); + //driverController.rightBumper().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()->m_drivebase.getPose())))); //driverController.leftBumper().onTrue(new FaceSpeaker(closedFieldRel)); + driverController.rightBumper().onTrue(new Aimbot(closedFieldRel, m_ArmSubsystem, m_PizzaBoxSubsystem, m_drivebase)); + driverController.leftBumper().onTrue(new FaceAmp(closedFieldRel)); + //driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); driverController.start().onTrue(new InstantCommand(m_drivebase::setHeading)); /* Operator Controllers */ -print"Hello World!" + operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); operatorController.b().onTrue(new ClimbAndShoot(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_PizzaBoxSubsystem, m_ReactionSubsystem)); operatorController.a().onTrue(new UnClimb(m_ClimbSubsystem, m_ArmSubsystem, m_ReactionSubsystem)); From 1123a776bb3978cb5b56845baa16c546eaba4dc7 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 16:48:47 -0700 Subject: [PATCH 56/59] changed aimbot to back speaker --- src/main/java/frc/robot/commands/Arm/Aimbot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/Arm/Aimbot.java b/src/main/java/frc/robot/commands/Arm/Aimbot.java index e052ec29..ebbca92b 100644 --- a/src/main/java/frc/robot/commands/Arm/Aimbot.java +++ b/src/main/java/frc/robot/commands/Arm/Aimbot.java @@ -23,9 +23,9 @@ public Aimbot(TeleopDrive t, ArmSubsystem a, PizzaBoxSubsystem p, SwerveSubsyste // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - Commands.runOnce(()-> {t.isSlow = true; t.isFaceSpeaker = true;}), + Commands.runOnce(()-> {t.isSlow = true; t.isBackSpeaker = true;}), new ScoreInSpeakerAdjustable(p, a, ()-> UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()-> s.getPose()))), - Commands.runOnce(()-> {t.isSlow = false; t.isFaceSpeaker = false;}) + Commands.runOnce(()-> {t.isSlow = false; t.isBackSpeaker = false;}) ); } } From eb2a277c7ff027c263f49fed7a9c31bbd6c324c7 Mon Sep 17 00:00:00 2001 From: CatonQ Date: Thu, 4 Apr 2024 17:06:38 -0700 Subject: [PATCH 57/59] teleopdrive thing im not sure --- .../frc/robot/commands/swervedrive/drivebase/TeleopDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java index 8aacdf70..5ccfe62a 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java @@ -81,7 +81,7 @@ public void execute() double angVelocity = Math.pow(omega.getAsDouble(), 3); if(DriverStation.getAlliance().isPresent() && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + DriverStation.getAlliance().get() == DriverStation.Alliance.Red && driveMode.getAsBoolean()) { xVelocity *= -1; yVelocity *= -1; From e4539f4c233dd52986bb8ce57c4155d004fd53e6 Mon Sep 17 00:00:00 2001 From: "Alvin G." Date: Mon, 26 Aug 2024 17:09:05 -0700 Subject: [PATCH 58/59] I added a comment --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f91069a2..8d60e96d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -86,7 +86,7 @@ public static final class ClimbConstants { public static final int CLIMB_ARM_ANGLE = 255; public static final int CLIMB_ARM_ARNGLE_FOR_SERVO = 330; } - + // hi this is a comment public static final class IntakeConstants { public static final int INTAKE_LOWER_INTAKE_ID = 55; From 8d4f10e0ccac12c82e8949b015aa41bdada9c344 Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 26 Aug 2024 17:14:26 -0700 Subject: [PATCH 59/59] another one --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8d60e96d..6154906a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -124,7 +124,7 @@ public static final class DriveConstants { public static final double THETA_TOLERANCE = 0.1; public static final double STEADY_STATE_TOLERANCE = 0.1; } - + // ur mom public static final class FaceSpeakerConstants { public static final double kP = 0.03;