diff --git a/.gitignore b/.gitignore index 11d4e4f4..a8fe6fdb 100644 --- a/.gitignore +++ b/.gitignore @@ -41,6 +41,8 @@ # Log file *.log +*.hoot +*.dat # BlueJ files *.ctxt diff --git a/ctre_sim/CANCoder vers. H - 01 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 01 - 0 - ext.dat deleted file mode 100644 index 906b2ae6..00000000 Binary files a/ctre_sim/CANCoder vers. H - 01 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 02 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 02 - 0 - ext.dat deleted file mode 100644 index 9fb87787..00000000 Binary files a/ctre_sim/CANCoder vers. H - 02 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat deleted file mode 100644 index 897c480c..00000000 Binary files a/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 04 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 04 - 0 - ext.dat deleted file mode 100644 index e2154ddc..00000000 Binary files a/ctre_sim/CANCoder vers. H - 04 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Pigeon 2 - 05 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 05 - 0 - ext.dat deleted file mode 100644 index b0c84a8c..00000000 Binary files a/ctre_sim/Pigeon 2 - 05 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat deleted file mode 100644 index f8851d38..00000000 Binary files a/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat deleted file mode 100644 index f01233d6..00000000 Binary files a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat deleted file mode 100644 index 4b697969..00000000 Binary files a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat deleted file mode 100644 index f430e336..00000000 Binary files a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat deleted file mode 100644 index 43d4fde2..00000000 Binary files a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 032 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 032 - 0 - ext.dat deleted file mode 100644 index de439e5a..00000000 Binary files a/ctre_sim/Talon FX vers. C - 032 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 042 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 042 - 0 - ext.dat deleted file mode 100644 index 1bd76460..00000000 Binary files a/ctre_sim/Talon FX vers. C - 042 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Talon FX vers. C - 050 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 050 - 0 - ext.dat deleted file mode 100644 index 9c2c6724..00000000 Binary files a/ctre_sim/Talon FX vers. C - 050 - 0 - ext.dat and /dev/null differ diff --git a/logs/2026-02-04_17-52-19/sim_2026-02-04_17-52-21.hoot b/logs/2026-02-04_17-52-19/sim_2026-02-04_17-52-21.hoot deleted file mode 100644 index 58af5e25..00000000 Binary files a/logs/2026-02-04_17-52-19/sim_2026-02-04_17-52-21.hoot and /dev/null differ diff --git a/logs/2026-02-04_17-59-58/sim_2026-02-04_17-59-59.hoot b/logs/2026-02-04_17-59-58/sim_2026-02-04_17-59-59.hoot deleted file mode 100644 index 2837a4d7..00000000 Binary files a/logs/2026-02-04_17-59-58/sim_2026-02-04_17-59-59.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-02-45/sim_2026-02-04_18-02-46.hoot b/logs/2026-02-04_18-02-45/sim_2026-02-04_18-02-46.hoot deleted file mode 100644 index 46040e5c..00000000 Binary files a/logs/2026-02-04_18-02-45/sim_2026-02-04_18-02-46.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-04-20/sim_2026-02-04_18-04-21.hoot b/logs/2026-02-04_18-04-20/sim_2026-02-04_18-04-21.hoot deleted file mode 100644 index 05617cb3..00000000 Binary files a/logs/2026-02-04_18-04-20/sim_2026-02-04_18-04-21.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-06-45/sim_2026-02-04_18-06-46.hoot b/logs/2026-02-04_18-06-45/sim_2026-02-04_18-06-46.hoot deleted file mode 100644 index 3e667eee..00000000 Binary files a/logs/2026-02-04_18-06-45/sim_2026-02-04_18-06-46.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-08-26/sim_2026-02-04_18-08-27.hoot b/logs/2026-02-04_18-08-26/sim_2026-02-04_18-08-27.hoot deleted file mode 100644 index 6d565620..00000000 Binary files a/logs/2026-02-04_18-08-26/sim_2026-02-04_18-08-27.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-12-33/sim_2026-02-04_18-12-34.hoot b/logs/2026-02-04_18-12-33/sim_2026-02-04_18-12-34.hoot deleted file mode 100644 index adee354e..00000000 Binary files a/logs/2026-02-04_18-12-33/sim_2026-02-04_18-12-34.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-15-05/sim_2026-02-04_18-15-07.hoot b/logs/2026-02-04_18-15-05/sim_2026-02-04_18-15-07.hoot deleted file mode 100644 index 7bd3210a..00000000 Binary files a/logs/2026-02-04_18-15-05/sim_2026-02-04_18-15-07.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-26-27/sim_2026-02-04_18-26-28.hoot b/logs/2026-02-04_18-26-27/sim_2026-02-04_18-26-28.hoot deleted file mode 100644 index 51ba7b0f..00000000 Binary files a/logs/2026-02-04_18-26-27/sim_2026-02-04_18-26-28.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-34-39/sim_2026-02-04_18-34-40.hoot b/logs/2026-02-04_18-34-39/sim_2026-02-04_18-34-40.hoot deleted file mode 100644 index 9f9adab2..00000000 Binary files a/logs/2026-02-04_18-34-39/sim_2026-02-04_18-34-40.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-37-41/sim_2026-02-04_18-37-42.hoot b/logs/2026-02-04_18-37-41/sim_2026-02-04_18-37-42.hoot deleted file mode 100644 index e748b2c1..00000000 Binary files a/logs/2026-02-04_18-37-41/sim_2026-02-04_18-37-42.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-49-21/sim_2026-02-04_18-49-22.hoot b/logs/2026-02-04_18-49-21/sim_2026-02-04_18-49-22.hoot deleted file mode 100644 index 4952d7f6..00000000 Binary files a/logs/2026-02-04_18-49-21/sim_2026-02-04_18-49-22.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-51-40/sim_2026-02-04_18-51-41.hoot b/logs/2026-02-04_18-51-40/sim_2026-02-04_18-51-41.hoot deleted file mode 100644 index a514f3b7..00000000 Binary files a/logs/2026-02-04_18-51-40/sim_2026-02-04_18-51-41.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-54-57/sim_2026-02-04_18-54-58.hoot b/logs/2026-02-04_18-54-57/sim_2026-02-04_18-54-58.hoot deleted file mode 100644 index 4ac6936c..00000000 Binary files a/logs/2026-02-04_18-54-57/sim_2026-02-04_18-54-58.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-57-57/sim_2026-02-04_18-57-59.hoot b/logs/2026-02-04_18-57-57/sim_2026-02-04_18-57-59.hoot deleted file mode 100644 index efd82ed8..00000000 Binary files a/logs/2026-02-04_18-57-57/sim_2026-02-04_18-57-59.hoot and /dev/null differ diff --git a/logs/2026-02-04_18-59-31/sim_2026-02-04_18-59-32.hoot b/logs/2026-02-04_18-59-31/sim_2026-02-04_18-59-32.hoot deleted file mode 100644 index 12e918ca..00000000 Binary files a/logs/2026-02-04_18-59-31/sim_2026-02-04_18-59-32.hoot and /dev/null differ diff --git a/logs/2026-02-04_19-01-18/sim_2026-02-04_19-01-19.hoot b/logs/2026-02-04_19-01-18/sim_2026-02-04_19-01-19.hoot deleted file mode 100644 index 543ad933..00000000 Binary files a/logs/2026-02-04_19-01-18/sim_2026-02-04_19-01-19.hoot and /dev/null differ diff --git a/logs/2026-02-04_19-03-54/sim_2026-02-04_19-03-55.hoot b/logs/2026-02-04_19-03-54/sim_2026-02-04_19-03-55.hoot deleted file mode 100644 index a20d6ee0..00000000 Binary files a/logs/2026-02-04_19-03-54/sim_2026-02-04_19-03-55.hoot and /dev/null differ diff --git a/logs/2026-02-04_19-06-06/sim_2026-02-04_19-06-07.hoot b/logs/2026-02-04_19-06-06/sim_2026-02-04_19-06-07.hoot deleted file mode 100644 index c838f088..00000000 Binary files a/logs/2026-02-04_19-06-06/sim_2026-02-04_19-06-07.hoot and /dev/null differ diff --git a/logs/2026-02-04_19-10-45/sim_2026-02-04_19-10-46.hoot b/logs/2026-02-04_19-10-45/sim_2026-02-04_19-10-46.hoot deleted file mode 100644 index 129dc0f3..00000000 Binary files a/logs/2026-02-04_19-10-45/sim_2026-02-04_19-10-46.hoot and /dev/null differ diff --git a/logs/2026-02-04_19-12-54/sim_2026-02-04_19-12-55.hoot b/logs/2026-02-04_19-12-54/sim_2026-02-04_19-12-55.hoot deleted file mode 100644 index 053c068a..00000000 Binary files a/logs/2026-02-04_19-12-54/sim_2026-02-04_19-12-55.hoot and /dev/null differ diff --git a/logs/2026-02-04_19-15-30/sim_2026-02-04_19-15-31.hoot b/logs/2026-02-04_19-15-30/sim_2026-02-04_19-15-31.hoot deleted file mode 100644 index 4d267936..00000000 Binary files a/logs/2026-02-04_19-15-30/sim_2026-02-04_19-15-31.hoot and /dev/null differ diff --git a/logs/2026-02-04_21-58-38/sim_2026-02-04_21-58-39.hoot b/logs/2026-02-04_21-58-38/sim_2026-02-04_21-58-39.hoot deleted file mode 100644 index b3c566b1..00000000 Binary files a/logs/2026-02-04_21-58-38/sim_2026-02-04_21-58-39.hoot and /dev/null differ diff --git a/logs/2026-02-04_22-02-44/sim_2026-02-04_22-02-45.hoot b/logs/2026-02-04_22-02-44/sim_2026-02-04_22-02-45.hoot deleted file mode 100644 index b1807083..00000000 Binary files a/logs/2026-02-04_22-02-44/sim_2026-02-04_22-02-45.hoot and /dev/null differ diff --git a/logs/2026-02-04_22-04-04/sim_2026-02-04_22-04-05.hoot b/logs/2026-02-04_22-04-04/sim_2026-02-04_22-04-05.hoot deleted file mode 100644 index ce30bbb4..00000000 Binary files a/logs/2026-02-04_22-04-04/sim_2026-02-04_22-04-05.hoot and /dev/null differ diff --git a/logs/2026-02-04_22-06-57/sim_2026-02-04_22-06-58.hoot b/logs/2026-02-04_22-06-57/sim_2026-02-04_22-06-58.hoot deleted file mode 100644 index a8072a95..00000000 Binary files a/logs/2026-02-04_22-06-57/sim_2026-02-04_22-06-58.hoot and /dev/null differ diff --git a/logs/2026-02-09_17-54-37/sim_2026-02-09_17-54-38.hoot b/logs/2026-02-09_17-54-37/sim_2026-02-09_17-54-38.hoot deleted file mode 100644 index 21cb8da8..00000000 Binary files a/logs/2026-02-09_17-54-37/sim_2026-02-09_17-54-38.hoot and /dev/null differ diff --git a/logs/2026-02-09_17-57-05/sim_2026-02-09_17-57-06.hoot b/logs/2026-02-09_17-57-05/sim_2026-02-09_17-57-06.hoot deleted file mode 100644 index 76fb89fb..00000000 Binary files a/logs/2026-02-09_17-57-05/sim_2026-02-09_17-57-06.hoot and /dev/null differ diff --git a/logs/2026-02-09_17-59-52/sim_2026-02-09_17-59-52.hoot b/logs/2026-02-09_17-59-52/sim_2026-02-09_17-59-52.hoot deleted file mode 100644 index 3254af8c..00000000 Binary files a/logs/2026-02-09_17-59-52/sim_2026-02-09_17-59-52.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-01-52/sim_2026-02-09_18-01-52.hoot b/logs/2026-02-09_18-01-52/sim_2026-02-09_18-01-52.hoot deleted file mode 100644 index e61ecc4c..00000000 Binary files a/logs/2026-02-09_18-01-52/sim_2026-02-09_18-01-52.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-04-49/sim_2026-02-09_18-04-49.hoot b/logs/2026-02-09_18-04-49/sim_2026-02-09_18-04-49.hoot deleted file mode 100644 index 30a255f4..00000000 Binary files a/logs/2026-02-09_18-04-49/sim_2026-02-09_18-04-49.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-08-35/sim_2026-02-09_18-08-35.hoot b/logs/2026-02-09_18-08-35/sim_2026-02-09_18-08-35.hoot deleted file mode 100644 index 324d5781..00000000 Binary files a/logs/2026-02-09_18-08-35/sim_2026-02-09_18-08-35.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-09-58/sim_2026-02-09_18-09-58.hoot b/logs/2026-02-09_18-09-58/sim_2026-02-09_18-09-58.hoot deleted file mode 100644 index d647f950..00000000 Binary files a/logs/2026-02-09_18-09-58/sim_2026-02-09_18-09-58.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-15-57/sim_2026-02-09_18-15-58.hoot b/logs/2026-02-09_18-15-57/sim_2026-02-09_18-15-58.hoot deleted file mode 100644 index c43af3b0..00000000 Binary files a/logs/2026-02-09_18-15-57/sim_2026-02-09_18-15-58.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-16-50/sim_2026-02-09_18-16-50.hoot b/logs/2026-02-09_18-16-50/sim_2026-02-09_18-16-50.hoot deleted file mode 100644 index b555aadf..00000000 Binary files a/logs/2026-02-09_18-16-50/sim_2026-02-09_18-16-50.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-23-07/sim_2026-02-09_18-23-07.hoot b/logs/2026-02-09_18-23-07/sim_2026-02-09_18-23-07.hoot deleted file mode 100644 index bbb4f73b..00000000 Binary files a/logs/2026-02-09_18-23-07/sim_2026-02-09_18-23-07.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-31-14/sim_2026-02-09_18-31-14.hoot b/logs/2026-02-09_18-31-14/sim_2026-02-09_18-31-14.hoot deleted file mode 100644 index 7468060c..00000000 Binary files a/logs/2026-02-09_18-31-14/sim_2026-02-09_18-31-14.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-33-55/sim_2026-02-09_18-33-55.hoot b/logs/2026-02-09_18-33-55/sim_2026-02-09_18-33-55.hoot deleted file mode 100644 index fbc33057..00000000 Binary files a/logs/2026-02-09_18-33-55/sim_2026-02-09_18-33-55.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-38-30/sim_2026-02-09_18-38-31.hoot b/logs/2026-02-09_18-38-30/sim_2026-02-09_18-38-31.hoot deleted file mode 100644 index e1b351a8..00000000 Binary files a/logs/2026-02-09_18-38-30/sim_2026-02-09_18-38-31.hoot and /dev/null differ diff --git a/logs/2026-02-09_18-39-32/sim_2026-02-09_18-39-33.hoot b/logs/2026-02-09_18-39-32/sim_2026-02-09_18-39-33.hoot deleted file mode 100644 index a1b25127..00000000 Binary files a/logs/2026-02-09_18-39-32/sim_2026-02-09_18-39-33.hoot and /dev/null differ diff --git a/logs/2026-02-11_14-59-06/sim_2026-02-11_14-59-07.hoot b/logs/2026-02-11_14-59-06/sim_2026-02-11_14-59-07.hoot deleted file mode 100644 index e5890d18..00000000 Binary files a/logs/2026-02-11_14-59-06/sim_2026-02-11_14-59-07.hoot and /dev/null differ diff --git a/logs/2026-02-11_15-04-53/sim_2026-02-11_15-04-53.hoot b/logs/2026-02-11_15-04-53/sim_2026-02-11_15-04-53.hoot deleted file mode 100644 index 8bf09076..00000000 Binary files a/logs/2026-02-11_15-04-53/sim_2026-02-11_15-04-53.hoot and /dev/null differ diff --git a/logs/2026-02-11_15-11-14/sim_2026-02-11_15-11-14.hoot b/logs/2026-02-11_15-11-14/sim_2026-02-11_15-11-14.hoot deleted file mode 100644 index d87aebda..00000000 Binary files a/logs/2026-02-11_15-11-14/sim_2026-02-11_15-11-14.hoot and /dev/null differ diff --git a/logs/2026-02-11_16-16-41/sim_2026-02-11_16-16-43.hoot b/logs/2026-02-11_16-16-41/sim_2026-02-11_16-16-43.hoot deleted file mode 100644 index 6571868e..00000000 Binary files a/logs/2026-02-11_16-16-41/sim_2026-02-11_16-16-43.hoot and /dev/null differ diff --git a/logs/2026-02-11_16-24-34/sim_2026-02-11_16-24-36.hoot b/logs/2026-02-11_16-24-34/sim_2026-02-11_16-24-36.hoot deleted file mode 100644 index d3c98197..00000000 Binary files a/logs/2026-02-11_16-24-34/sim_2026-02-11_16-24-36.hoot and /dev/null differ diff --git a/logs/2026-02-11_16-33-40/sim_2026-02-11_16-33-42.hoot b/logs/2026-02-11_16-33-40/sim_2026-02-11_16-33-42.hoot deleted file mode 100644 index 1c8347d9..00000000 Binary files a/logs/2026-02-11_16-33-40/sim_2026-02-11_16-33-42.hoot and /dev/null differ diff --git a/logs/2026-02-11_17-16-02/sim_2026-02-11_17-16-03.hoot b/logs/2026-02-11_17-16-02/sim_2026-02-11_17-16-03.hoot deleted file mode 100644 index 32813a06..00000000 Binary files a/logs/2026-02-11_17-16-02/sim_2026-02-11_17-16-03.hoot and /dev/null differ diff --git a/logs/2026-02-13_18-00-50/sim_2026-02-13_18-00-50.hoot b/logs/2026-02-13_18-00-50/sim_2026-02-13_18-00-50.hoot deleted file mode 100644 index df644c95..00000000 Binary files a/logs/2026-02-13_18-00-50/sim_2026-02-13_18-00-50.hoot and /dev/null differ diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd3..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/simgui.json b/simgui.json index e6579fff..d01bfcf5 100644 --- a/simgui.json +++ b/simgui.json @@ -1,19 +1,19 @@ { + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/PIVOT SIM": "Mechanism2d", - "/SmartDashboard/Pivot_Mechanism2d_SIM": "Mechanism2d", - "/SmartDashboard/Visualizers/Intake": "Mechanism2d" + "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d" }, "windows": { - "/SmartDashboard/Autonomous": { - "window": { - "visible": true - } - }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, @@ -25,7 +25,7 @@ "visible": true } }, - "/SmartDashboard/Pivot_Mechanism2d_SIM": { + "/SmartDashboard/Visualizers/ClimberHopper": { "window": { "visible": true } @@ -41,75 +41,14 @@ }, "transitory": { "SmartDashboard": { - "INTAKE_SIMULATION": { - "open": true - }, - "Intake": { - "Settings": { - "Pivot": { - "open": true - }, - "open": true - }, + "ClimberHopper": { "open": true }, - "PIVOT SIM": { - "PivotSim": { - "open": true - } - }, "open": true } } }, "NetworkTables Info": { - "Clients": { - "open": true - }, - "Connections": { - "open": true - }, - "Server": { - "open": true - }, "visible": true - }, - "Plot": { - "Plot <0>": { - "plots": [ - { - "backgroundColor": [ - 0.0, - 0.0, - 0.0, - 0.8500000238418579 - ], - "height": 685, - "series": [ - { - "color": [ - 0.2980392277240753, - 0.44705885648727417, - 0.6901960968971252, - 1.0 - ], - "id": "NT:/SmartDashboard/INTAKE_SIMULATION/ Target Angle (DEG)" - }, - { - "color": [ - 0.8666667342185974, - 0.5176470875740051, - 0.32156863808631897, - 1.0 - ], - "id": "NT:/SmartDashboard/INTAKE_SIMULATION/ Actual Angle (DEG)" - } - ] - } - ], - "window": { - "visible": false - } - } } } diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 33d66c09..4fa0dcc5 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,14 +5,15 @@ /***************************************************************/ package com.stuypulse.robot; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.commands.vision.SetMegaTagMode; +import com.stuypulse.robot.subsystems.vision.LimelightVision; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -23,8 +24,7 @@ public class Robot extends TimedRobot { private RobotContainer robot; private Command auto; private static Alliance alliance; - - StructPublisher publisher; + private PowerDistribution powerDistribution; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -41,17 +41,17 @@ public void robotInit() { DataLogManager.start(); SignalLogger.start(); - publisher = NetworkTableInstance.getDefault() - .getStructTopic("DriveTrainPose", Pose3d.struct).publish(); + powerDistribution = new PowerDistribution(); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); - Pose3d drivetrainPose = new Pose3d(CommandSwerveDrivetrain.getInstance().getPose()); - publisher.set(drivetrainPose); - + if (DriverStation.getAlliance().isPresent()) { + alliance = DriverStation.getAlliance().get(); + } } /*********************/ @@ -59,7 +59,9 @@ public void robotPeriodic() { /*********************/ @Override - public void disabledInit() {} + public void disabledInit() { + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG1)); + } @Override public void disabledPeriodic() {} @@ -70,6 +72,8 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + auto = robot.getAutonomousCommand(); if (auto != null) { @@ -89,6 +93,8 @@ public void autonomousExit() {} @Override public void teleopInit() { + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + if (auto != null) { auto.cancel(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ccf3a8b8..8411c15c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,10 +11,12 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; +import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterLeftCorner; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterReverse; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterRightCorner; @@ -24,19 +26,24 @@ import com.stuypulse.robot.commands.intake.IntakeOutake; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveXMode; import com.stuypulse.robot.commands.turret.TurretFerry; +import com.stuypulse.robot.commands.turret.TurretIdle; import com.stuypulse.robot.commands.turret.TurretLeftCorner; import com.stuypulse.robot.commands.turret.TurretRightCorner; +import com.stuypulse.robot.commands.turret.TurretSeed; import com.stuypulse.robot.commands.turret.TurretShoot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; +import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; +import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter; import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -69,14 +76,18 @@ public interface EnabledSubsystems { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem - private final CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final Handoff handoff = Handoff.getInstance(); - private final HoodedShooter hoodedShooter = HoodedShooter.getInstance(); private final Intake intake = Intake.getInstance(); private final Spindexer spindexer = Spindexer.getInstance(); - private final Turret turret = Turret.getInstance(); + + private final CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); private final LimelightVision vision = LimelightVision.getInstance(); + private final Turret turret = Turret.getInstance(); + + private final HoodedShooter hoodedShooter = HoodedShooter.getInstance(); + private final Shooter shooter = Shooter.getInstance(); + private final Hood hood = Hood.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -96,6 +107,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); + climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); } /***************/ @@ -103,20 +115,36 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - driver.getDPadRight() - .whileTrue( - new SwerveXMode().alongWith( - new HoodedShooterShoot().alongWith( - new TurretShoot()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); + + driver.getDPadDown() + .onTrue(new TurretIdle()) + .onTrue(new TurretSeed()); + + driver.getDPadUp() + .onTrue(new SwerveResetHeading()); + + // SCORING ROUTINE + driver.getTopButton() + .whileTrue(new TurretShoot() + .alongWith(new HoodedShooterShoot()) + .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) + .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new HoodedShooterStow()) + .alongWith(new HandoffStop())); + + driver.getTopButton() + .whileTrue(new TurretShoot() + .alongWith(new HoodedShooterInterpolation()) + .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) + .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) + .onFalse(new SpindexerStop() + .alongWith(new HoodedShooterStow()) + .alongWith(new HandoffStop())); // driver.getDPadDown() // .onTrue(new HoodedShooterShoot()) @@ -253,26 +281,6 @@ private void configureButtonBindings() { new HandoffStop().alongWith( new IntakeStop()))) ); - - // // Ferry in place - // driver.getLeftMenuButton() - // .onTrue( - // new FerryRoutine().alongWith(new SwerveXMode())) - // .onFalse( - // new HoodedShooterStow().alongWith( - // new SpindexerRun().alongWith( - // new HandoffStop())) - // ); - - // // Ferry - // driver.getRightMenuButton() - // .onTrue( - // new FerryRoutine()) - // .onFalse( - // new HoodedShooterStow().alongWith( - // new SpindexerRun().alongWith( - // new HandoffStop())) - // ); } /**************/ @@ -297,17 +305,17 @@ public void configureSysids() { // autonChooser.addOption("SysID Module Rotation Quasi Forwards", swerve.sysIdRotQuasi(Direction.kForward)); // autonChooser.addOption("SysID Module Rotation Quasi Backwards", swerve.sysIdRotQuasi(Direction.kReverse)); - // SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); - // autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); - // autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); - // autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); - - // SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); - // autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); - // autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); - // autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); + SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); + autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); + autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); + autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); + autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); + + SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); + autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); + autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); + autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); + autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine(); autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward)); diff --git a/src/main/java/com/stuypulse/robot/commands/BuzzController.java b/src/main/java/com/stuypulse/robot/commands/BuzzController.java index 5ec5db3c..69b6844c 100644 --- a/src/main/java/com/stuypulse/robot/commands/BuzzController.java +++ b/src/main/java/com/stuypulse/robot/commands/BuzzController.java @@ -8,7 +8,7 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.util.StopWatch; -import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.DriverConstants; import edu.wpi.first.wpilibj2.command.Command; @@ -23,13 +23,13 @@ public BuzzController(Gamepad driver) { @Override public void initialize() { - driver.setRumble(Settings.Driver.BUZZ_INTENSITY); + driver.setRumble(DriverConstants.Driver.BUZZ_INTENSITY); timer.reset(); } @Override public boolean isFinished() { - return timer.getTime() >= Settings.Driver.BUZZ_TIME; + return timer.getTime() >= DriverConstants.Driver.BUZZ_TIME; } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/TurretHoodAlignToTarget.java b/src/main/java/com/stuypulse/robot/commands/TurretHoodAlignToTarget.java deleted file mode 100644 index ed9ee5be..00000000 --- a/src/main/java/com/stuypulse/robot/commands/TurretHoodAlignToTarget.java +++ /dev/null @@ -1,111 +0,0 @@ -// package com.stuypulse.robot.commands; - -// import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; -// import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; -// import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -// import com.stuypulse.robot.util.hoodedshooter.ShotCalculator; -// import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.AlignAngleSolution; -// import com.stuypulse.robot.Robot; -// import com.stuypulse.robot.constants.Constants; -// import com.stuypulse.robot.constants.Field; -// import com.stuypulse.robot.constants.Settings; - -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.geometry.Pose3d; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Rotation3d; -// import edu.wpi.first.math.geometry.Twist2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.wpilibj.smartdashboard.Field2d; -// import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.Command; - -// public class TurretHoodAlignToTarget extends Command{ -// private final HoodedShooter hdsr; -// private final CommandSwerveDrivetrain swerve; -// // private final Turret turret; - -// private final Field2d field; - -// private FieldObject2d targetPose2d; -// private FieldObject2d virtualHubPose2d; -// private FieldObject2d futureShooterPose; - - -// public TurretHoodAlignToTarget() { -// hdsr = HoodedShooter.getInstance(); -// swerve = CommandSwerveDrivetrain.getInstance(); - -// field = Field.FIELD2D; -// virtualHubPose2d = field.getObject("virtualHubPose"); -// targetPose2d = field.getObject("targetPose"); -// futureShooterPose = field.getObject("futureShooterPose"); - -// addRequirements(hdsr); -// } - -// @Override -// public void initialize() { - -// } - -// @Override -// public void execute() { -// // update targetPose each tick -// Pose3d targetPose = Pose3d.kZero; -// if (hdsr.getState() == HoodedShooterState.SHOOT) { -// targetPose = Field.hubCenter3d; -// } else if (hdsr.getState() == HoodedShooterState.FERRY){ -// targetPose = new Pose3d(Field.getFerryZonePose(swerve.getPose().getTranslation())); -// } - -// Pose2d currentPose = swerve.getPose(); - -// ChassisSpeeds robotRelSpeeds = swerve.getChassisSpeeds(); -// ChassisSpeeds fieldRelSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( -// robotRelSpeeds, -// currentPose.getRotation() -// ); - -// double axMetersPerSecondSquared = swerve.getPigeon2().getAccelerationX().getValueAsDouble(); -// double ayMetersPerSecondSquared = swerve.getPigeon2().getAccelerationY().getValueAsDouble(); - -// double shooterRPS = hdsr.getTargetRPM() / 60.0; - -// Pose2d shooterPose = currentPose.plus(Constants.HoodedShooter.TURRET_OFFSET).exp( -// new Twist2d( -// robotRelSpeeds.vxMetersPerSecond * Settings.HoodedShooter.UPDATE_DELAY.doubleValue(), -// robotRelSpeeds.vyMetersPerSecond * Settings.HoodedShooter.UPDATE_DELAY.doubleValue(), -// 0 -// ) -// ); - -// AlignAngleSolution sol = ShotCalculator.solveShootOnTheFly( -// new Pose3d(shooterPose.getX(), shooterPose.getY(), Constants.HoodedShooter.TURRET_HEIGHT, Rotation3d.kZero), -// targetPose, -// axMetersPerSecondSquared, -// ayMetersPerSecondSquared, -// fieldRelSpeeds, -// shooterRPS, -// Constants.Align.MAX_ITERATIONS, -// Constants.Align.TIME_TOLERANCE -// ); - -// // hdsr.setTargetAngle(sol.launchPitchAngle()); // this doesn't work with the HoodedShooter structure we want - -// // this is the required yaw for shooting into the effective hub -// Rotation2d targetTurretAngle = sol.requiredYaw().minus(currentPose.getRotation()); - -// // transform for sim since blue is always origin -// targetPose2d.setPose(Robot.isBlue() ? targetPose.toPose2d() : Field.transformToOppositeAlliance(targetPose).toPose2d()); -// virtualHubPose2d.setPose((Robot.isBlue() ? sol.estimateTargetPose() : Field.transformToOppositeAlliance(sol.estimateTargetPose())).toPose2d()); -// futureShooterPose.setPose((Robot.isBlue() ? shooterPose : Field.transformToOppositeAlliance(shooterPose))); - - - -// SmartDashboard.putNumber("HoodedShooter/calculated yaw", targetTurretAngle.getDegrees()); -// SmartDashboard.putNumber("HoodedShooter/launch pitch angle", sol.launchPitchAngle().getDegrees()); -// // TODO: set turret angle here -// } -// } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java new file mode 100644 index 00000000..57036b6f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; + +public class ClimberDown extends ClimberHopperSetState { + public ClimberDown() { + super(ClimberHopperState.CLIMBER_DOWN); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java new file mode 100644 index 00000000..ccd03002 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java @@ -0,0 +1,76 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + + +public class ClimberHopperDefaultCommand extends Command { + private final ClimberHopper climberHopper; + private final CommandSwerveDrivetrain swerve; + private Pose2d pose; + private boolean flag = false; // To prevent repeated stalling under trench + + public ClimberHopperDefaultCommand() { + climberHopper = ClimberHopper.getInstance(); + swerve = CommandSwerveDrivetrain.getInstance(); + pose = swerve.getPose(); + + + addRequirements(climberHopper); + } + + @Override + public void execute() { + // === Robot Position Logic === + // Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left. + pose = swerve.getPose(); + + boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); + + boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); + + boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance; + + boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance + + // === Climber Position and State Logic === + boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN; + + boolean stalledByBalls = climberHopper.getStalling() && inDownState; + // boolean stalledByBalls = true; + + if (isUnderTrench) { + if (!stalledByBalls && !flag) { + climberHopper.setState(ClimberHopperState.HOPPER_DOWN); + } else { + climberHopper.setState(ClimberHopperState.HOPPER_UP); + // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!! + flag = true; // prevent hopper from going back down while still under trench with too many balls + } + } else { // If not under trench... + if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { + // Set the hopper up + climberHopper.setState(ClimberHopperState.HOPPER_UP); + } + } + + if (!isUnderTrench) { + flag = false; + } + + SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java new file mode 100644 index 00000000..18bfb424 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java @@ -0,0 +1,23 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberHopperReset extends Command { + private final ClimberHopper climberHopper; + + public ClimberHopperReset() { + climberHopper = ClimberHopper.getInstance(); + } + + @Override + public void execute() { + + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java new file mode 100644 index 00000000..6849ef97 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java @@ -0,0 +1,26 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.subsystems.climberhopper.*; +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ClimberHopperSetState extends Command { + private final ClimberHopper climberHopper = ClimberHopper.getInstance(); + private final ClimberHopperState state; + + public ClimberHopperSetState(ClimberHopperState state) { + this.state = state; + addRequirements(climberHopper); + } + + @Override + public void initialize() { + climberHopper.setState(state); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java new file mode 100644 index 00000000..d9db87ce --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; + +public class ClimberUp extends ClimberHopperSetState { + public ClimberUp() { + super(ClimberHopperState.CLIMBER_UP); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java new file mode 100644 index 00000000..5eb46595 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; + +public class HopperDown extends ClimberHopperSetState { + public HopperDown() { + super(ClimberHopperState.HOPPER_DOWN); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java new file mode 100644 index 00000000..d4044578 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.climberhopper; + +import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; + +public class HopperUp extends ClimberHopperSetState { + public HopperUp() { + super(ClimberHopperState.HOPPER_UP); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java new file mode 100644 index 00000000..26ad75b4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.hoodedshooter; + +import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; + +public class HoodedShooterInterpolation extends HoodedShooterSetState { + public HoodedShooterInterpolation() { + super(HoodedShooterState.INTERPOLATION); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index fd878f08..df7fb7a9 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -15,8 +15,8 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import com.stuypulse.robot.constants.Settings.Driver.Drive; -import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java index 46c6eff7..0fd73f71 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java @@ -91,7 +91,7 @@ public SwerveDrivePIDToPose(Supplier targetPose) { xTolerance = Settings.Swerve.Alignment.Tolerances.X_TOLERANCE; yTolerance = Settings.Swerve.Alignment.Tolerances.Y_TOLERANCE; - thetaTolerance = Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE.getAsDouble(); + thetaTolerance = Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE_DEG; maxVelocityWhenAligned = Settings.Swerve.Alignment.Tolerances.MAX_VELOCITY_WHEN_ALIGNED; canEnd = () -> true; diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java new file mode 100644 index 00000000..4e5d8412 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java @@ -0,0 +1,26 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetMegaTagMode extends InstantCommand { + + private MegaTagMode megaTagMode; + + public SetMegaTagMode(MegaTagMode mode) { + this.megaTagMode = mode; + } + + @Override + public void initialize() { + super.initialize(); + LimelightVision.getInstance().setMegaTagMode(megaTagMode); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java deleted file mode 100644 index 59334b39..00000000 --- a/src/main/java/com/stuypulse/robot/constants/Constants.java +++ /dev/null @@ -1,68 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.constants; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; - -public interface Constants { - public interface HoodedShooter { - public interface Hood { - public final double GEAR_RATIO = 1290300.0 / 5967.0; - public final double SENSOR_TO_HOOD_RATIO = 360.0 / 36.0; - public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(36.80); - public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12); // TBD - public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10); // TBD - public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10); // TBD - - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(20.0 * 10.0); - } - public interface Shooter { - public final double GEAR_RATIO = 1.0; - } - } - - public interface Align { - int MAX_ITERATIONS = 5; - double TIME_TOLERANCE = 0.01; - } - - public interface Spindexer { - public final double GEAR_RATIO = 40.0 / 12.0; - } - - public interface Turret { - double RANGE = 210.0; - Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-2.50), Units.inchesToMeters(11.19), Rotation2d.kZero); - double TURRET_HEIGHT = Units.inchesToMeters(10.984); - public interface Encoder18t { - public final int TEETH = 18; - public final Rotation2d OFFSET = new Rotation2d(); - } - - public interface Encoder17t { - public final int TEETH = 17; - public final Rotation2d OFFSET = new Rotation2d(); - } - - public interface BigGear { - public final int TEETH = 95; - } - - public interface SoftwareLimit { - public final double FORWARD_MAX_ROTATIONS = 1.5; - public final double BACKWARDS_MAX_ROTATIONS = 1.5; - } - - public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; - } - - public interface Handoff { - public final double GEAR_RATIO = 1.0; - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java new file mode 100644 index 00000000..a997419c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -0,0 +1,29 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.constants; + +import com.stuypulse.stuylib.network.SmartNumber; + +public interface DriverConstants { + + public interface Driver { + double BUZZ_TIME = 1.0; + double BUZZ_INTENSITY = 1.0; + + public interface Drive { + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.05); + + SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); + SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); + } + public interface Turn { + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); + + SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); + SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index bda8f794..f5b4f0eb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -30,6 +30,8 @@ public interface Field { double WIDTH = Units.inchesToMeters(317.000); double LENGTH = Units.inchesToMeters(651.200); + public static final double trenchXTolerance = Units.inchesToMeters(50); + // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); public static final Pose3d hubCenter3d = new Pose3d(hubCenter.getX(), hubCenter.getY(), Units.inchesToMeters(72), Rotation3d.kZero); @@ -60,6 +62,24 @@ public static Pose2d getFerryZonePose(Translation2d robot) { } } + /*** TRENCH COORDINATES ***/ + public interface NearLeftTrench { + public static final Pose2d leftEdge = new Pose2d(Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); + public static final Pose2d rightEdge = new Pose2d(Units.inchesToMeters(182.11), WIDTH - Units.inchesToMeters(50.59), new Rotation2d()); + } + public interface NearRightTrench { + public static final Pose2d leftEdge = new Pose2d(Units.inchesToMeters(182.11), Units.inchesToMeters(50.59), new Rotation2d()); + public static final Pose2d rightEdge = new Pose2d(Units.inchesToMeters(182.11), Units.inchesToMeters(0), new Rotation2d()); + } + public interface FarLeftTrench { + public static final Pose2d leftEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); + public static final Pose2d rightEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), WIDTH - Units.inchesToMeters(50.59), new Rotation2d()); + } + public interface FarRightTrench { + public static final Pose2d leftEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), Units.inchesToMeters(50.59), new Rotation2d()); + public static final Pose2d rightEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), Units.inchesToMeters(0), new Rotation2d()); + } + /*** APRILTAGS ***/ enum NamedTags { diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 2ead207c..d172cde0 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -8,6 +8,17 @@ import com.pathplanner.lib.config.PIDConstants; public class Gains { + + public interface ClimberHopper { + double kP = 1.0; + double kI = 0.0; + double kD = 0.20; + + double kS = 0.0; + double kV = 0.123; + double kA = 0.0; + } + public interface HoodedShooter { public interface Shooter { double kP = 0.45; @@ -66,32 +77,34 @@ public interface Handoff { } public interface Turret { - double kS = 0.179; - double kV = 0.0; - double kA = 0.0; - double kP = 1300.0; double kI = 0.0; double kD = 140.0; + + double kS = 0.179; + double kV = 0.0; + double kA = 0.0; } public interface Swerve { public interface Drive { - double kS = 0.0; - double kV = 0.124; - double kA = 0.0; double kP = 0.1; double kI = 0.0; double kD = 0.0; + + double kS = 0.0; + double kV = 0.124; + double kA = 0.0; } public interface Turn { - double kS = 0.1; - double kV = 2.66; - double kA = 0.0; double kP = 100.0; double kI = 0.0; double kD = 0.5; + + double kS = 0.1; + double kV = 2.66; + double kA = 0.0; } public interface Alignment { @@ -111,8 +124,8 @@ public interface Rotation { double akI = 0.0; double akD = 0.0; - PIDConstants XY = new PIDConstants(0.0, 0.0, 0.0); - PIDConstants THETA = new PIDConstants(0.0, 0.0, 0.0); + PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); + PIDConstants THETA = new PIDConstants(3.0, 0.0, 0.2); } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 3e24572d..bcc80e26 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -35,44 +35,53 @@ * - The Open Loop Ramp Rate */ public interface Motors { + + public interface ClimberHopper { + TalonFXConfig MOTOR = new TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + .withCurrentLimitAmps(50) + .withSupplyCurrentLimitAmps(50) + .withRampRate(Settings.ClimberHopper.RAMP_RATE); + } + public interface HoodedShooter { public interface Shooter { TalonFXConfig SHOOTER = new TalonFXConfig() - // .withSupplyCurrentLimitAmps(100.0) - // .withCurrentLimitAmps(100.0) - // .withRampRate(0.25) - .withCurrentLimitEnable(false) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, - Gains.HoodedShooter.Shooter.kD, 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, - Gains.HoodedShooter.Shooter.kA, 0) - .withSensorToMechanismRatio(Constants.HoodedShooter.Shooter.GEAR_RATIO); + // .withSupplyCurrentLimitAmps(100.0) + // .withCurrentLimitAmps(100.0) + // .withRampRate(0.25) + .withCurrentLimitEnable(false) + .withNeutralMode(NeutralModeValue.Coast) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, + Gains.HoodedShooter.Shooter.kD, 0) + .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, + Gains.HoodedShooter.Shooter.kA, 0) + .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); } public interface Hood { TalonFXConfig HOOD = new TalonFXConfig() - .withCurrentLimitAmps(80) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Hood.kP, Gains.HoodedShooter.Hood.kI, - Gains.HoodedShooter.Hood.kD, 0) - .withFFConstants(Gains.HoodedShooter.Hood.kS, Gains.HoodedShooter.Hood.kV, Gains.HoodedShooter.Hood.kA, 0) - .withSensorToMechanismRatio(Constants.HoodedShooter.Hood.GEAR_RATIO); + .withCurrentLimitAmps(80) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withPIDConstants(Gains.HoodedShooter.Hood.kP, Gains.HoodedShooter.Hood.kI, Gains.HoodedShooter.Hood.kD, 0) + .withFFConstants(Gains.HoodedShooter.Hood.kS, Gains.HoodedShooter.Hood.kV, Gains.HoodedShooter.Hood.kA, 0) + .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO); SoftwareLimitSwitchConfigs hoodSoftwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(Constants.HoodedShooter.Hood.MAX_ANGLE.getRotations()) - .withReverseSoftLimitThreshold(Constants.HoodedShooter.Hood.MIN_ANGLE.getRotations()); + .withForwardSoftLimitThreshold(Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations()) + .withReverseSoftLimitThreshold(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() .withMagnetSensor(new MagnetSensorConfigs() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Constants.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); + .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); } } @@ -83,6 +92,17 @@ public interface Intake { .withNeutralMode(NeutralModeValue.Coast) .withInvertedValue(InvertedValue.Clockwise_Positive); // TODO: add gear ratio, find inversions + TalonFXConfig PIVOT = new TalonFXConfig() + .withCurrentLimitAmps(40) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.Clockwise_Positive) // TODO: find inversions + .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) + .withFFConstants(Gains.Intake.Pivot.kS, Gains.Intake.Pivot.kV, Gains.Intake.Pivot.kA, Gains.Intake.Pivot.kG, 0) + .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); + } + + public interface Spindexeer { TalonFXConfig PIVOT = new TalonFXConfig() .withCurrentLimitAmps(40) .withRampRate(0.25) @@ -101,7 +121,7 @@ public interface Spindexer { .withInvertedValue(InvertedValue.Clockwise_Positive) .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) - .withSensorToMechanismRatio(Constants.Spindexer.GEAR_RATIO); + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); } public interface Turret { @@ -112,7 +132,7 @@ public interface Turret { .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Turret.kP, Gains.Turret.kI, Gains.Turret.kD, 0) .withFFConstants(Gains.Turret.kS, Gains.Turret.kV, Gains.Turret.kA, 0) - .withSensorToMechanismRatio(Constants.Turret.GEAR_RATIO_MOTOR_TO_MECH); + .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); SoftwareLimitSwitchConfigs SOFT_LIMIT = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) @@ -124,13 +144,13 @@ public interface Turret { .withMagnetSensor(new MagnetSensorConfigs() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Constants.Turret.Encoder17t.OFFSET.getRotations())); + .withMagnetOffset(Settings.Turret.Constants.Encoder17t.OFFSET.getRotations())); CANcoderConfiguration ENCODER_18T = new CANcoderConfiguration() .withMagnetSensor(new MagnetSensorConfigs() .withSensorDirection(SensorDirectionValue.Clockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Constants.Turret.Encoder18t.OFFSET.getRotations())); + .withMagnetOffset(Settings.Turret.Constants.Encoder18t.OFFSET.getRotations())); } public interface Handoff { @@ -141,16 +161,7 @@ public interface Handoff { .withInvertedValue(InvertedValue.CounterClockwise_Positive) .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) - .withSensorToMechanismRatio(Constants.Handoff.GEAR_RATIO); - } - - public interface ClimberHopper { - TalonFXConfig MOTOR = new TalonFXConfig() - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - .withCurrentLimitAmps(60) - .withSupplyCurrentLimitAmps(60) - .withRampRate(0.25); + .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); } public static class TalonFXConfig { diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index e89a032b..924ec08a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -43,7 +43,6 @@ public interface Intake { int PIVOT = 8; int ROLLER_LEADER = 9; int ROLLER_FOLLOWER = 10; - int ABSOLUTE_ENCODER = 11; } public interface Spindexer { @@ -52,7 +51,6 @@ public interface Spindexer { } public interface Turret { - // from alphabot int MOTOR = 50; int ENCODER17T = 38; int ENCODER18T = 21; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0e949e4a..7675dea4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; @@ -18,7 +19,7 @@ import com.pathplanner.lib.path.PathConstraints; /*- - * File containing tunable settings for every subsystem on the robot. + * File containing constants and tunable settings for every subsystem on the robot. * * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. @@ -29,57 +30,23 @@ public interface Settings { boolean DEBUG_MODE = true; CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); - public interface ClimberHopper { - // TODO: GET THESE - // Voltages - double CLIMBER_UP = 2; - double CLIMBER_DOWN = -3; - double HOPPER_DOWN = -3; - double HOPPER_UP = 2; - - double MASS_KG = 1; - - double STALL = 10; - double EXTENDED = 0; - double RETRACTED = 0; - - double ROTATIONS_AT_BOTTOM = 0; - // TODO: get these limits - double MIN_HEIGHT_METERS = 0; - double MAX_HEIGHT_METERS = 10; - - double DEBOUNCE = 0.25; - - double GYRO_TOLERANCE = 0; - double HEIGHT_TOLERANCE = 1; - - double RAMP_RATE = 50; - - double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (Encoders.NUM_ROTATIONS_TO_REACH_TOP / Encoders.GEARING)) / 2 / Math.PI; - - public interface Encoders { - // TODO: get these - double GEARING = 52.0/12.0; - - double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13); // Number of rotations that the motor has to spin, NOT the gear - double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; - double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60; - } - } - public interface Handoff { + double GEAR_RATIO = 1.0; + double HANDOFF_STOP = 0.0; double HANDOFF_MAX = 4800.0; double HANDOFF_REVERSE = -500.0; double RPM_TOLERANCE = 200.0; - public final SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/RPM Override", HANDOFF_MAX); + public final SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); } - public interface Intake { // TODO: Get all values for this + public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); - Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(150.0); + Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); - public final Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.1); + public final Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); + public final double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; + public final double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; Rotation2d PIVOT_ANGLE_OFFSET = new Rotation2d(); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190); @@ -88,11 +55,7 @@ public interface Intake { // TODO: Get all values for this Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); - double GEAR_RATIO = 48; - double JKgMetersSquared = 0.001; - - double VOLTAGE_MAX = 12; - double VOLTAGE_MIN = -12; + double GEAR_RATIO = 48.0; } public interface Spindexer { @@ -101,34 +64,34 @@ public interface Spindexer { double STOP_SPEED = 0.0; double RPM_TOLERANCE = 400.0; + + public interface Constants { + double GEAR_RATIO = 8.0 / 1.0; + } } public interface HoodedShooter { - - SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); - SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); - - double SHOOTER_TOLERANCE_RPM = 25.0; - double HOOD_TOLERANCE_DEG = 5.0; + double SHOOTER_TOLERANCE_RPM = 50.0; + double HOOD_TOLERANCE_DEG = 0.5; public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { - // values calculated with kinematics and RPM = 3000. TODO: tuning - {1.0, Units.degreesToRadians(61.329899416056854)}, // meters, radians - {1.5, Units.degreesToRadians(50.64110128774519)}, - {2.0, Units.degreesToRadians(42.43985862934761)}, - {2.5, Units.degreesToRadians(36.18629462556821)}, - {3.0, Units.degreesToRadians(31.36657857810849)}, - {3.5, Units.degreesToRadians(27.587819826188184)}, - {4.0, Units.degreesToRadians(24.570004144436282)}, - {4.5, Units.degreesToRadians(22.116965225162573)}, - {5.0, Units.degreesToRadians(20.090654257188444)} + {1.43, Units.degreesToRadians(21.0)}, // meters, radians + {3.65, Units.degreesToRadians(28.0)}, + {5.32, Units.degreesToRadians(33.5)} + }; + } + public interface RPMInterpolation{ + double[][] distanceRPMInterpolationValues = { + {1.43, 3000.0}, // meters, RPM + {3.65, 3400.0}, + {5.32, 3850.0} }; } - SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/ShootOnTheFly/update delay", 0.00); - - public interface ShooterRPMS { + public interface RPMs { + SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); + SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); public final double REVERSE = -0.0; public final double HUB_RPM = 0.0; public final double LEFT_CORNER_RPM = 0.0; // TBD @@ -136,10 +99,25 @@ public interface ShooterRPMS { public final double STOW = 0.0; // TBD } - public interface ShooterRPMDistances { - public final double RPM1Distance = 0.0; - public final double RPM2Distance = 0.0; - public final double RPM3Distance = 0.0; + public interface Angles { + SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); + SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); + + public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7); + public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(36.80); + public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12); + public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10); + public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10); + } + + public interface Hood { + public final double GEAR_RATIO = 1290300.0 / 5967.0; + public final double SENSOR_TO_HOOD_RATIO = 360.0 / 36.0; + + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); + } + public interface Shooter { + public final double GEAR_RATIO = 1.0; } } @@ -152,47 +130,89 @@ public interface Turret { Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); - } + public interface Constants { + double RANGE = 210.0; - public interface Swerve { - double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; - double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; - public interface Motion { - SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 2.5); - SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 2.5); - SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540)); - SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720)); + Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); + double TURRET_HEIGHT = Units.inchesToMeters(0.0); + public interface Encoder18t { + public final int TEETH = 18; + public final Rotation2d OFFSET = new Rotation2d(); + } - PathConstraints DEFAULT_CONSTRAINTS = - new PathConstraints( - MAX_VELOCITY.get(), - MAX_ACCELERATION.get(), - MAX_ANGULAR_VELOCITY.get(), - MAX_ANGULAR_ACCELERATION.get()); - } + public interface Encoder17t { + public final int TEETH = 17; + public final Rotation2d OFFSET = new Rotation2d(); + } + + public interface BigGear { + public final int TEETH = 95; + } - public interface Turn { - // boolean INVERTED = true; - // double GEAR_RATIO = (150.0 / 7.0); // 21.4285714286 + public interface SoftwareLimit { + public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; + public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; + } + + public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; } + } - public interface Drive { - double L2 = ((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)); // 6.74607175 - double L3 = ((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 6.12244898 - double L4 = ((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 5.35714285714 + public interface ClimberHopper { + public interface Constants { + double GEAR_RATIO = 45.0; + + double MIN_HEIGHT_METERS = 0; + double MAX_HEIGHT_METERS = 1; + + double MASS_KG = 1; - // double WHEEL_DIAMETER = 4; - // double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; + double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13); // Number of rotations that the motor has to spin, NOT the gear + double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; + double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60; - // double GEAR_RATIO = Swerve.Drive.L4; + double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2 / Math.PI; } - + + double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; + double CLIMBER_DOWN_HEIGHT_METERS = 0.2; + double HOPPER_DOWN_HEIGHT_METERS = Constants.MIN_HEIGHT_METERS; + double HOPPER_UP_HEIGHT_METERS = 0.5; + + double STALL = 10.0; + + double ROTATIONS_AT_BOTTOM = 0.0; + + double DEBOUNCE = 0.25; + + double GYRO_TOLERANCE = 0.0; + + double HEIGHT_TOLERANCE_METERS = 0.02; + + double RAMP_RATE = 50.0; + + double MOTOR_VOLTAGE = 3.5; + } + + public interface Vision { + Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); + Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694); + } + + public interface ShootOnTheFly { + int MAX_ITERATIONS = 5; + double TIME_TOLERANCE = 0.01; + } + + public interface Swerve { + double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; + double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; public interface Constraints { double MAX_VELOCITY_M_PER_S = 4.3; double MAX_ACCEL_M_PER_S_SQUARED = 15.0; double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400); double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(900); - + PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( MAX_VELOCITY_M_PER_S, @@ -212,7 +232,7 @@ public interface Constraints { public interface Tolerances { double X_TOLERANCE = Units.inchesToMeters(2.0); double Y_TOLERANCE = Units.inchesToMeters(2.0); - SmartNumber THETA_TOLERANCE = new SmartNumber("Angle Tolerance", 2); + double THETA_TOLERANCE_DEG = 2.0; Pose2d POSE_TOLERANCE = new Pose2d( Units.inchesToMeters(2.0), @@ -224,32 +244,7 @@ public interface Tolerances { double ALIGNMENT_DEBOUNCE = 0.15; } - public interface Targets { - - } + public interface Targets {} } } - - public interface Vision { - Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); - Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694); - } - - public interface Driver { - double BUZZ_TIME = 1.0; - double BUZZ_INTENSITY = 1.0; - - public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.05); - - SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); - SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); - } - public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); - - SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); - SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); - } - } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index 44faed5b..b2d70988 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -27,31 +27,29 @@ public static ClimberHopper getInstance() { } public enum ClimberHopperState { - CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP), - CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN), - HOPPER_UP(Settings.ClimberHopper.HOPPER_UP), - HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN), - HOLDING_UP(Settings.ClimberHopper.EXTENDED), - HOLDING_DOWN(Settings.ClimberHopper.RETRACTED); + CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_HEIGHT_METERS), + CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_HEIGHT_METERS), + HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), + HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN_HEIGHT_METERS); - private double targetVoltage; + private double targetHeight; - private ClimberHopperState(double targetVoltage) { - this.targetVoltage = targetVoltage; + private ClimberHopperState(double targetHeight) { + this.targetHeight = targetHeight; } - public double getTargetVoltage() { - return targetVoltage; + public double getTargetHeight() { + return targetHeight; } } protected ClimberHopperState state; - public ClimberHopper() { + protected ClimberHopper() { this.state = ClimberHopperState.CLIMBER_UP; } - + public ClimberHopperState getState() { return state; } @@ -61,7 +59,10 @@ public void setState(ClimberHopperState state) { } public abstract boolean getStalling(); - public abstract double getPosition(); + public abstract double getCurrentHeight(); + public abstract boolean atTargetHeight(); + + // public abstract void setVoltageOverride(Optional voltage); @Override public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index 6b9848f7..de373ca9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -13,7 +13,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import java.util.Optional; public class ClimberHopperImpl extends ClimberHopper { @@ -21,11 +23,14 @@ public class ClimberHopperImpl extends ClimberHopper { private final BStream stalling; private double voltage; + private Optional voltageOverride; + public ClimberHopperImpl() { super(); motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER); Motors.ClimberHopper.MOTOR.configure(motor); - motor.setPosition(0); // hopper all the way down according to le henry + + motor.setPosition(0); stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); } @@ -35,18 +40,50 @@ public boolean getStalling() { return stalling.getAsBoolean(); } - public double getPosition() { // TODO: convert motor encoder position to meters somehow - return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.Encoders.POSITION_CONVERSION_FACTOR; + @Override + public double getCurrentHeight() { // TODO: convert motor encoder position to meters somehow + return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.Constants.POSITION_CONVERSION_FACTOR; + } + + private boolean isWithinTolerance(double toleranceMeters) { + return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; } + @Override + public boolean atTargetHeight() { + return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); + } + + // @Override + // public void setVoltageOverride(Optional voltage) { + // this.voltageOverride = voltage; + // } + @Override public void periodic() { - voltage = getState().getTargetVoltage(); + super.periodic(); + + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } + } else { + voltage = 0; + } + + // TODO: Figure out some way to reset the encoder reading when stall + // if (atTargetHeight() && getState() == ClimberHopperState.HOPPER_DOWN) { + // if (voltageOverride.isPresent()) { + + // } + // } + + motor.setControl(new VoltageOut(voltage).withEnableFOC(true)); - motor.setVoltage(voltage); SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); SmartDashboard.putNumber("ClimberHopper/Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index dcac6c51..e6d6bbb6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -12,7 +12,9 @@ import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class ClimberHopperSim extends ClimberHopper { + private final ElevatorSim sim; private final ClimberHopperVisualizer visualizer; private double voltage; @@ -22,37 +24,63 @@ public ClimberHopperSim() { sim = new ElevatorSim( DCMotor.getKrakenX60(1), - Settings.ClimberHopper.Encoders.GEARING, - Settings.ClimberHopper.MASS_KG, - Settings.ClimberHopper.DRUM_RADIUS_METERS, - Settings.ClimberHopper.MIN_HEIGHT_METERS, - Settings.ClimberHopper.MAX_HEIGHT_METERS, - true, - 0.0 + Settings.ClimberHopper.Constants.GEAR_RATIO, + Settings.ClimberHopper.Constants.MASS_KG, + Settings.ClimberHopper.Constants.DRUM_RADIUS_METERS, + Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS, + Settings.ClimberHopper.Constants.MAX_HEIGHT_METERS, + false, + Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS ); } - // wrote ts so it would compile (ろく なな) public boolean getStalling() { return sim.getCurrentDrawAmps() > Settings.ClimberHopper.STALL; } - public double getPosition() { + @Override + public double getCurrentHeight() { return sim.getPositionMeters(); } + private boolean isWithinTolerance(double toleranceMeters) { + return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; + } + + @Override + public boolean atTargetHeight() { + return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); + } + + // @Override + // public void setVoltageOverride(Optional voltage) { + // this.voltageOverride = voltage; + // } + @Override public void periodic() { super.periodic(); - voltage = getState().getTargetVoltage(); + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } + } else { + voltage = 0; + } + + // TODO: Figure out some way to reset the encoder reading when stall sim.setInputVoltage(voltage); + SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); SmartDashboard.putNumber("ClimberHopper/Current", sim.getCurrentDrawAmps()); SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - SmartDashboard.putNumber("ClimberHopper/Position", getPosition()); - visualizer.update(getPosition()); + SmartDashboard.putNumber("ClimberHopper/Height", getCurrentHeight()); + visualizer.update(getCurrentHeight()); + sim.update(0.02); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java index 1f299cb9..cc890cf5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java @@ -29,41 +29,41 @@ public static ClimberHopperVisualizer getInstance() { private final MechanismRoot2d climber; ClimberHopperVisualizer() { - canvas = new Mechanism2d(25, 25); - hopper = canvas.getRoot("Hopper", 8, 10); - bot = canvas.getRoot("Bot", 8, 5); - climber = canvas.getRoot("Climber", 4, 5); + canvas = new Mechanism2d(2.5, 2.5); + hopper = canvas.getRoot("Hopper", 0.8, 1); + bot = canvas.getRoot("Bot", 0.8, 0.5); + climber = canvas.getRoot("Climber", 0.4, 0.5); hopper.append(new MechanismLigament2d( "Hopper", - 5, + 0.5, 0, - 5, + 0.5, new Color8Bit(Color.kYellow) ) ); bot.append(new MechanismLigament2d( "Bot", - 10, + 1, 0, - 10, + 1, new Color8Bit(Color.kRed) )); climber.append(new MechanismLigament2d( "Climber", - 2, + 0.2, 90, - 5, + 0.5, new Color8Bit(Color.kOrange) )); } public void update(double height) { - - hopper.setPosition(8, height + 5); + + hopper.setPosition(0.8, height + 0.5); SmartDashboard.putData("Visualizers/ClimberHopper", canvas); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 99f6dacb..d431d722 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -51,7 +51,7 @@ public void periodic() { } else if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withVelocity(getTargetRPM() / 60.0)); + motor.setControl(controller.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java index 32f4039d..286dad3e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -41,9 +41,11 @@ public enum HoodedShooterState { STOW(HoodState.STOW, ShooterState.SHOOT), SHOOT(HoodState.SHOOT, ShooterState.SHOOT), FERRY(HoodState.FERRY, ShooterState.FERRY), - REVERSE(HoodState.IDLE, ShooterState.REVERSE), + REVERSE(HoodState.SHOOT, ShooterState.REVERSE), + HUB(HoodState.HUB, ShooterState.HUB), LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER), - RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER); + RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER), + INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION); private HoodState hoodState; private ShooterState shooterState; @@ -72,6 +74,10 @@ public HoodedShooterState getState(){ return state; } + public boolean bothAtTolerance() { + return isShooterAtTolerance() && isHoodAtTolerance(); + } + public boolean isShooterAtTolerance() { return shooter.atTolerance(); } @@ -107,6 +113,8 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Current RPM", getShooterRPM()); SmartDashboard.putNumber("HoodedShooter/Current Angle", getHoodAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/Angle Error (Deg)", hood.getTargetAngle().getDegrees() - getHoodAngle().getDegrees()); + SmartDashboard.putBoolean("HoodedShooter/Shooter At Tolerance?", isShooterAtTolerance()); SmartDashboard.putBoolean("HoodedShooter/Hood At Tolerance?", isHoodAtTolerance()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java index 36d1d3e5..c2ce8e7e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java @@ -5,8 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.hoodedshooter.hood; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -22,7 +22,7 @@ public abstract class Hood extends SubsystemBase{ instance = new HoodImpl(); } - public static Hood getInstance() { + public static Hood getInstance(){ return instance; } @@ -30,30 +30,34 @@ public enum HoodState { STOW, FERRY, SHOOT, + HUB, LEFT_CORNER, RIGHT_CORNER, - IDLE; + IDLE, + INTERPOLATION; } public Hood() { state = HoodState.STOW; } - public HoodState getState() { + public HoodState getState(){ return state; } - public void setState(HoodState state) { + public void setState(HoodState state){ this.state = state; } public Rotation2d getTargetAngle() { return switch(state) { - case STOW -> Constants.HoodedShooter.Hood.MIN_ANGLE; - case FERRY -> getFerryAngle(); - case SHOOT -> getScoreAngle(); //HoodAngleCalculator.calculateHoodAngleSOTM().get(); - case LEFT_CORNER -> Constants.HoodedShooter.Hood.LEFT_CORNER_ANGLE; - case RIGHT_CORNER -> Constants.HoodedShooter.Hood.RIGHT_CORNER_ANGLE; + case STOW -> Settings.HoodedShooter.Angles.MIN_ANGLE; + case FERRY -> Rotation2d.fromDegrees(30); + case SHOOT -> Rotation2d.fromDegrees(Settings.HoodedShooter.Angles.SHOOT_ANGLE.get()); + case HUB -> Settings.HoodedShooter.Angles.HUB_ANGLE; + case LEFT_CORNER -> Settings.HoodedShooter.Angles.LEFT_CORNER_ANGLE; + case RIGHT_CORNER -> Settings.HoodedShooter.Angles.RIGHT_CORNER_ANGLE; + case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); case IDLE -> getHoodAngle(); }; } @@ -74,12 +78,4 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Hood/Target Angle", getTargetAngle().getDegrees()); SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getHoodAngle().getDegrees()); } - - public Rotation2d getFerryAngle() { - return Rotation2d.fromDegrees(15); - } - - public Rotation2d getScoreAngle() { - return Rotation2d.fromDegrees(15); - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java index 5db238de..4cab66ce 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java @@ -6,7 +6,6 @@ package com.stuypulse.robot.subsystems.hoodedshooter.hood; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -28,6 +27,8 @@ public class HoodImpl extends Hood { private Optional voltageOverride; + private boolean hasUsedAbsoluteEncoder; + public HoodImpl() { hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR); hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER); @@ -37,8 +38,6 @@ public HoodImpl() { hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.hoodSoftwareLimitSwitchConfigs); hoodEncoder.getConfigurator().apply(Motors.HoodedShooter.Hood.HOOD_ENCODER); - hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Constants.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); - controller = new PositionVoltage(getTargetAngle().getRotations()); voltageOverride = Optional.empty(); @@ -53,18 +52,22 @@ public Rotation2d getHoodAngle() { public void periodic() { super.periodic(); + if (!hasUsedAbsoluteEncoder) { + hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); + } + if (EnabledSubsystems.HOOD.get()) { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); + hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations()).withEnableFOC(true)); } } else { hoodMotor.stopMotor(); } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Constants.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); //* 360.0 / (360.0/35.0) / .97); + SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); //* 360.0 / (360.0/35.0) / .97); SmartDashboard.putNumber("HoodedShooter/Hood/Input Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java index d0e3acb9..8dae4c8c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -34,8 +35,10 @@ public enum ShooterState { SHOOT, FERRY, REVERSE, + HUB, LEFT_CORNER, - RIGHT_CORNER; + RIGHT_CORNER, + INTERPOLATION; } public Shooter() { @@ -55,19 +58,20 @@ public double getTargetRPM() { case STOP -> 0; case SHOOT -> getShootRPM(); case FERRY -> getFerryRPM(); - case REVERSE -> Settings.HoodedShooter.ShooterRPMS.REVERSE; - case LEFT_CORNER -> Settings.HoodedShooter.ShooterRPMS.LEFT_CORNER_RPM; - case RIGHT_CORNER -> Settings.HoodedShooter.ShooterRPMS.RIGHT_CORNER_RPM; + case REVERSE -> Settings.HoodedShooter.RPMs.REVERSE; + case HUB -> Settings.HoodedShooter.RPMs.HUB_RPM; + case LEFT_CORNER -> Settings.HoodedShooter.RPMs.LEFT_CORNER_RPM; + case RIGHT_CORNER -> Settings.HoodedShooter.RPMs.RIGHT_CORNER_RPM; + case INTERPOLATION -> HoodAngleCalculator.interpolateShooterRPM().get(); }; } public double getShootRPM() { - return Settings.HoodedShooter.SHOOT_RPM.get(); - // TODO: implement interpolation + return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // will return different speeds in future based on distance to hub } public double getFerryRPM() { - return Settings.HoodedShooter.FERRY_RPM.get(); + return Settings.HoodedShooter.RPMs.FERRY_RPM.get(); } public boolean atTolerance() { @@ -87,4 +91,4 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getShooterRPM()); SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java index 90e511a7..688826a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java @@ -69,7 +69,7 @@ public void periodic() { shooterLeader.setVoltage(voltageOverride.get()); shooterFollower.setControl(follower); } else { - shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0)); + shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); shooterFollower.setControl(follower); } } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index ec669262..43a9462c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -68,8 +68,8 @@ public void periodic() { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { - pivot.setControl(pivotController.withPosition(getState().getTargetAngle().getRotations())); - rollerLeader.setControl(rollerController.withOutput(getState().getTargetDutyCycle())); + pivot.setControl(pivotController.withPosition(getState().getTargetAngle().getRotations()).withEnableFOC(true)); + rollerLeader.setControl(rollerController.withOutput(getState().getTargetDutyCycle()).withEnableFOC(true)); rollerFollower.setControl(follower); } } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 42f8b205..af7e2a82 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -63,8 +63,8 @@ public void periodic() { leadMotor.setVoltage(voltageOverride.get()); followerMotor.setControl(follower); } else { - leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); - followerMotor.setControl(follower); + leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true)); + followerMotor.setControl(follower); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 7030f6bb..4d79f238 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; +import com.stuypulse.robot.subsystems.turret.Turret; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -28,6 +29,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -51,6 +53,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { private final static CommandSwerveDrivetrain instance; + private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); + private Pose2d turretPose = new Pose2d(); + static { instance = TunerConstants.createDrivetrain(); } @@ -420,35 +425,58 @@ public void drive(Vector2D velocity, double rotation) { )); } + public Pose2d getTurretPose() { + return turretPose; + } + @Override public void periodic() { - SmartDashboard.putNumber("Swerve/Pose/X", getPose().getX()); - SmartDashboard.putNumber("Swerve/Pose/Y", getPose().getY()); - SmartDashboard.putNumber("Swerve/Pose/Theta", getPose().getRotation().getDegrees()); + Pose2d pose = getPose(); + turretPose = new Pose2d( + pose.getTranslation().plus(Settings.Turret.Constants.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), + pose.getRotation().plus(Turret.getInstance().getAngle()) + ); + + turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); + + SmartDashboard.putNumber("Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); + + SmartDashboard.putNumber("Swerve/Pose/X", pose.getX()); + SmartDashboard.putNumber("Swerve/Pose/Y", pose.getY()); + SmartDashboard.putNumber("Swerve/Pose/Theta", pose.getRotation().getDegrees()); for (int i = 0; i < 4; i++) { - SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Speed (m per s)", getModule(i).getCurrentState().speedMetersPerSecond); - SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Target Speed (m per s)", getModule(i).getTargetState().speedMetersPerSecond); - SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Angle (deg)", getModule(i).getCurrentState().angle.getDegrees() % 360); - SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Target Angle (deg)", getModule(i).getTargetState().angle.getDegrees() % 360); + String prefix = "Swerve/Modules/Module " + i; + SwerveModuleState current = getModule(i).getCurrentState(); + SwerveModuleState target = getModule(i).getTargetState(); + + + SmartDashboard.putNumber(prefix + "/Speed (m per s)", current.speedMetersPerSecond); + SmartDashboard.putNumber(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); + SmartDashboard.putNumber(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); + SmartDashboard.putNumber(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber(prefix + "/Stator Current", getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber(prefix + "/Supply Current", getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); + } } - Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? getPose() : Field.transformToOppositeAlliance(getPose())); + Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); + if (Settings.DEBUG_MODE) { - for (int i = 0; i < 4; i++) { - SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Stator Current", getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Supply Current", getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); - } - - SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", getChassisSpeeds().vxMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", getChassisSpeeds().vyMetersPerSecond); - - SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", getFieldRelativeSpeeds().x); - SmartDashboard.putNumber("Swerve/Field Relative Rotation", getPose().getRotation().getDegrees()); - SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); - - SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", getChassisSpeeds().omegaRadiansPerSecond); } - } + + SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", getChassisSpeeds().vxMetersPerSecond); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", getChassisSpeeds().vyMetersPerSecond); + + SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", getFieldRelativeSpeeds().x); + SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); + SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + + SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", getChassisSpeeds().omegaRadiansPerSecond); + + SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.hubCenter.getTranslation().getDistance(getPose().getTranslation())); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index b066a2f2..bfa74453 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -10,7 +10,6 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -18,7 +17,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -27,7 +25,6 @@ public abstract class Turret extends SubsystemBase { private static final Turret instance; private TurretState state; private Vector2D driverInput; - private FieldObject2d turret2d; static { instance = Robot.isReal() ? new TurretImpl() : new TurretSim(); @@ -40,8 +37,6 @@ public static Turret getInstance() { public Turret() { driverInput = new Vector2D(0, 0); state = TurretState.IDLE; - - turret2d = Field.FIELD2D.getObject("Turret 2D"); } public void setDriverInput(Gamepad gamepad) { @@ -63,11 +58,11 @@ public Rotation2d getTargetAngle() { return switch (getState()) { case IDLE -> getAngle(); case ZERO -> Rotation2d.kZero; - case FERRYING -> Rotation2d.fromDegrees(0); //TODO: CHANGE TO getFerryAngle(); case SHOOTING -> getScoringAngle(); + case FERRYING -> getFerryAngle(); + case TESTING -> driverInputToAngle(); case HUB -> Settings.Turret.HUB; case LEFT_CORNER -> Settings.Turret.LEFT_CORNER; - case TESTING -> driverInputToAngle(); case RIGHT_CORNER -> Settings.Turret.RIGHT_CORNER; }; } @@ -110,15 +105,6 @@ public void periodic() { SmartDashboard.putString("States/Turret", state.name()); SmartDashboard.putNumber("Turret/Target Angle", getTargetAngle().getDegrees()); - Pose2d robotPose = CommandSwerveDrivetrain.getInstance().getPose(); - - Pose2d turretTranslation = robotPose.plus(Constants.Turret.TURRET_OFFSET); - Rotation2d turretRotation = robotPose.getRotation().plus(getAngle()); - - Pose2d turretPose = new Pose2d(turretTranslation.getTranslation(), turretRotation); - - turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); - if (Settings.DEBUG_MODE) { if (EnabledSubsystems.TURRET.get()) { TurretVisualizer.getInstance().updateTurretAngle(getAngle().plus((Robot.isBlue() ? Rotation2d.kZero : Rotation2d.k180deg)), atTargetAngle()); @@ -129,11 +115,9 @@ public void periodic() { } } - // Should match implementation on mini turret - // Current logic is as of 2/15 public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { Pose2d robotPose = CommandSwerveDrivetrain.getInstance().getPose(); - Pose2d turretPose = robotPose.plus(Constants.Turret.TURRET_OFFSET); + Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); // TODO: TEST IF THIS PLUS SHOULD BE MINUS Vector2D turret = new Vector2D(turretPose.getTranslation()); Vector2D target = new Vector2D(targetPose.getTranslation()); @@ -147,10 +131,9 @@ public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { SmartDashboard.putNumber("Turret/Turret to Target Vector X", turretToTarget.x); SmartDashboard.putNumber("Turret/Turret to Target Vector Y", turretToTarget.y); + SmartDashboard.putNumber("Turret/Target Pose X", targetPose.getX()); SmartDashboard.putNumber("Turret/Target Pose Y", targetPose.getY()); - // SmartDashboard.putNumber("Turret/Robot to Target Vector X", robotToHub.x); - // SmartDashboard.putNumber("Turret/Robot to Target Vector Y", robotToHub.y); SmartDashboard.putNumber("Turret/Zero Vector X", zeroVector.x); SmartDashboard.putNumber("Turret/Zero Vector Y", zeroVector.y); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index 2dd63bea..b190e035 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -7,10 +7,10 @@ import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Turret.Constants; import com.stuypulse.robot.util.SysId; import edu.wpi.first.math.geometry.Rotation2d; @@ -64,23 +64,23 @@ public Rotation2d getAbsoluteTurretAngle() { final Rotation2d encoder17tPosition = getEncoderPos17t(); final double numberOfGearTeethRotated17 = (encoder17tPosition.getRotations() - * (double) Constants.Turret.Encoder17t.TEETH); + * (double) Constants.Encoder17t.TEETH); final Rotation2d encoder18tPosition = getEncoderPos18t(); final double numberOfGearTeethRotated18 = (encoder18tPosition.getRotations() - * (double) Constants.Turret.Encoder18t.TEETH); + * (double) Constants.Encoder18t.TEETH); - final double crt_Partial17 = numberOfGearTeethRotated17 * inverseMod17t * Constants.Turret.Encoder17t.TEETH; - final double crt_Partial18 = numberOfGearTeethRotated18 * inverseMod18t * Constants.Turret.Encoder18t.TEETH; + final double crt_Partial17 = numberOfGearTeethRotated17 * inverseMod17t * Constants.Encoder17t.TEETH; + final double crt_Partial18 = numberOfGearTeethRotated18 * inverseMod18t * Constants.Encoder18t.TEETH; double crt_pos = (crt_Partial17 + crt_Partial18) - % (Constants.Turret.Encoder17t.TEETH * Constants.Turret.Encoder18t.TEETH); + % (Constants.Encoder17t.TEETH * Constants.Encoder18t.TEETH); // Java's % operator is not actually the same as the modulo operator, the lines below account for that - crt_pos = (crt_pos < 0) ? (crt_pos + Constants.Turret.Encoder17t.TEETH * Constants.Turret.Encoder18t.TEETH) + crt_pos = (crt_pos < 0) ? (crt_pos + Constants.Encoder17t.TEETH * Constants.Encoder18t.TEETH) : crt_pos; - final double turretAngle = (crt_pos / (double) Constants.Turret.BigGear.TEETH); + final double turretAngle = (crt_pos / (double) Constants.BigGear.TEETH); return Rotation2d.fromRotations(turretAngle); } @@ -101,7 +101,7 @@ private double getDelta(double target, double current) { if (delta > 180.0) delta -= 360; else if (delta < -180) delta += 360; - if (Math.abs(current + delta) < Constants.Turret.RANGE) return delta; + if (Math.abs(current + delta) < Constants.RANGE) return delta; return delta < 0 ? delta + 360 : delta - 360; } @@ -130,7 +130,7 @@ public void periodic() { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0)); + motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withEnableFOC(true)); } } else { motor.stopMotor(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 4b2fb7e2..79efc83d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -35,6 +35,12 @@ public static LimelightVision getInstance() { private String[] names; private SmartBoolean enabled; private SmartBoolean[] camerasEnabled; + private MegaTagMode megaTagMode; + + public enum MegaTagMode { + MEGATAG1, + MEGATAG2 + } public LimelightVision() { names = new String[Cameras.LimelightCameras.length]; @@ -53,6 +59,7 @@ public LimelightVision() { } camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; + for (int i = 0; i < camerasEnabled.length; i++) { camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); LimelightHelpers.SetIMUMode(names[i], 0); @@ -60,6 +67,7 @@ public LimelightVision() { } enabled = new SmartBoolean("Vision/Is Enabled", true); + megaTagMode = MegaTagMode.MEGATAG1; } public void setTagWhitelist(int... ids) { @@ -84,6 +92,10 @@ public void setCameraEnabled(String name, boolean enabled) { } } + public void setMegaTagMode(MegaTagMode mode) { + this.megaTagMode = mode; + } + public void setIMUMode(int mode) { for (String name : names) { LimelightHelpers.SetIMUMode(name, mode); @@ -97,6 +109,7 @@ public void periodic() { if (camerasEnabled[i].get()) { String limelightName = names[i]; + // Seed robot heading (used by MT2) LimelightHelpers.SetRobotOrientation( limelightName, (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360, @@ -107,25 +120,37 @@ public void periodic() { 0 ); - PoseEstimate poseEstimate = Robot.isBlue() - ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName) - : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); + + PoseEstimate poseEstimate; + + // MegaTag switching + if (megaTagMode == MegaTagMode.MEGATAG1) { + poseEstimate = Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName) + : LimelightHelpers.getBotPoseEstimate_wpiRed(limelightName); + } else { + poseEstimate = Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName) + : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName); + } + // Adding to pose estimator if (poseEstimate != null && poseEstimate.tagCount > 0) { Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; - - //CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MIN_STDDEVS.times(1 + poseEstimate.avgTagDist)); - CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT2_STDEVS); + + CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); + SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); SmartDashboard.putNumber("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", true); - } - else { + } else { SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); } + + SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); } } } diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java index edabe04b..df3faf2f 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -5,9 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.util.hoodedshooter; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.AlignAngleSolution; @@ -15,7 +16,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -24,6 +27,7 @@ public class HoodAngleCalculator { public static InterpolatingDoubleTreeMap distanceAngleInterpolator; + public static InterpolatingDoubleTreeMap distanceRPMInterpolator; static { distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); @@ -32,6 +36,13 @@ public class HoodAngleCalculator { } } + static { + distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { + distanceRPMInterpolator.put(pair[0], pair[1]); + } + } + public static Supplier interpolateHoodAngle() { return () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); @@ -41,9 +52,29 @@ public static Supplier interpolateHoodAngle() { double distanceMeters = hubPose.getDistance(currentPose); - SmartDashboard.putNumber("HoodedShooter/Distance to Hub", distanceMeters); + Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); + + SmartDashboard.putNumber("HoodedShooter/Interpolated Target Angle", targetAngle.getDegrees()); + + return targetAngle; + }; + } + + public static Supplier interpolateShooterRPM() { + return () -> { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Translation2d hubPose = Field.getHubPose().getTranslation(); + Translation2d currentPose = swerve.getPose().getTranslation(); + + double distanceMeters = hubPose.getDistance(currentPose); + + + double targetRPM = distanceRPMInterpolator.get(distanceMeters); - return Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); + SmartDashboard.putNumber("HoodedShooter/Interpolated RPM ", targetRPM); + + return targetRPM; }; } @@ -53,6 +84,7 @@ public static Supplier calculateHoodAngleSOTM() { HoodedShooter hdsr = HoodedShooter.getInstance(); Pose2d currentPose = swerve.getPose(); + Pose2d turretPose = swerve.getTurretPose(); ChassisSpeeds robotRelSpeeds = swerve.getChassisSpeeds(); ChassisSpeeds fieldRelSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( @@ -61,20 +93,22 @@ public static Supplier calculateHoodAngleSOTM() { ); Pose3d targetPose = Field.hubCenter3d; + Pose3d turretPose3d = new Pose3d(new Translation3d(turretPose.getX(), turretPose.getY(), Settings.Turret.Constants.TURRET_HEIGHT), new Rotation3d()); + double axMetersPerSecondSquared = swerve.getPigeon2().getAccelerationX().getValueAsDouble(); double ayMetersPerSecondSquared = swerve.getPigeon2().getAccelerationY().getValueAsDouble(); double shooterRPS = hdsr.getTargetRPM() / 60.0; AlignAngleSolution sol = ShotCalculator.solveShootOnTheFly( - new Pose3d(currentPose.plus(Constants.Turret.TURRET_OFFSET)), + turretPose3d, targetPose, axMetersPerSecondSquared, ayMetersPerSecondSquared, fieldRelSpeeds, // current speeds shooterRPS, - Constants.Align.MAX_ITERATIONS, - Constants.Align.TIME_TOLERANCE + Settings.ShootOnTheFly.MAX_ITERATIONS, + Settings.ShootOnTheFly.TIME_TOLERANCE ); return sol.launchPitchAngle();