From 7912e9ccae423ca6801331de7f4ce483f2849073 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 17:28:20 -0400 Subject: [PATCH 01/16] updated main --- build.gradle | 2 +- vendordeps/AdvantageKit.json | 6 +++--- vendordeps/ChoreoLib2026.json | 6 +++--- vendordeps/REVLib.json | 18 +++++++++--------- ...ib-2026.1.1.json => ReduxLib-2026.1.2.json} | 12 ++++++------ vendordeps/ThriftyLib-2026.json | 4 ++-- 6 files changed, 24 insertions(+), 24 deletions(-) rename vendordeps/{ReduxLib-2026.1.1.json => ReduxLib-2026.1.2.json} (89%) diff --git a/build.gradle b/build.gradle index e9b0020a..322a386e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.1.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 162ad66b..b4e470d3 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "26.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "26.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "26.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json index 322c9e20..48751078 100644 --- a/vendordeps/ChoreoLib2026.json +++ b/vendordeps/ChoreoLib2026.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2026.json", "name": "ChoreoLib", - "version": "2026.0.1", + "version": "2026.0.2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2026.0.1" + "version": "2026.0.2" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e5938..c6b1b9e4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.5", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.5" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/ReduxLib-2026.1.1.json b/vendordeps/ReduxLib-2026.1.2.json similarity index 89% rename from vendordeps/ReduxLib-2026.1.1.json rename to vendordeps/ReduxLib-2026.1.2.json index 09dd4f08..46e64b20 100644 --- a/vendordeps/ReduxLib-2026.1.1.json +++ b/vendordeps/ReduxLib-2026.1.2.json @@ -1,7 +1,7 @@ { - "fileName": "ReduxLib-2026.1.1.json", + "fileName": "ReduxLib-2026.1.2.json", "name": "ReduxLib", - "version": "2026.1.1", + "version": "2026.1.2", "frcYear": "2026", "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2026.1.1" + "version": "2026.1.2" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-fifo", - "version": "2026.1.1", + "version": "2026.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2026.1.1", + "version": "2026.1.2", "libName": "ReduxLib", "headerClassifier": "headers", "sourcesClassifier": "sources", @@ -52,7 +52,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-fifo", - "version": "2026.1.1", + "version": "2026.1.2", "libName": "reduxfifo", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/ThriftyLib-2026.json b/vendordeps/ThriftyLib-2026.json index 9aa58086..9ad94e71 100644 --- a/vendordeps/ThriftyLib-2026.json +++ b/vendordeps/ThriftyLib-2026.json @@ -1,7 +1,7 @@ { "fileName": "ThriftyLib-2026.json", "name": "ThriftyLib", - "version": "2026.0.1", + "version": "2026.1.2", "frcYear": "2026", "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.thethriftybot.frc", "artifactId": "ThriftyLib-java", - "version": "2026.0.1" + "version": "2026.1.2" } ], "jniDependencies": [], From 5c02216859a94d82310b47db6396f3953b81f4e1 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 17:31:47 -0400 Subject: [PATCH 02/16] further updates --- ...6.1.0.json => Phoenix6-replay-26.1.3.json} | 70 +++++++++---------- ...sl-2026.1.20.json => yagsl-2026.3.12.json} | 18 +---- 2 files changed, 38 insertions(+), 50 deletions(-) rename vendordeps/{Phoenix6-replay-26.1.0.json => Phoenix6-replay-26.1.3.json} (92%) rename vendordeps/{yagsl-2026.1.20.json => yagsl-2026.3.12.json} (65%) diff --git a/vendordeps/Phoenix6-replay-26.1.0.json b/vendordeps/Phoenix6-replay-26.1.3.json similarity index 92% rename from vendordeps/Phoenix6-replay-26.1.0.json rename to vendordeps/Phoenix6-replay-26.1.3.json index e49c760a..e4dd4e45 100644 --- a/vendordeps/Phoenix6-replay-26.1.0.json +++ b/vendordeps/Phoenix6-replay-26.1.3.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-replay-26.1.0.json", + "fileName": "Phoenix6-replay-26.1.3.json", "name": "CTRE-Phoenix (v6) Replay", - "version": "26.1.0", + "version": "26.1.3", "frcYear": "2026", "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,7 +48,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "api-cpp-replay", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -62,7 +62,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +76,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,7 +118,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -132,7 +132,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -146,7 +146,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -160,7 +160,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -174,7 +174,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -202,7 +202,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -216,7 +216,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -230,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -246,7 +246,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -259,7 +259,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "wpiapi-cpp-replay", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPIReplay", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools_Replay", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -448,7 +448,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -464,7 +464,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -480,7 +480,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/yagsl-2026.1.20.json b/vendordeps/yagsl-2026.3.12.json similarity index 65% rename from vendordeps/yagsl-2026.1.20.json rename to vendordeps/yagsl-2026.3.12.json index 40d4b399..c0f23969 100644 --- a/vendordeps/yagsl-2026.1.20.json +++ b/vendordeps/yagsl-2026.3.12.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl-2026.1.20.json", + "fileName": "yagsl-2026.3.12.json", "name": "YAGSL", - "version": "2026.1.20", + "version": "2026.3.12", "frcYear": "2026", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2026.1.20" + "version": "2026.3.12" }, { "groupId": "org.dyn4j", @@ -34,18 +34,6 @@ "offlineFileName": "REVLib.json", "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json" }, - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Phoenix6 Replay is required!", - "offlineFileName": "Phoenix6-replay-26.1.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json" - }, - { - "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", - "errorMessage": "Phoenix5 Replay Compatibility is required!", - "offlineFileName": "Phoenix5-replay-5.36.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-replay-frc2026-latest.json" - }, { "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "errorMessage": "ThriftyLib is required!", From f23e87baf9220d3054221f207e89aea87ddb0a43 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 17:41:18 -0400 Subject: [PATCH 03/16] some more --- .idea/compiler.xml | 7 +++++-- .idea/modules.xml | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/.idea/compiler.xml b/.idea/compiler.xml index 3b8c5d3b..7e4c0622 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -5,12 +5,15 @@ - - + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml index 1a56902e..e4f69e03 100644 --- a/.idea/modules.xml +++ b/.idea/modules.xml @@ -2,7 +2,7 @@ - + \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 8d488b33..75d50fa8 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick From f857a26000bd2d1634062ba527e2e43340c2b4ef Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 17:44:09 -0400 Subject: [PATCH 04/16] some more --- .idea/modules.xml | 8 -------- gradle/wrapper/gradle-wrapper.properties | 5 ++--- 2 files changed, 2 insertions(+), 11 deletions(-) delete mode 100644 .idea/modules.xml diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index e4f69e03..00000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 34bd9ce9..246322d2 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,7 +1,6 @@ +#Mon Mar 30 17:42:42 EDT 2026 distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip -networkTimeout=10000 -validateDistributionUrl=true +distributionUrl=https\://services.gradle.org/distributions/gradle-8.14.4-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists From 00a5dbdd4ab183ac29412a2b85c5afdc430f7b50 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 22:15:07 -0400 Subject: [PATCH 05/16] WIP --- src/main/java/frc/robot/RobotContainer.java | 22 ++++++--- .../frc/robot/constants/GameConstants.java | 1 + .../robot/subsystems/ControllerSubsystem.java | 47 ++++++++++++++++--- .../robot/subsystems/ShooterSubsystem.java | 13 +++++ .../frc/robot/subsystems/TurretSubsystem.java | 6 ++- .../swervedrive/SwerveSubsystem.java | 10 ++-- 6 files changed, 82 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 967bf250..390a2912 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import choreo.auto.AutoFactory; import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -81,6 +82,9 @@ import frc.robot.utils.simulation.RobotVisualizer; import swervelib.SwerveInputStream; import swervelib.imu.SwerveIMU; +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnField; + /** * This class is where the bulk of the robot should be declared. Since @@ -145,15 +149,16 @@ public RobotContainer() { turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo()); climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); - shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); + RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); gyroSubsystem = new GyroSubsystem(gyroIo); SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro); drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this) : null; + shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this, turretSubsystem) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case REPLAY -> { @@ -163,14 +168,15 @@ public RobotContainer() { climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); - shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); + intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; + shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this, turretSubsystem) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } @@ -182,13 +188,14 @@ public RobotContainer() { climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer)); - shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); + intakeDeployer = new IntakeDeployerSubsystem( IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null; + shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this, turretSubsystem) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); @@ -203,6 +210,7 @@ public RobotContainer() { autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem, feederSubsystem, hopperSubsystem, turretSubsystem, anglerSubsystem, controllerSubsystem, intakeDeployer); configureBindings(); putShuffleboardCommands(); + } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 75d50fa8..c187d8c3 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -178,4 +178,5 @@ public enum Mode { public static final double BLUE_SHUTTLING_TARGET_X_POSITION = 2.8; public static final double SHUTTLING_TARGET_LOWER_Y_POSITION = 2.6; public static final double SHUTTLING_TARGET_HIGHER_Y_POSITION = 5.5; + public static final double PIECES_PER_SECOND = 0.5; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 6158ba3c..2b982dbc 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,9 +1,10 @@ package frc.robot.subsystems; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; +import frc.robot.constants.enums.ShootingState; import frc.robot.utils.math.TurretCalculations; import java.util.ArrayList; @@ -13,9 +14,6 @@ import frc.robot.RobotContainer; import frc.robot.commands.intakeDeployment.ToggleDeployment; -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.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,6 +22,11 @@ import frc.robot.constants.enums.DeploymentState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; +import swervelib.simulation.ironmaple.utils.FieldMirroringUtils; + +import javax.naming.ldap.Control; public class ControllerSubsystem extends SubsystemBase { @@ -73,13 +76,16 @@ public class ControllerSubsystem extends SubsystemBase { private ShootState previousState; private ShotTargets activeTargets; private boolean driverActivatedShooting = false; + private final TurretSubsystem turret; + private double lastShot; - public ControllerSubsystem(SwerveSubsystem drivebase, IntakeDeployerSubsystem intakeDeployer, RobotContainer robotContainer) { + public ControllerSubsystem(SwerveSubsystem drivebase, IntakeDeployerSubsystem intakeDeployer, RobotContainer robotContainer, TurretSubsystem turret) { this.drivebase = drivebase; this.robotContainer = robotContainer; this.previousState = getCurrentShootState(); this.activeTargets = STOPPED_TARGETS; this.intakeDeployer = intakeDeployer; + this.turret = turret; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -111,11 +117,39 @@ public void periodic() { Logger.recordOutput(TARGET_HOPPER_SPEED_KEY, activeTargets.hopperSpin); } previousState = currentState; + Pose3d[] fuelPoses = SimulatedArena.getInstance() + .getGamePiecesArrayByType("Fuel"); + // Publish to telemetry using AdvantageKit + Logger.recordOutput("FieldSimulation/FuelPositions", fuelPoses); + if (Logger.getTimestamp()-lastShot>1000000/Constants.PIECES_PER_SECOND) { + lastShot = Logger.getTimestamp(); + if (robotContainer.getShootingState().getShootState() != ShootingState.ShootState.STOPPED) { + SimulatedArena.getInstance() + .addGamePieceProjectile(new RebuiltFuelOnFly( + drivebase.getSimulationPose().get().getTranslation(), + new Translation2d(), // shooter offet from center + drivebase.getFieldVelocity(), + Rotation2d.fromDegrees(turret.getLastAngle()-180).plus(drivebase.getHeading()), + Units.Meters.of(0.4), // initial height of the ball, in meters + Units.MetersPerSecond.of(calculateShotVelocity(activeTargets.shooterVelocityRpm)), // initial velocity, in m/s + Units.Degrees.of(90-activeTargets.anglerAngleDegrees)) // shooter angle + .enableBecomesGamePieceOnFieldAfterTouchGround() + .withProjectileTrajectoryDisplayCallBack( + (poses) -> Logger.recordOutput("successfulShotsTrajectory", poses.toArray(Pose3d[]::new)), + (poses) -> Logger.recordOutput("missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + } + } } private ShootState getCurrentShootState() { return robotContainer.getShootingState().getShootState(); } + private double calculateShotVelocity(double rpm) { + double gearRatio = 0.75; //MotorRotations/WheelRotations + double radiusWheel = 0.0508; + double efficiencyConstant = 0.775; + return -rpm*Math.PI/gearRatio/60*radiusWheel*efficiencyConstant; + } private Pose2d getRobotPose() { boolean useManualPose = shouldUseManualPose(); @@ -305,6 +339,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc }else if(state == ShootState.SHUTTLING){ return (((-distance*distance) - 5 * distance) - 2800); } + System.out.println("usingDefault"); return profile.defaultShooterVelocityRpm; } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9e89cd0e..ed1cd8b8 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -10,10 +10,17 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.diag.DiagSparkMaxEncoder; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.motor.RealSparkMaxIo; @@ -27,6 +34,11 @@ import frc.robot.utils.motor.TunablePIDManager; import frc.robot.utils.simulation.MotorSimulator; import frc.robot.utils.simulation.RobotVisualizer; +import org.littletonrobotics.junction.Logger; +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnField; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; +import swervelib.simulation.ironmaple.utils.FieldMirroringUtils; public class ShooterSubsystem extends SubsystemBase { @@ -76,6 +88,7 @@ public void setPidVelocity(double velocity) { public void periodic() { pidManager.periodic(); io.periodic(); + } public static SparkMaxPidMotorIo createMockIo() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 8c3cf7cc..bd26f9f4 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -64,7 +64,7 @@ public void setAngle(double targetAngle) { Constants.TURRET_MIN_ANGLE); if (lastAngle != targetAngle) { setPosition(targetRotations); - lastAngle = targetAngle; + lastAngle = targetAngle; //TODO: Change This } } @@ -80,6 +80,10 @@ public static double calculateRotationsForAngle( return MathUtil.clamp(targetEncoder, encoderLow, encoderHigh); } + public double getLastAngle() { + return lastAngle; + } + /** * Drive forward at the homing speed. Command should stop when limit is hit. */ diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ef7becba..cbc9920c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -8,15 +8,14 @@ import edu.wpi.first.math.Vector; import static edu.wpi.first.units.Units.Meter; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -38,6 +37,9 @@ import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; import swervelib.parser.SwerveParserWithImu; +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; +import swervelib.simulation.ironmaple.utils.FieldMirroringUtils; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -71,6 +73,7 @@ public class SwerveSubsystem extends SubsystemBase { * @param swerveIMU The IMU implementation to provide to the SwerveDrive */ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { + SimulatedArena.getInstance().resetFieldForAuto(); Pose2d startingPose = new Pose2d(new Translation2d(Meter.of(0), Meter.of(4)), Rotation2d.fromDegrees(0)); @@ -124,6 +127,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig public void periodic() { //add vision pose here //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d())); + } @Override From ada50b0d8c25d789505d30ce2a7aba1c79dc222d Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 23:43:19 -0400 Subject: [PATCH 06/16] WIP --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/constants/GameConstants.java | 17 ++- .../robot/subsystems/ControllerSubsystem.java | 106 ++++++++++++------ .../swervedrive/SwerveSubsystem.java | 1 - .../robot/utils/math/TurretCalculations.java | 78 +++++-------- 5 files changed, 105 insertions(+), 99 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4b05d7bb..5c05dde0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -142,7 +142,7 @@ public void robotPeriodic() { SmartDashboard.putBoolean("Hub Active?", hubActive()); if (Constants.currentMode == Constants.Mode.SIM) { Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); - Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationPose().get()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationRawOdomPose()); } } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index c187d8c3..2bd26250 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,8 +1,6 @@ package frc.robot.constants; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -142,12 +140,11 @@ public enum Mode { public static final double GRAVITY = 9.81; public static final double HUB_HEIGHT = 1.83; public static final double SHOOTER_HEIGHT = 0.5; - public static final double BLUE_HUB_X_POSITION = 4.6256; - public static final double BLUE_HUB_Y_POSITION = 4.0345; - public static final double RED_HUB_X_POSITION = 11.9154; - public static final double RED_HUB_Y_POSITION = 4.0345; - public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .05; - public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .12; + public static final Pose2d BLUE_HUB_POS = new Pose2d(4.6256, 4.0345, new Rotation2d()); + public static final Pose2d RED_HUB_POS = new Pose2d(11.9154, 4.0345, new Rotation2d()); + public static final Transform2d TURRET_OFFSET = new Transform2d(0.05, 0.12, new Rotation2d()); //TODO: needs value from hardware + public static final boolean ACCOUNT_FOR_ANGULAR_MOMENTUM = true; + public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = TURRET_OFFSET.getTranslation().getNorm(); // Shift timings public static final int SHIFT_1_START = 10; @@ -178,5 +175,5 @@ public enum Mode { public static final double BLUE_SHUTTLING_TARGET_X_POSITION = 2.8; public static final double SHUTTLING_TARGET_LOWER_Y_POSITION = 2.6; public static final double SHUTTLING_TARGET_HIGHER_Y_POSITION = 5.5; - public static final double PIECES_PER_SECOND = 0.5; + public static final double PIECES_PER_SECOND = 3; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 2b982dbc..6cddc59b 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; @@ -34,10 +35,8 @@ public class ControllerSubsystem extends SubsystemBase { private static final double SHOOT_DELAY_SECONDS = 0.5; // Placeholder target poses until real field target values are finalized - private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, - Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero); - private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, - Constants.RED_HUB_Y_POSITION, Rotation2d.kZero); + private static final Pose2d BLUE_HUB_TARGET_POSE = Constants.BLUE_HUB_POS; + private static final Pose2d RED_HUB_TARGET_POSE = Constants.RED_HUB_POS; private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY"; private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation"; @@ -96,9 +95,10 @@ public ControllerSubsystem(SwerveSubsystem drivebase, IntakeDeployerSubsystem in public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); + ChassisSpeeds robotSpeeds = getRobotSpeeds(); updateStopDelayState(currentState); updateShootDelayState(currentState); - updateTargets(currentState, robotPose); + updateTargets(currentState, robotPose, robotSpeeds); if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); @@ -117,13 +117,10 @@ public void periodic() { Logger.recordOutput(TARGET_HOPPER_SPEED_KEY, activeTargets.hopperSpin); } previousState = currentState; - Pose3d[] fuelPoses = SimulatedArena.getInstance() - .getGamePiecesArrayByType("Fuel"); - // Publish to telemetry using AdvantageKit - Logger.recordOutput("FieldSimulation/FuelPositions", fuelPoses); if (Logger.getTimestamp()-lastShot>1000000/Constants.PIECES_PER_SECOND) { lastShot = Logger.getTimestamp(); if (robotContainer.getShootingState().getShootState() != ShootingState.ShootState.STOPPED) { + Translation2d target = (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? BLUE_HUB_TARGET_POSE: RED_HUB_TARGET_POSE).getTranslation(); SimulatedArena.getInstance() .addGamePieceProjectile(new RebuiltFuelOnFly( drivebase.getSimulationPose().get().getTranslation(), @@ -133,10 +130,14 @@ public void periodic() { Units.Meters.of(0.4), // initial height of the ball, in meters Units.MetersPerSecond.of(calculateShotVelocity(activeTargets.shooterVelocityRpm)), // initial velocity, in m/s Units.Degrees.of(90-activeTargets.anglerAngleDegrees)) // shooter angle - .enableBecomesGamePieceOnFieldAfterTouchGround() + .withTargetPosition(()-> new Translation3d(target.getX(), target.getY(), 1.82)) + // Set the tolerance: x: ±0.5m, y: ±1.2m, z: ±0.3m (this is the size of the speaker's "mouth") + .withTargetTolerance(new Translation3d(0.4, 0.4, 0.15)) + .disableBecomesGamePieceOnFieldAfterTouchGround() .withProjectileTrajectoryDisplayCallBack( (poses) -> Logger.recordOutput("successfulShotsTrajectory", poses.toArray(Pose3d[]::new)), (poses) -> Logger.recordOutput("missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + } } } @@ -145,11 +146,14 @@ private ShootState getCurrentShootState() { return robotContainer.getShootingState().getShootState(); } private double calculateShotVelocity(double rpm) { - double gearRatio = 0.75; //MotorRotations/WheelRotations - double radiusWheel = 0.0508; - double efficiencyConstant = 0.775; + double gearRatio = 1; //MotorRotations/WheelRotations + double radiusWheel = 0.123825; + double efficiencyConstant = 0.42; return -rpm*Math.PI/gearRatio/60*radiusWheel*efficiencyConstant; } + private ChassisSpeeds getRobotSpeeds() { + return drivebase.getFieldVelocity(); + } private Pose2d getRobotPose() { boolean useManualPose = shouldUseManualPose(); @@ -187,7 +191,7 @@ private void updateShootDelayState(ShootState currentState) { } } - private void updateTargets(ShootState state, Pose2d robotPose) { + private void updateTargets(ShootState state, Pose2d robotPose, ChassisSpeeds robotSpeeds) { if(!activeTargets.intakeDeploy && intakeDeployer.getDeploymentState() == DeploymentState.DOWN){ new ToggleDeployment(intakeDeployer, this).schedule(); } @@ -200,9 +204,9 @@ private void updateTargets(ShootState state, Pose2d robotPose) { // No color, do nothing... useShotTargets(FIXED_TARGETS); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { - useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); + useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPose, robotSpeeds)); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); + useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPose, robotSpeeds)); } else { // Unknown color, do nothing... useShotTargets(FIXED_TARGETS); @@ -212,9 +216,9 @@ private void updateTargets(ShootState state, Pose2d robotPose) { if (Robot.allianceColor().isEmpty()) { useShotTargets(FIXED_TARGETS); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { - useShotTargets(calculateTargetsFromPose(state,BLUE_SHUTTLE_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(state,BLUE_SHUTTLE_PROFILE, robotPose, robotSpeeds)); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(state, RED_SHUTTLE_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(state, RED_SHUTTLE_PROFILE, robotPose, robotSpeeds)); } else { useShotTargets(FIXED_TARGETS); } @@ -276,11 +280,17 @@ private boolean isTurretTargetOutOfRange(double turretAngleDegrees) { || turretAngleDegrees > Constants.TURRET_MAX_ANGLE; } - private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) { - double computedDistanceMeters = calculateDistanceMeters(state, robotPose, profile.targetPose); - double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, profile); - double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, profile); - double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, profile); + private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) { + Twist2d momentumAdjustment = getMomentumAdjustment(robotPose, Constants.ACCOUNT_FOR_ANGULAR_MOMENTUM, robotSpeeds, + equalizeFlightTime(calculateDistanceMeters(state, robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds)); + PoseControlProfile adjustedProfile = new PoseControlProfile(profile.targetPose, profile.defaultAnglerAngleDegrees, profile.defaultShooterVelocityRpm, profile.defaultTurretAngleDegrees); + adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment); + Logger.recordOutput("adjustedAimPoint", adjustedProfile.targetPose); + double computedDistanceMeters = calculateDistanceMeters(state, robotPose, adjustedProfile.targetPose); + boolean shootHub = profile == BLUE_HUB_PROFILE || profile == RED_HUB_PROFILE; + double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, adjustedProfile); + double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, adjustedProfile); + double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, adjustedProfile); return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true, true); } @@ -315,19 +325,23 @@ private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPos } return predictedPose; } - //Flight time derived from testing videos + // Linear regression through 3 static shot distance vs time points. private double calculateFlightTime(double computedDistanceMeters) { return 0.208*computedDistanceMeters + 0.647; } + // Since t(x)=mx+b (previous function), we can solve for x + // t = m*(d - v_robot * t)+b + // t + v_robot * t = m * d + b + // t = (m * d + b) / (v_robot + 1) + private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) { + return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond+1); + } + private double calculateAnglerAngleDegrees(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { - if (state == ShootState.SHOOTING_HUB) { - double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; - return 0.169 * distance * distance - - 1.73 * distance - + 20.4; - } - return profile.defaultAnglerAngleDegrees; + double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; + return 0.169 * distance * distance + - 1.73 * distance +20.4; } private double calculateShooterVelocity(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { @@ -346,13 +360,35 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) { if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING){ return Math.floor( - Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(), - robotPose.getRotation().getRadians(), + Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose,profile.targetPose, Robot.allianceColor().get() == DriverStation.Alliance.Blue))); } return profile.defaultTurretAngleDegrees; } - + public Twist2d getMomentumAdjustment(Pose2d robotPose, boolean useAngularMomentumAdjustment, ChassisSpeeds robotSpeeds, double timeOfFlight) { + // calculate the change per second of the turret's position relative to the center due to robot rotation. This + // is basically angular speed. Result is in robot coordinate system. + ChassisSpeeds adjustSpeeds; + if (useAngularMomentumAdjustment) { + Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation(); + + + // take the robot's current speed (relative to field) + // convert the turret's angular velocity from robot-relative to field-relative + // since the rotation velocity is perpendicular to the robot x-axis, so we need to rotate it by 90 degrees + // which is why we use -Y,X instead of X,Y for the conversion. + // add the two to get the effective speed of the turret/projectile. + adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus + (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(), angularVelocityAdjustment.getX(), 0.0, robotPose.getRotation())); + } else { + adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)); + } + // convert the effective speed to a twist (displacement over time) + // positive timeOfFlight tells us how much we move + // negative timeOfFlight tells us how much we need to move to compensate for our movement + timeOfFlight = 0; + return adjustSpeeds.toTwist2d(-timeOfFlight); + } // Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { return activeTargets.anglerAngleDegrees; @@ -422,7 +458,7 @@ private ShotTargets( // Class to save all the target poses and each subsystem position at that point // so we can calculate true values later on private static final class PoseControlProfile { - private final Pose2d targetPose; + private Pose2d targetPose; private final double defaultAnglerAngleDegrees; private final double defaultShooterVelocityRpm; private final double defaultTurretAngleDegrees; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index cbc9920c..09dd6123 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -73,7 +73,6 @@ public class SwerveSubsystem extends SubsystemBase { * @param swerveIMU The IMU implementation to provide to the SwerveDrive */ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { - SimulatedArena.getInstance().resetFieldForAuto(); Pose2d startingPose = new Pose2d(new Translation2d(Meter.of(0), Meter.of(4)), Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index ce98ced9..a78e246d 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -1,28 +1,30 @@ package frc.robot.utils.math; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; -import frc.robot.constants.enums.ShootingState.ShootState; public class TurretCalculations { - /* - * - * This is a utility class that takes the robot position and provides the pan angle. + /* + * + * This is a utility class that takes the robot position and provides the pan angle. * The pan angle is the angle that the turret should be from the horizontal in radians. * For example, when the turret is facing directly forward (with respect to the robot), the pan - * angle would be pi/2 radians (1.57). - * + * angle would be pi/2 radians (1.57). + * * This class takes the robot's x and y positions: robotPosX and robotPosY. These are the - * distances (in meters) from the origin. The origin is defined as the bottom right corner + * distances (in meters) from the origin. The origin is defined as the bottom right corner * of the blue alliance. - * + * */ // All angles are in radians, all distances are in meters, all velocities in m/s @@ -34,61 +36,33 @@ public class TurretCalculations { // all positions are in meters // robotRotation is in radians // Returns - turret angle in radians - public static double calculateTurretAngle(ShootState state, double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { - - // Get turret offset from robot center - double turretOffsetX = GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - double turretOffsetY = GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET; + public static double calculateTurretAngle(Pose2d robotPos,Pose2d adjTargetPose, boolean isBlueAlliance) { - // Rotate the offset by robot rotation (offset rotates with robot) - double rotatedOffsetX = turretOffsetX * Math.cos(robotRotation) - turretOffsetY * Math.sin(robotRotation); - double rotatedOffsetY = turretOffsetX * Math.sin(robotRotation) + turretOffsetY * Math.cos(robotRotation); + Pose2d turretPos = robotPos.transformBy(GameConstants.TURRET_OFFSET); - // Calculate actual turret position on field - double turretPosX = robotPosX + rotatedOffsetX; - double turretPosY = robotPosY + rotatedOffsetY; + Pose2d hubPos = adjTargetPose; - double hubPosX; - double hubPosY; - if(state == ShootState.SHOOTING_HUB) { - if (isBlueAlliance) { - // hub position determined by which alliance robot is on - hubPosX = Constants.BLUE_HUB_X_POSITION; - hubPosY = Constants.BLUE_HUB_Y_POSITION; - } else { - hubPosX = Constants.RED_HUB_X_POSITION; - hubPosY = Constants.RED_HUB_Y_POSITION; - } - }else{ - //Sets the hubPosX to be either on the blue or red side - //Sets the hubPoseY to be lower or upper depending on the Y position of the robot - hubPosX = isBlueAlliance ? Constants.BLUE_HUB_X_POSITION : Constants.RED_HUB_X_POSITION; - hubPosY = robotPosY > (Constants.FIELD_WIDTH / 2) ? Constants.SHUTTLING_TARGET_HIGHER_Y_POSITION : Constants.SHUTTLING_TARGET_LOWER_Y_POSITION; - } - /* - * This finds the unadjusted pan angle (assuming there is no robot rotation) using - * trigonometry. We take the arctangent of the y-distance beween the robot and the hub - * and the x-distance between the robot and the hub, giving us the unadjusted pan - * angle. The function atan2 ensures the sign of the angle is correct based on the signs - * of the input numbers. - * - */ - if(Constants.DEBUG){ - Logger.recordOutput("Hub pose", new Pose2d(new Translation2d(hubPosX,hubPosY), new Rotation2d())); - } - double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX); + /* + * This finds the unadjusted pan angle (assuming there is no robot rotation) using + * trigonometry. We take the arctangent of the y-distance beween the robot and the hub + * and the x-distance between the robot and the hub, giving us the unadjusted pan + * angle. The function atan2 ensures the sign of the angle is correct based on the signs + * of the input numbers. + * + */ + Translation2d hubRelativeToRobot = hubPos.relativeTo(turretPos).getTranslation(); /* * Adjusts the pan angle to account for the robot's current rotation. We subtract the - * angle of the robot's rotation from the unadjusted angle of the turret to find the + * angle of the robot's rotation from the unadjusted angle of the turret to find the * pan angle, which is the proper angle of the turret adjusted for the robot's rotation * and the fact that the turret 0 angle in in the back of the robot. */ - double panAngle = panAngleUnadjusted - (robotRotation + Math.PI); + double panAngle = hubRelativeToRobot.getAngle().getRadians()+Math.PI; // normalize angle between -PI and PI double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI)); return normalizedPanAngle; } -} +} \ No newline at end of file From e52ccf34af2f37b4594d62566aeaaa076df24189 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 30 Mar 2026 23:43:31 -0400 Subject: [PATCH 07/16] WIP --- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 6cddc59b..c7754a8b 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -386,7 +386,6 @@ public Twist2d getMomentumAdjustment(Pose2d robotPose, boolean useAngularMomentu // convert the effective speed to a twist (displacement over time) // positive timeOfFlight tells us how much we move // negative timeOfFlight tells us how much we need to move to compensate for our movement - timeOfFlight = 0; return adjustSpeeds.toTwist2d(-timeOfFlight); } // Getters for all the subsystems to set posistion. From c6c583f3920db0a705ac5602080a50ff5f7c20d5 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 31 Mar 2026 18:59:24 -0400 Subject: [PATCH 08/16] Added auto aim shoot mode --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ .../frc/robot/commands/lightStrip/SetLed.java | 3 +++ .../frc/robot/constants/enums/ShootingState.java | 3 ++- .../frc/robot/subsystems/ControllerSubsystem.java | 15 ++++++++++++--- 4 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 86ab1c78..b997c8fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,6 +336,10 @@ private void configureBindings() { } public void putShuffleboardCommands() { + SmartDashboard.putData( + "Shooting State: Auto aim", + new SetShootingState(shootState, ShootState.AUTO_AIM)); + if (Constants.DEBUG) { SmartDashboard.putNumber(RunDashboardShotTest.ANGLER_TARGET_POSITION_KEY, 0.0); SmartDashboard.putNumber(RunDashboardShotTest.SHOOTER_TARGET_RPM_KEY, Constants.SHOOTER_SPEED); diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 4158e718..87dc24e5 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -57,6 +57,9 @@ public void execute() { case SHOOTING_HUB: lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); break; + case AUTO_AIM: + lightStrip.setPattern(BlinkinPattern.RAINBOW_LAVA_PALETTE); + break; case SHUTTLING: lightStrip.setPattern(BlinkinPattern.GREEN); break; diff --git a/src/main/java/frc/robot/constants/enums/ShootingState.java b/src/main/java/frc/robot/constants/enums/ShootingState.java index 18e1141c..b01aec87 100644 --- a/src/main/java/frc/robot/constants/enums/ShootingState.java +++ b/src/main/java/frc/robot/constants/enums/ShootingState.java @@ -8,7 +8,8 @@ public enum ShootState { FIXED, // Shoot from a known location FIXED_2, // Shoot from another known location SHOOTING_HUB, // Auto-aim into the hub - SHUTTLING // Auto-aim into alliance zone + SHUTTLING, // Auto-aim into alliance zone + AUTO_AIM //auto aim without hopper and feeder } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 4f4f7366..2934cc3c 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -186,6 +186,15 @@ private void updateTargets(ShootState state, Pose2d robotPose) { useShotTargets(FIXED_TARGETS); } } + case AUTO_AIM ->{ if (Robot.allianceColor().isEmpty()) { + useShotTargets(FIXED_TARGETS); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { + useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { + useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); + } else { + useShotTargets(FIXED_TARGETS); + }} } } @@ -235,7 +244,7 @@ private void useShotTargets(ShotTargets shotTargets) { false, activeTargets.intakeDeploy); - } + } } @@ -249,8 +258,8 @@ private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, profile); double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, profile); - return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, - true, true); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, state != ShootState.AUTO_AIM, + state != ShootState.AUTO_AIM, true); } private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d targetPose) { From 3c877557628e5d14bd7e36199c9c3d2d18bf22e0 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 31 Mar 2026 19:09:00 -0400 Subject: [PATCH 09/16] added fast depot sequence, updated fast depot path, autochooser and auto action --- src/main/deploy/choreo/Depot_Fast.traj | 103 ++++++++++++++++++ .../frc/robot/autochooser/AutoAction.java | 1 + .../frc/robot/autochooser/AutoChooser.java | 5 + .../commands/auto/newauto/FastDepot.java | 30 +++++ 4 files changed, 139 insertions(+) create mode 100644 src/main/deploy/choreo/Depot_Fast.traj create mode 100644 src/main/java/frc/robot/commands/auto/newauto/FastDepot.java diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj new file mode 100644 index 00000000..ca631ef0 --- /dev/null +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -0,0 +1,103 @@ +{ + "name":"Depot_Fast", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.596719741821289, "y":5.060794830322266, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.263749837875366, "y":5.565431118011475, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.11590838432312, "y":5.966093063354492, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.596719741821289 m", "val":3.596719741821289}, "y":{"exp":"5.060794830322266 m", "val":5.060794830322266}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.263749837875366 m", "val":2.263749837875366}, "y":{"exp":"5.565431118011475 m", "val":5.565431118011475}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.1159083843231201 m", "val":1.11590838432312}, "y":{"exp":"5.966093063354492 m", "val":5.966093063354492}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":45.359237, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.43, + "side":0.43, + "back":0.43 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.00817,1.87662], + "samples":[ + {"t":0.0, "x":3.59672, "y":5.06079, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.6482, "ay":4.79312, "alpha":0.0, "fx":[-143.42823,-143.42823,-143.42823,-143.42823], "fy":[54.35308,54.35308,54.35308,54.35308]}, + {"t":0.04583, "x":3.58344, "y":5.06583, "heading":3.14159, "vx":-0.57962, "vy":0.21965, "omega":0.0, "ax":-12.6349, "ay":4.78808, "alpha":0.0, "fx":[-143.27734,-143.27734,-143.27734,-143.27734], "fy":[54.2959,54.2959,54.2959,54.2959]}, + {"t":0.09165, "x":3.54361, "y":5.08092, "heading":3.14159, "vx":-1.15863, "vy":0.43907, "omega":0.0, "ax":-5.31855, "ay":2.0155, "alpha":0.0, "fx":[-60.31135,-60.31135,-60.31135,-60.31135], "fy":[22.85539,22.85539,22.85539,22.85539]}, + {"t":0.13748, "x":3.48493, "y":5.10316, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00016,-0.00016,-0.00016,-0.00016], "fy":[0.00006,0.00006,0.00006,0.00006]}, + {"t":0.1833, "x":3.42067, "y":5.12751, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.22913, "x":3.3564, "y":5.15186, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.27496, "x":3.29214, "y":5.17622, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.32078, "x":3.22787, "y":5.20057, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.36661, "x":3.16361, "y":5.22493, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.41243, "x":3.09934, "y":5.24928, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.45826, "x":3.03508, "y":5.27363, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.50409, "x":2.97082, "y":5.29799, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.54991, "x":2.90655, "y":5.32234, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.59574, "x":2.84229, "y":5.34669, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.64157, "x":2.77802, "y":5.37105, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.68739, "x":2.71376, "y":5.3954, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.73322, "x":2.64949, "y":5.41975, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.77904, "x":2.58523, "y":5.44411, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.82487, "x":2.52096, "y":5.46846, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":0.8707, "x":2.4567, "y":5.49281, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00013,-0.00013,-0.00013,-0.00013], "fy":[-0.00034,-0.00034,-0.00034,-0.00034]}, + {"t":0.91652, "x":2.39243, "y":5.51717, "heading":3.14159, "vx":-1.40236, "vy":0.53143, "omega":0.0, "ax":-0.00154, "ay":-0.00407, "alpha":0.0, "fx":[-0.01748,-0.01748,-0.01748,-0.01748], "fy":[-0.04614,-0.04614,-0.04614,-0.04614]}, + {"t":0.96235, "x":2.32817, "y":5.54152, "heading":3.14159, "vx":-1.40243, "vy":0.53124, "omega":0.0, "ax":-0.14414, "ay":-0.40882, "alpha":0.0, "fx":[-1.63456,-1.63456,-1.63456,-1.63456], "fy":[-4.63588,-4.63588,-4.63588,-4.63588]}, + {"t":1.00817, "x":2.26375, "y":5.56543, "heading":3.14159, "vx":-1.40903, "vy":0.51251, "omega":0.0, "ax":-0.15284, "ay":-0.40772, "alpha":0.0, "fx":[-1.73318,-1.73318,-1.73318,-1.73318], "fy":[-4.62349,-4.62349,-4.62349,-4.62349]}, + {"t":1.05388, "x":2.19919, "y":5.58843, "heading":3.14159, "vx":-1.41602, "vy":0.49387, "omega":0.0, "ax":-0.00145, "ay":-0.00416, "alpha":0.0, "fx":[-0.01644,-0.01644,-0.01644,-0.01644], "fy":[-0.04714,-0.04714,-0.04714,-0.04714]}, + {"t":1.09959, "x":2.13446, "y":5.611, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00012,-0.00012,-0.00012,-0.00012], "fy":[-0.00035,-0.00035,-0.00035,-0.00035]}, + {"t":1.1453, "x":2.06974, "y":5.63357, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.19101, "x":2.00501, "y":5.65613, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.23671, "x":1.94028, "y":5.6787, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.28242, "x":1.87556, "y":5.70126, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.32813, "x":1.81083, "y":5.72383, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.37384, "x":1.7461, "y":5.74639, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.41955, "x":1.68138, "y":5.76896, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.46525, "x":1.61665, "y":5.79152, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.51096, "x":1.55192, "y":5.81409, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.55667, "x":1.4872, "y":5.83665, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.60238, "x":1.42247, "y":5.85922, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.64809, "x":1.35775, "y":5.88178, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69379, "x":1.29302, "y":5.90435, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.00001, "ay":-0.00001, "alpha":0.0, "fx":[0.00017,0.00017,0.00017,0.00017], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, + {"t":1.7395, "x":1.22829, "y":5.92691, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":5.45068, "ay":-1.90024, "alpha":0.0, "fx":[61.80972,61.80972,61.80972,61.80972], "fy":[-21.54838,-21.54838,-21.54838,-21.54838]}, + {"t":1.78521, "x":1.16926, "y":5.94749, "heading":3.14159, "vx":-1.16694, "vy":0.40683, "omega":0.0, "ax":12.75848, "ay":-4.44792, "alpha":0.0, "fx":[144.67873,144.67873,144.67873,144.67873], "fy":[-50.43854,-50.43854,-50.43854,-50.43854]}, + {"t":1.83092, "x":1.12925, "y":5.96144, "heading":3.14159, "vx":-0.58378, "vy":0.20352, "omega":0.0, "ax":12.77198, "ay":-4.45262, "alpha":0.0, "fx":[144.8318,144.8318,144.8318,144.8318], "fy":[-50.4919,-50.4919,-50.4919,-50.4919]}, + {"t":1.87662, "x":1.11591, "y":5.96609, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index ce806027..142ace20 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -8,6 +8,7 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), SHOOT("Shoot"), + FAST_SHOOT("Fast Shoot"), //SHOOT_PICKUP("Shoot and Pickup"), //DISTURBANCE("Disturbance"), //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"), diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index cec590a7..45b6fcbb 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -11,6 +11,7 @@ import frc.robot.Robot; import frc.robot.commands.auto.neutral.DepotNeutral; import frc.robot.commands.auto.neutral.OutpostNeutral; +import frc.robot.commands.auto.newauto.FastDepot; import frc.robot.commands.auto.shoot.ShootBlue; import frc.robot.commands.auto.shoot.ShootRed; import frc.robot.commands.auto.disturbance.DepotDisturbance; @@ -125,6 +126,10 @@ private void populateCommandMap() { new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue), new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue), + new FastDepot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + /* //shoot-pickup diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java new file mode 100644 index 00000000..0241966f --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -0,0 +1,30 @@ +package frc.robot.commands.auto.newauto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.auto.AutoReset; +import frc.robot.commands.auto.AutoShoot; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.*; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class FastDepot extends LoggableSequentialCommandGroup { + public FastDepot( + SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, + HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, + ControllerSubsystem controller, IntakeDeployerSubsystem intake) { + super( + new LoggableParallelCommandGroup( + LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast").withTimeout(10)), + new AutoReset(shootstate, turret, angler), + new SetShootingState(shootstate, ShootState.AUTO_AIM) + ), + new AutoShoot(hopper, feeder, 0) + ); + } +} From cd0ad99f6f03f016a3fc40796ed74f6dce109a3f Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 31 Mar 2026 20:01:25 -0400 Subject: [PATCH 10/16] having issues with choreo not going to where we tell it to go --- src/main/deploy/choreo/Depot_Fast.traj | 82 +++++++------------ .../frc/robot/apriltags/SimApriltagIO.java | 4 +- .../frc/robot/autochooser/AutoChooser.java | 2 +- .../frc/robot/autochooser/FieldLocation.java | 3 +- .../commands/auto/newauto/FastDepot.java | 4 +- .../frc/robot/constants/GameConstants.java | 2 +- vendordeps/ChoreoLib2026.json | 6 +- 7 files changed, 41 insertions(+), 62 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index ca631ef0..9ef8528d 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,26 +3,22 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596719741821289, "y":5.060794830322266, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.263749837875366, "y":5.565431118011475, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.11590838432312, "y":5.966093063354492, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.3662222921848297, "y":5.994427680969238, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"3.596719741821289 m", "val":3.596719741821289}, "y":{"exp":"5.060794830322266 m", "val":5.060794830322266}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.263749837875366 m", "val":2.263749837875366}, "y":{"exp":"5.565431118011475 m", "val":5.565431118011475}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.1159083843231201 m", "val":1.11590838432312}, "y":{"exp":"5.966093063354492 m", "val":5.966093063354492}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.3662222921848297 m", "val":0.3662222921848297}, "y":{"exp":"5.994427680969238 m", "val":5.994427680969238}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -53,50 +49,30 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.00817,1.87662], + "waypoints":[0.0,1.04949], "samples":[ - {"t":0.0, "x":3.59672, "y":5.06079, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.6482, "ay":4.79312, "alpha":0.0, "fx":[-143.42823,-143.42823,-143.42823,-143.42823], "fy":[54.35308,54.35308,54.35308,54.35308]}, - {"t":0.04583, "x":3.58344, "y":5.06583, "heading":3.14159, "vx":-0.57962, "vy":0.21965, "omega":0.0, "ax":-12.6349, "ay":4.78808, "alpha":0.0, "fx":[-143.27734,-143.27734,-143.27734,-143.27734], "fy":[54.2959,54.2959,54.2959,54.2959]}, - {"t":0.09165, "x":3.54361, "y":5.08092, "heading":3.14159, "vx":-1.15863, "vy":0.43907, "omega":0.0, "ax":-5.31855, "ay":2.0155, "alpha":0.0, "fx":[-60.31135,-60.31135,-60.31135,-60.31135], "fy":[22.85539,22.85539,22.85539,22.85539]}, - {"t":0.13748, "x":3.48493, "y":5.10316, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00016,-0.00016,-0.00016,-0.00016], "fy":[0.00006,0.00006,0.00006,0.00006]}, - {"t":0.1833, "x":3.42067, "y":5.12751, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.22913, "x":3.3564, "y":5.15186, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.27496, "x":3.29214, "y":5.17622, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.32078, "x":3.22787, "y":5.20057, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.36661, "x":3.16361, "y":5.22493, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.41243, "x":3.09934, "y":5.24928, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.45826, "x":3.03508, "y":5.27363, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.50409, "x":2.97082, "y":5.29799, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.54991, "x":2.90655, "y":5.32234, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.59574, "x":2.84229, "y":5.34669, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.64157, "x":2.77802, "y":5.37105, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.68739, "x":2.71376, "y":5.3954, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.73322, "x":2.64949, "y":5.41975, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.77904, "x":2.58523, "y":5.44411, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.82487, "x":2.52096, "y":5.46846, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, - {"t":0.8707, "x":2.4567, "y":5.49281, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00013,-0.00013,-0.00013,-0.00013], "fy":[-0.00034,-0.00034,-0.00034,-0.00034]}, - {"t":0.91652, "x":2.39243, "y":5.51717, "heading":3.14159, "vx":-1.40236, "vy":0.53143, "omega":0.0, "ax":-0.00154, "ay":-0.00407, "alpha":0.0, "fx":[-0.01748,-0.01748,-0.01748,-0.01748], "fy":[-0.04614,-0.04614,-0.04614,-0.04614]}, - {"t":0.96235, "x":2.32817, "y":5.54152, "heading":3.14159, "vx":-1.40243, "vy":0.53124, "omega":0.0, "ax":-0.14414, "ay":-0.40882, "alpha":0.0, "fx":[-1.63456,-1.63456,-1.63456,-1.63456], "fy":[-4.63588,-4.63588,-4.63588,-4.63588]}, - {"t":1.00817, "x":2.26375, "y":5.56543, "heading":3.14159, "vx":-1.40903, "vy":0.51251, "omega":0.0, "ax":-0.15284, "ay":-0.40772, "alpha":0.0, "fx":[-1.73318,-1.73318,-1.73318,-1.73318], "fy":[-4.62349,-4.62349,-4.62349,-4.62349]}, - {"t":1.05388, "x":2.19919, "y":5.58843, "heading":3.14159, "vx":-1.41602, "vy":0.49387, "omega":0.0, "ax":-0.00145, "ay":-0.00416, "alpha":0.0, "fx":[-0.01644,-0.01644,-0.01644,-0.01644], "fy":[-0.04714,-0.04714,-0.04714,-0.04714]}, - {"t":1.09959, "x":2.13446, "y":5.611, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00012,-0.00012,-0.00012,-0.00012], "fy":[-0.00035,-0.00035,-0.00035,-0.00035]}, - {"t":1.1453, "x":2.06974, "y":5.63357, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, - {"t":1.19101, "x":2.00501, "y":5.65613, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.23671, "x":1.94028, "y":5.6787, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.28242, "x":1.87556, "y":5.70126, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.32813, "x":1.81083, "y":5.72383, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.37384, "x":1.7461, "y":5.74639, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.41955, "x":1.68138, "y":5.76896, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.46525, "x":1.61665, "y":5.79152, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.51096, "x":1.55192, "y":5.81409, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.55667, "x":1.4872, "y":5.83665, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.60238, "x":1.42247, "y":5.85922, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.64809, "x":1.35775, "y":5.88178, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.69379, "x":1.29302, "y":5.90435, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.00001, "ay":-0.00001, "alpha":0.0, "fx":[0.00017,0.00017,0.00017,0.00017], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, - {"t":1.7395, "x":1.22829, "y":5.92691, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":5.45068, "ay":-1.90024, "alpha":0.0, "fx":[61.80972,61.80972,61.80972,61.80972], "fy":[-21.54838,-21.54838,-21.54838,-21.54838]}, - {"t":1.78521, "x":1.16926, "y":5.94749, "heading":3.14159, "vx":-1.16694, "vy":0.40683, "omega":0.0, "ax":12.75848, "ay":-4.44792, "alpha":0.0, "fx":[144.67873,144.67873,144.67873,144.67873], "fy":[-50.43854,-50.43854,-50.43854,-50.43854]}, - {"t":1.83092, "x":1.12925, "y":5.96144, "heading":3.14159, "vx":-0.58378, "vy":0.20352, "omega":0.0, "ax":12.77198, "ay":-4.45262, "alpha":0.0, "fx":[144.8318,144.8318,144.8318,144.8318], "fy":[-50.4919,-50.4919,-50.4919,-50.4919]}, - {"t":1.87662, "x":1.11591, "y":5.96609, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.99576, "ay":3.75989, "alpha":0.0, "fx":[-147.36948,-147.36948,-147.36948,-147.36948], "fy":[42.63641,42.63641,42.63641,42.63641]}, + {"t":0.04998, "x":3.57977, "y":5.0647, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":-12.99392, "ay":3.75935, "alpha":0.0, "fx":[-147.34857,-147.34857,-147.34857,-147.34857], "fy":[42.63036,42.63036,42.63036,42.63036]}, + {"t":0.09995, "x":3.53109, "y":5.07878, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":-12.99134, "ay":3.75861, "alpha":0.0, "fx":[-147.31929,-147.31929,-147.31929,-147.31929], "fy":[42.62189,42.62189,42.62189,42.62189]}, + {"t":0.14993, "x":3.44995, "y":5.10225, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":-12.98746, "ay":3.75748, "alpha":0.0, "fx":[-147.27529,-147.27529,-147.27529,-147.27529], "fy":[42.60916,42.60916,42.60916,42.60916]}, + {"t":0.1999, "x":3.33637, "y":5.13511, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":-12.98098, "ay":3.75561, "alpha":0.0, "fx":[-147.20183,-147.20183,-147.20183,-147.20183], "fy":[42.5879,42.5879,42.5879,42.5879]}, + {"t":0.24988, "x":3.19037, "y":5.17736, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":-12.96798, "ay":3.75185, "alpha":0.0, "fx":[-147.05443,-147.05443,-147.05443,-147.05443], "fy":[42.54526,42.54526,42.54526,42.54526]}, + {"t":0.29986, "x":3.01196, "y":5.22897, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":-12.92876, "ay":3.7405, "alpha":0.0, "fx":[-146.60968,-146.60968,-146.60968,-146.60968], "fy":[42.41658,42.41658,42.41658,42.41658]}, + {"t":0.34983, "x":2.80121, "y":5.28995, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":-3.46426, "ay":1.00227, "alpha":0.0, "fx":[-39.28405,-39.28405,-39.28405,-39.28405], "fy":[11.36552,11.36552,11.36552,11.36552]}, + {"t":0.39981, "x":2.56998, "y":5.35684, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":-0.00026, "ay":0.00008, "alpha":0.0, "fx":[-0.00296,-0.00296,-0.00296,-0.00296], "fy":[0.00086,0.00086,0.00086,0.00086]}, + {"t":0.44978, "x":2.33443, "y":5.42499, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.49976, "x":2.09889, "y":5.49314, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.54973, "x":1.86334, "y":5.56129, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.59971, "x":1.62779, "y":5.62944, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.00026, "ay":-0.00008, "alpha":0.0, "fx":[0.00296,0.00296,0.00296,0.00296], "fy":[-0.00086,-0.00086,-0.00086,-0.00086]}, + {"t":0.64969, "x":1.39224, "y":5.69758, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":3.46426, "ay":-1.00227, "alpha":0.0, "fx":[39.28405,39.28405,39.28405,39.28405], "fy":[-11.36552,-11.36552,-11.36552,-11.36552]}, + {"t":0.69966, "x":1.16102, "y":5.76448, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":12.92876, "ay":-3.7405, "alpha":0.0, "fx":[146.60968,146.60968,146.60968,146.60968], "fy":[-42.41658,-42.41658,-42.41658,-42.41658]}, + {"t":0.74964, "x":0.95027, "y":5.82545, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":12.96798, "ay":-3.75185, "alpha":0.0, "fx":[147.05443,147.05443,147.05443,147.05443], "fy":[-42.54526,-42.54526,-42.54526,-42.54526]}, + {"t":0.79961, "x":0.77185, "y":5.87707, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":12.98098, "ay":-3.75561, "alpha":0.0, "fx":[147.20183,147.20183,147.20183,147.20183], "fy":[-42.5879,-42.5879,-42.5879,-42.5879]}, + {"t":0.84959, "x":0.62585, "y":5.91931, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":12.98746, "ay":-3.75748, "alpha":0.0, "fx":[147.27529,147.27529,147.27529,147.27529], "fy":[-42.60916,-42.60916,-42.60916,-42.60916]}, + {"t":0.89957, "x":0.51227, "y":5.95217, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":12.99134, "ay":-3.75861, "alpha":0.0, "fx":[147.31929,147.31929,147.31929,147.31929], "fy":[-42.62189,-42.62189,-42.62189,-42.62189]}, + {"t":0.94954, "x":0.43114, "y":5.97565, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":12.99392, "ay":-3.75935, "alpha":0.0, "fx":[147.34857,147.34857,147.34857,147.34857], "fy":[-42.63036,-42.63036,-42.63036,-42.63036]}, + {"t":0.99952, "x":0.38245, "y":5.98973, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":12.99576, "ay":-3.75989, "alpha":0.0, "fx":[147.36948,147.36948,147.36948,147.36948], "fy":[-42.63641,-42.63641,-42.63641,-42.63641]}, + {"t":1.04949, "x":0.36622, "y":5.99443, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 80cc9d06..d8ad47a1 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -44,8 +44,8 @@ public void simReadings() { if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0); Vector stdDevs = truster.calculateTrust(measurement); - double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); - double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); + double readingX = pose.getX(); //+ random.nextGaussian() * stdDevs.get(0); + double readingY = pose.getY(); //+ random.nextGaussian() * stdDevs.get(1); double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 45b6fcbb..99470516 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -127,7 +127,7 @@ private void populateCommandMap() { commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue), new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); - commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue), + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT), new FastDepot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 3bdae5c9..18e718a0 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -17,7 +17,8 @@ public enum FieldLocation { INVALID(-1, -1, -1, "INVALID"), DEPOT_SIDE(3.599, 7.33, 180, "Depot side - Driver Station LEFT"), //robot is 24 cm away from the wall MID(3.599, 4.029, 180, "Middle"), - OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"); //robot is 24 cm away from the wall + OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"), //robot is 24 cm away from the wall + FAST_DEPOT(3.596, 5.06, 180, "Depot side - for the fast auto"); //robot's back left corner is next to hopper corner private static final double RED_X_POS = 9.338; // meters public static final double HEIGHT_OF_FIELD = 8.043; diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index 0241966f..7bed88a6 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -11,6 +11,7 @@ import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.utils.logging.commands.LoggableWaitCommand; public class FastDepot extends LoggableSequentialCommandGroup { public FastDepot( @@ -20,10 +21,11 @@ public FastDepot( super( new LoggableParallelCommandGroup( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast").withTimeout(10)), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")), new AutoReset(shootstate, turret, angler), new SetShootingState(shootstate, ShootState.AUTO_AIM) ), + new LoggableWaitCommand(3), new AutoShoot(hopper, feeder, 0) ); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 78c19113..fd15231e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json index 322c9e20..48751078 100644 --- a/vendordeps/ChoreoLib2026.json +++ b/vendordeps/ChoreoLib2026.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2026.json", "name": "ChoreoLib", - "version": "2026.0.1", + "version": "2026.0.2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2026.0.1" + "version": "2026.0.2" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, From ba0eec9b6ff53c51017b30c9a1f84c1f5081814e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 31 Mar 2026 20:31:16 -0400 Subject: [PATCH 11/16] Changed P of drive command WIP --- src/main/java/frc/robot/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java index e75b9fee..c3b31b0a 100644 --- a/src/main/java/frc/robot/Drive.java +++ b/src/main/java/frc/robot/Drive.java @@ -9,8 +9,8 @@ public class Drive extends SubsystemBase{ - private final PIDController xController = new PIDController(.01, 0.0, 0.0); - private final PIDController yController = new PIDController(.01, 0.0, 0.0); + private final PIDController xController = new PIDController(10, 0.0, 0.0); + private final PIDController yController = new PIDController(10, 0.0, 0.0); private final PIDController headingController = new PIDController(.01, 0.0, 0.0); private SwerveSubsystem subsystem; From 6d0c3854819688088e80be0f684a498d1ed517cc Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 31 Mar 2026 21:02:24 -0400 Subject: [PATCH 12/16] Chnaged angler turret and angler reset speeds --- src/main/java/frc/robot/constants/GameConstants.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index fd15231e..37c34e21 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -101,12 +101,12 @@ public enum Mode { public static final double ANGLER_FF = 0.0; public static final double ANGLER_HOME_ROTATIONS = 0.0; public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler - public static final double ANGLER_ENCODER_HIGH = 272; //Highest encoder position of Angler - public static final double ANGLER_ANGLE_LOW = 16; //Lowest angle position of Angler - public static final double ANGLER_ANGLE_HIGH = 37; //Highest angle position of Angler + public static final double ANGLER_ENCODER_HIGH = 265; //Highest encoder position of Angler + public static final double ANGLER_ANGLE_LOW = 17; //Lowest angle position of Angler + public static final double ANGLER_ANGLE_HIGH = 38; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState - public static final double ANGLER_LIMIT_SPEED = 0.3; + public static final double ANGLER_LIMIT_SPEED = 0.5; // turret (pan angle) PID @@ -121,8 +121,8 @@ public enum Mode { public static final double TURRET_ENCODER_MIN = 0; //Lowest encoder position of Turret public static final double TURRET_ENCODER_MAX = 77; //Highest encoder position of Turret public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward - public static final double TURRET_MIN_ANGLE = -92; - public static final double TURRET_MAX_ANGLE = 92; + public static final double TURRET_MIN_ANGLE = -96; + public static final double TURRET_MAX_ANGLE = 96; public static final double TURRET_LIMIT_SPEED = 0.2; public static final double TURRET_OUT_OF_RANGE_FLOP_RPM = -1500.0; public static final double TURRET_PID_DISTANCE_THRESHOLD = 10; From fa6bd26f8cf59229793fa27324cca6be55b177df Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Wed, 1 Apr 2026 18:59:00 -0400 Subject: [PATCH 13/16] Updated shuttling target locations --- src/main/java/frc/robot/utils/math/TurretCalculations.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index ce98ced9..67887034 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -62,7 +62,7 @@ public static double calculateTurretAngle(ShootState state, double robotPosX, do }else{ //Sets the hubPosX to be either on the blue or red side //Sets the hubPoseY to be lower or upper depending on the Y position of the robot - hubPosX = isBlueAlliance ? Constants.BLUE_HUB_X_POSITION : Constants.RED_HUB_X_POSITION; + hubPosX = isBlueAlliance ? Constants.BLUE_SHUTTLING_TARGET_X_POSITION : Constants.RED_SHUTTLING_TARGET_X_POSITION; hubPosY = robotPosY > (Constants.FIELD_WIDTH / 2) ? Constants.SHUTTLING_TARGET_HIGHER_Y_POSITION : Constants.SHUTTLING_TARGET_LOWER_Y_POSITION; } /* From 133dc22720638d3bcd55a71cd7e947d697256821 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 1 Apr 2026 19:14:59 -0400 Subject: [PATCH 14/16] more --- .idea/gradle.xml | 1 + .idea/misc.xml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.idea/gradle.xml b/.idea/gradle.xml index ce1c62c7..c1b38748 100644 --- a/.idea/gradle.xml +++ b/.idea/gradle.xml @@ -5,6 +5,7 @@