From edfafcef6cff4a33a2a4caed9741e84247004c92 Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Sun, 11 Jan 2026 15:57:11 -0800 Subject: [PATCH 1/3] =?UTF-8?q?Fixed=20errors=20and=20ported=20to=202026:?= =?UTF-8?q?=20Reefscape=20port=20to=202026:=20Photonlib=20not=20version=20?= =?UTF-8?q?=E2=80=9C2026=E2=80=9D=20Solution:=20gaslight=20photonlib=20by?= =?UTF-8?q?=20changing=20its=20json=20(make=20sure=20to=20update=20this=20?= =?UTF-8?q?when=20they=20release=20the=20new=20version)=20AnthonyVision.ja?= =?UTF-8?q?va:=20k2025Reefscape=20cannot=20be=20resolved=20Solution:=20upd?= =?UTF-8?q?ate=20to=20k2025ReefscapeWelded=20ElevatorSubsystem.java,=20Fun?= =?UTF-8?q?nelSubsystem.java:=20constructor=20Follower=20undefined=20Solut?= =?UTF-8?q?ion:=20Follower=20accepts=20MotorAlignmentValue=20and=20not=20b?= =?UTF-8?q?oolean?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .SysId/sysid.json | 9 - .github/workflows/main.yml | 31 -- .gitignore | 1 - .vscode/launch.json | 42 +-- .vscode/settings.json | 121 +++--- .wpilib/wpilib_preferences.json | 2 +- CODEOWNERS | 1 - WPILib-License.md | 2 +- build.gradle | 65 +--- settings.gradle | 6 +- simgui-ds.json | 103 ----- .../frc/robot/AutoRoutines/AutoProducer.java | 2 +- src/main/java/frc/robot/Constants.java | 54 ++- src/main/java/frc/robot/Robot.java | 76 ++-- src/main/java/frc/robot/RobotContainer.java | 5 +- .../robot/commandGroups/JamesHardenScore.java | 29 +- .../ArmToAngleAndSpinFlywheel.java | 2 - .../commands/DaleCommands/ArmToAngleCmd.java | 2 +- .../ZeroElevatorHardStop.java | 10 +- .../ZeroElevatorToFFiltered.java | 10 +- .../robot/commands/EndWhenCloseEnough.java | 4 +- .../FunnelCommands/CoralCheckedIn.java | 6 +- .../SwerveCommands/ApplySwerveVoltage.java | 2 - .../SwerveCommands/JamesHardenMovement.java | 48 +-- .../SwerveCommands/SwerveJoystickCommand.java | 8 +- .../frc/robot/subsystems/AnthonyVision.java | 356 ++++++++++++++++++ .../frc/robot/subsystems/ArmSubsystem.java | 12 +- .../robot/subsystems/ElevatorSubsystem.java | 59 ++- .../frc/robot/subsystems/FunnelSubsystem.java | 13 +- .../frc/robot/subsystems/SwerveSubsystem.java | 87 ++--- .../subsystems/TootsieSlideSubsystem.java | 4 +- .../frc/robot/subsystems/VisionSystem.java | 274 -------------- vendordeps/DogLog.json | 6 +- ...enix5-5.35.1.json => Phoenix5-5.36.0.json} | 36 +- ...c2025-latest.json => Phoenix6-26.1.0.json} | 130 ++++--- vendordeps/WPILibNewCommands.json | 3 +- vendordeps/photonlib.json | 15 +- 37 files changed, 784 insertions(+), 852 deletions(-) delete mode 100644 .SysId/sysid.json delete mode 100644 .github/workflows/main.yml delete mode 100644 CODEOWNERS delete mode 100644 simgui-ds.json create mode 100644 src/main/java/frc/robot/subsystems/AnthonyVision.java delete mode 100644 src/main/java/frc/robot/subsystems/VisionSystem.java rename vendordeps/{Phoenix5-5.35.1.json => Phoenix5-5.36.0.json} (87%) rename vendordeps/{Phoenix6-frc2025-latest.json => Phoenix6-26.1.0.json} (84%) diff --git a/.SysId/sysid.json b/.SysId/sysid.json deleted file mode 100644 index 9fbf59af..00000000 --- a/.SysId/sysid.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "SysId": { - "Data Selector": { - "window": { - "name": "Joe Mama Selector" - } - } - } -} diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml deleted file mode 100644 index 23556b2e..00000000 --- a/.github/workflows/main.yml +++ /dev/null @@ -1,31 +0,0 @@ -on: [push] -# A workflow run is made up of one or more jobs that can run sequentially or in parallel -jobs: - spotless: - # The type of runner that the job will run on - runs-on: ubuntu-22.04 - # Steps represent a sequence of tasks that will be executed as part of the job - steps: - # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - uses: actions/setup-java@v4 - with: - distribution: 'zulu' - java-version: 17 - - run: ./gradlew spotlessCheck - build: - # The type of runner that the job will run on - runs-on: ubuntu-22.04 - # Steps represent a sequence of tasks that will be executed as part of the job - steps: - # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - uses: actions/setup-java@v4 - with: - distribution: 'zulu' - java-version: 17 - - run: ./gradlew build diff --git a/.gitignore b/.gitignore index 351b3691..9a9ca7be 100644 --- a/.gitignore +++ b/.gitignore @@ -185,4 +185,3 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath -.gradle/* \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index 5b804e84..c9c9713d 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,21 +1,21 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c3..5e6ede86 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,60 +1,61 @@ -{ - "java.configuration.updateBuildConfiguration": "automatic", - "java.server.launchMode": "Standard", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "java.test.config": [ - { - "name": "WPIlibUnitTests", - "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], - "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , - "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" - } - }, - ], - "java.test.defaultConfig": "WPIlibUnitTests", - "java.import.gradle.annotationProcessing.enabled": false, - "java.completion.favoriteStaticMembers": [ - "org.junit.Assert.*", - "org.junit.Assume.*", - "org.junit.jupiter.api.Assertions.*", - "org.junit.jupiter.api.Assumptions.*", - "org.junit.jupiter.api.DynamicContainer.*", - "org.junit.jupiter.api.DynamicTest.*", - "org.mockito.Mockito.*", - "org.mockito.ArgumentMatchers.*", - "org.mockito.Answers.*", - "edu.wpi.first.units.Units.*" - ], - "java.completion.filteredTypes": [ - "java.awt.*", - "com.sun.*", - "sun.*", - "jdk.*", - "org.graalvm.*", - "io.micrometer.shaded.*", - "java.beans.*", - "java.util.Base64.*", - "java.util.Timer", - "java.sql.*", - "javax.swing.*", - "javax.management.*", - "javax.smartcardio.*", - "edu.wpi.first.math.proto.*", - "edu.wpi.first.math.**.proto.*", - "edu.wpi.first.math.**.struct.*", - ] -} +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ], + "java.dependency.enableDependencyCheckup": false +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 0ab98669..a05d65e5 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025", + "projectYear": "2026", "teamNumber": 3501 } \ No newline at end of file diff --git a/CODEOWNERS b/CODEOWNERS deleted file mode 100644 index 1310873e..00000000 --- a/CODEOWNERS +++ /dev/null @@ -1 +0,0 @@ -* @firebots-software/software-reviewers diff --git a/WPILib-License.md b/WPILib-License.md index 645e5425..eb3061b0 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors +Copyright (c) 2009-2026 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 909d64a9..f2ceadbc 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" - id 'com.diffplug.spotless' version '6.20.0' + id "edu.wpi.first.GradleRIO" version "2026.1.1" } java { @@ -35,7 +34,7 @@ deploy { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' deleteOldFiles = false // Change to true to delete files on roboRIO that no - // longer exist in deploy directory of this project + // longer exist in deploy directory of this project } } } @@ -44,7 +43,8 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava -// Set to true to use debug for JNI. +// Set to true to use debug for all targets including JNI, which will drastically impact +// performance. wpi.java.debugJni = false // Set this to true to enable desktop support. @@ -88,12 +88,10 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { - configurations.runtimeClasspath.collect { - it.isDirectory() ? it : zipTree(it) - } - } - from sourceSets.main.allSource + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from('src') { into 'backup/src' } + from('vendordeps') { into 'backup/vendordeps' } + from('build.gradle') { into 'backup' } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } @@ -107,50 +105,3 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } - -tasks.named("build") { - dependsOn("spotlessApply") -} - -spotless { - java { - target fileTree('.') { - include '**/*.java' - exclude '**/build/**', '**/build-*/**', '**/bin/**' - } - toggleOffOn() - googleJavaFormat() - removeUnusedImports() - trimTrailingWhitespace() - endWithNewline() - } - groovyGradle { - target fileTree('.') { - include '**/*.gradle' - exclude '**/build/**', '**/build-*/**' - } - greclipse() - indentWithSpaces(4) - trimTrailingWhitespace() - endWithNewline() - } - format 'xml', { - target fileTree('.') { - include '**/*.xml' - exclude '**/build/**', '**/build-*/**' - } - eclipseWtp('xml') - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } - format 'misc', { - target fileTree('.') { - include '**/*.md', '**/.gitignore' - exclude '**/build/**', '**/build-*/**' - } - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } -} diff --git a/settings.gradle b/settings.gradle index 969c7b09..25f6f6e8 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,8 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } diff --git a/simgui-ds.json b/simgui-ds.json deleted file mode 100644 index cdbac704..00000000 --- a/simgui-ds.json +++ /dev/null @@ -1,103 +0,0 @@ -{ - "keyboardJoysticks": [ - { - "axisConfig": [ - { - "decKey": 65, - "incKey": 68 - }, - { - "decKey": 87, - "incKey": 83 - }, - { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 - } - ], - "axisCount": 3, - "buttonCount": 4, - "buttonKeys": [ - 90, - 88, - 67, - 86 - ], - "povConfig": [ - { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } - ], - "povCount": 1 - }, - { - "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 - } - ], - "axisCount": 2, - "buttonCount": 4, - "buttonKeys": [ - 77, - 44, - 46, - 47 - ], - "povCount": 0 - }, - { - "axisConfig": [ - { - "decKey": 263, - "incKey": 262 - }, - { - "decKey": 265, - "incKey": 264 - } - ], - "axisCount": 2, - "buttonCount": 6, - "buttonKeys": [ - 260, - 268, - 266, - 261, - 269, - 267 - ], - "povCount": 0 - }, - { - "axisCount": 0, - "buttonCount": 0, - "povCount": 0 - } - ], - "robotJoysticks": [ - { - "guid": "Keyboard0" - }, - {}, - {}, - {}, - { - "guid": "Keyboard1" - } - ] -} diff --git a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java index 9f1b4256..136263e2 100644 --- a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java +++ b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java @@ -93,7 +93,7 @@ private void settyCycle( JamesHardenMovement movementCommand, maintainCommand; if (!scorePosition.isBranch()) { - DogLog.log("JamesHardenScore/Errors", "called without real branch"); + DogLog.log("Commands/JamesHardenScore/Errors", "Called without a real branch"); return; } movementCommand = JamesHardenMovement.toSpecificBranch(driveTrain, () -> scorePosition, false); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ef43561..5095db4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,10 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; 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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; @@ -43,6 +46,24 @@ public static class Kalman { } public static class HardenConstants { + public static final double QKP = 3.4; + public static final double QKI = 0.45; + public static final double QKD = 0.0005; + public static final double QCRUISE = 4.368; + public static final double QACCEL = 8.0; + public static final double QIZONE = 0.35; + public static final double QIRANGE_LOWER = 0.0; + public static final double QIRANGE_UPPER = 0.2; + + public static final double HKP = 3.7; + public static final double HKI = 0.4; + public static final double HKD = 0.0; + public static final double HCRUISE = 9.417; + public static final double HACCEL = 10.971; + public static final double HIZONE = 0.14; + public static final double HIRANGE_LOWER = 0.0; + public static final double HIRANGE_UPPER = Math.PI / 4.0; + public static class EndWhenCloseEnough { public static final double translationalToleranceTeleop = 0.8d; // 0.43105229381 worked before public static final double translationalToleranceAuto = 1d; @@ -90,6 +111,33 @@ public String getLoggingName() { public static final double LEFT_CAM_TO_ROBOT_ROTATION_ROLL = 0; public static final double LEFT_CAM_TO_ROBOT_ROTATION_PITCH = Units.degreesToRadians(-12.5); public static final double LEFT_CAM_TO_ROBOT_ROTATION_YAW = Units.degreesToRadians(-40); + + public static Transform3d getCameraTransform(Cameras cam) { + switch (cam) { + case RIGHT_CAM: + return new Transform3d( + new Translation3d( + RIGHT_CAM_TO_ROBOT_TRANSLATION_X, + RIGHT_CAM_TO_ROBOT_TRANSLATION_Y, + RIGHT_CAM_TO_ROBOT_TRANSLATION_Z), + new Rotation3d( + RIGHT_CAM_TO_ROBOT_ROTATION_ROLL, + RIGHT_CAM_TO_ROBOT_ROTATION_PITCH, + RIGHT_CAM_TO_ROBOT_ROTATION_YAW)); + case LEFT_CAM: + return new Transform3d( + new Translation3d( + LEFT_CAM_TO_ROBOT_TRANSLATION_X, + LEFT_CAM_TO_ROBOT_TRANSLATION_Y, + LEFT_CAM_TO_ROBOT_TRANSLATION_Z), + new Rotation3d( + LEFT_CAM_TO_ROBOT_ROTATION_ROLL, + LEFT_CAM_TO_ROBOT_ROTATION_PITCH, + LEFT_CAM_TO_ROBOT_ROTATION_YAW)); + default: + throw new IllegalArgumentException("Unknown camera: " + cam); + } + } } public static class AutoRoutines { @@ -878,15 +926,15 @@ public static class ElevatorConstants { public static final double STATOR_CURRENT_LIMIT = 50.0; // TODO: change for actual match public static final double SUPPLY_CURRENT_LIMIT = 30.0; // TODO: change for actual match - public static double S0C_KP = 1.0; // 1.0 before (okay) - public static double S0C_KI = 0.0; + public static double S0C_KP = 1.04; // 1.0 before (okay) + public static double S0C_KI = 0.00; public static double S0C_KD = 0.005; public static double S1C_KP = 0.3501; public static double S1C_KI = 0.0; public static double S1C_KD = 0.0; - public static double S0C_KS = 0.0; + public static double S0C_KS = 0.00; public static double S0C_KG = 0.29 + 0.05; public static double S0C_KA = 0.0004657452997; // 0.04 public static double S0C_KV = 0.124; // 10.66 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1743b9ec..0abcf2c2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,10 +10,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.ElevatorCommands.ZeroElevatorHardStop; +import frc.robot.subsystems.AnthonyVision; import frc.robot.subsystems.CoralPosition; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.SwerveSubsystem; -import frc.robot.subsystems.VisionSystem; import frc.robot.util.LoggedTalonFX; /** @@ -34,8 +34,8 @@ public class Robot extends TimedRobot { // private VisionSystem visionLeft = VisionSystem.getInstance(Constants.Vision.Cameras.LEFT_CAM); private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance(); private final RobotContainer m_robotContainer; - private VisionSystem visionRight; - private VisionSystem visionLeft; + private AnthonyVision visionRight; + private AnthonyVision visionLeft; // standard deviation for x (meters), y (meters) and rotation (radians) camera data @@ -50,9 +50,10 @@ public Robot() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); visionRight = - VisionSystem.getInstance(Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide()); + AnthonyVision.getInstance( + Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide()); visionLeft = - VisionSystem.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide()); + AnthonyVision.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide()); absoluteInit(); } @@ -65,8 +66,6 @@ public Robot() { */ @Override public void robotPeriodic() { - DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel()); - DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide()); LoggedTalonFX.periodic_static(); CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); @@ -74,8 +73,6 @@ public void robotPeriodic() { visionRight.addFilteredPose(); visionLeft.addFilteredPose(); - DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); - DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel()); DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide()); } @@ -89,24 +86,44 @@ public void disabledInit() { @Override public void robotInit() { DogLog.setOptions( - new DogLogOptions().withNtPublish(false).withCaptureDs(true).withLogExtras(true)); - DogLog.log("PIDArmKP", Constants.Arm.S0C_KP); - DogLog.log("PIDArmKI", Constants.Arm.S0C_KI); - DogLog.log("PIDArmKD", Constants.Arm.S0C_KD); - DogLog.log("PIDArmKS", Constants.Arm.S0C_KS); - DogLog.log("PIDArmKG", Constants.Arm.S0C_KG); - - DogLog.log("PIDElevatorKP", Constants.ElevatorConstants.S0C_KP); - DogLog.log("PIDElevatorKI", Constants.ElevatorConstants.S0C_KI); - DogLog.log("PIDElevatorKD", Constants.ElevatorConstants.S0C_KD); - DogLog.log("PIDElevatorKS", Constants.ElevatorConstants.S0C_KS); - DogLog.log("PIDElevatorKG", Constants.ElevatorConstants.S0C_KG); - - DogLog.log("PIDTootsieKP", Constants.TootsieSlide.S0C_KP); - DogLog.log("PIDTootsieKI", Constants.TootsieSlide.S0C_KI); - DogLog.log("PIDTootsieKD", Constants.TootsieSlide.S0C_KD); - DogLog.log("PIDTootsieKS", Constants.TootsieSlide.S0C_KS); - DogLog.log("PIDTootsieKG", Constants.TootsieSlide.S0C_KG); + new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true)); + DogLog.log("PIDValues/ArmKP", Constants.Arm.S0C_KP); + DogLog.log("PIDValues/ArmKI", Constants.Arm.S0C_KI); + DogLog.log("PIDValues/ArmKD", Constants.Arm.S0C_KD); + DogLog.log("PIDValues/ArmKS", Constants.Arm.S0C_KS); + DogLog.log("PIDValues/ArmKG", Constants.Arm.S0C_KG); + + DogLog.log("PIDValues/ElevatorKP", Constants.ElevatorConstants.S0C_KP); + DogLog.log("PIDValues/ElevatorKI", Constants.ElevatorConstants.S0C_KI); + DogLog.log("PIDValues/ElevatorKD", Constants.ElevatorConstants.S0C_KD); + DogLog.log("PIDValues/ElevatorKS", Constants.ElevatorConstants.S0C_KS); + DogLog.log("PIDValues/ElevatorKG", Constants.ElevatorConstants.S0C_KG); + + DogLog.log("PIDValues/TootsieKP", Constants.TootsieSlide.S0C_KP); + DogLog.log("PIDValues/TootsieKI", Constants.TootsieSlide.S0C_KI); + DogLog.log("PIDValues/TootsieKD", Constants.TootsieSlide.S0C_KD); + DogLog.log("PIDValues/TootsieKS", Constants.TootsieSlide.S0C_KS); + DogLog.log("PIDValues/TootsieKG", Constants.TootsieSlide.S0C_KG); + + // Q set + DogLog.log("PIDValues/QKP", Constants.HardenConstants.QKP); + DogLog.log("PIDValues/QKI", Constants.HardenConstants.QKI); + DogLog.log("PIDValues/QKD", Constants.HardenConstants.QKD); + DogLog.log("PIDValues/QCRUISE", Constants.HardenConstants.QCRUISE); + DogLog.log("PIDValues/QACCEL", Constants.HardenConstants.QACCEL); + DogLog.log("PIDValues/QIZONE", Constants.HardenConstants.QIZONE); + DogLog.log("PIDValues/QIRANGE_LOWER", Constants.HardenConstants.QIRANGE_LOWER); + DogLog.log("PIDValues/QIRANGE_UPPER", Constants.HardenConstants.QIRANGE_UPPER); + + // H set + DogLog.log("PIDValues/HKP", Constants.HardenConstants.HKP); + DogLog.log("PIDValues/HKI", Constants.HardenConstants.HKI); + DogLog.log("PIDValues/HKD", Constants.HardenConstants.HKD); + DogLog.log("PIDValues/HCRUISE", Constants.HardenConstants.HCRUISE); + DogLog.log("PIDValues/HACCEL", Constants.HardenConstants.HACCEL); + DogLog.log("PIDValues/HIZONE", Constants.HardenConstants.HIZONE); + DogLog.log("PIDValues/HIRANGE_LOWER", Constants.HardenConstants.HIRANGE_LOWER); + DogLog.log("PIDValues/HIRANGE_UPPER", Constants.HardenConstants.HIRANGE_UPPER); // Commented this code that logs the electric data because it crashed the robot code // there is an error related to the usage of this // DogLog.setPdh(new PowerDistribution()); @@ -155,10 +172,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - // DogLog.log("PID Constant", IncreasePArm.broomIndex()); - // DogLog.log("Mechanism Type", IncreasePArm.mechIndex()); - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 404e2792..89f40b82 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,7 +83,6 @@ public void doTelemetry() { if (driveTrain.getCurrentCommand() != null) { commandName = driveTrain.getCurrentCommand().getName(); } - DogLog.log("Robot/SwerveDriveCommand", commandName); } public RobotContainer() { @@ -471,7 +470,7 @@ public static void setAlliance() { } public BooleanSupplier getRedSide() { - DogLog.log("get alliance", redside.getAsBoolean()); + DogLog.log("Info/Alliance", redside.getAsBoolean()); return redside; } @@ -479,7 +478,7 @@ public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ int autoValue = autoChooser.getSelected(); Command autoCommand; - DogLog.log("auto/selected", autoValue); + DogLog.log("Info/AutoSelected", autoValue); switch (autoValue) { case 1: autoCommand = diff --git a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java index 37e05084..1122e68f 100644 --- a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java +++ b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java @@ -72,7 +72,7 @@ public JamesHardenScore( JamesHardenMovement movementCommand, maintainCommand; if (!branch.isBranch()) { - DogLog.log("JamesHardenScore/Errors", "called without real branch"); + DogLog.log("Commands/JamesHardenScore/Errors", "Called without a real branch"); return; } @@ -85,8 +85,6 @@ public JamesHardenScore( } else { elevateCommand = new SetElevatorLevel(elevatorSubsystem, height, true); } - // if (DriverStation.isTeleop()) { - DogLog.log("JamesHardenScore/Version", "teleop"); addCommands( movementCommand.alongWith( @@ -94,30 +92,5 @@ public JamesHardenScore( .andThen(elevateCommand)), new ParallelDeadlineGroup( new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand)); - // addCommands( - // maintainCommand - // .alongWith( - // (new EndWhenCloseEnough(() -> maintainCommand.getTargetPose2d())) - // .andThen(elevateCommand)) - // .alongWith( - // new EndWhenCloseEnough(() -> maintainCommand.getTargetPose2d(), - // Constants.HardenConstants.RegularCommand.xyIndividualTolerance * Math.sqrt(2), - // Constants.HardenConstants.RegularCommand.headingTolerance) - // .andThen(new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5)))); - // ) - // new ParallelDeadlineGroup( - // new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand)); - // } else { - // DogLog.log("JamesHardenScore/Version", "auto"); - // addCommands( - // movementCommand - // .withTimeout(5.0) - // .alongWith( - // (new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d())) - // .andThen(elevateCommand)), - // new ParallelDeadlineGroup( - // new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand)); - // new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d(), 2.8284271247); - // } } } diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index a5d410af..a3401471 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -1,7 +1,6 @@ // TODO: THIS COMMAND NEED TO BE UPDATED TO WORK PROPERLY package frc.robot.commands.DaleCommands; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.ArmSubsystem; @@ -27,7 +26,6 @@ public void initialize() {} @Override public void execute() { armPlusFlywheel.spinFlywheel(); // Spin the flywheel at desired speed - DogLog.log("Running to target angle: ", angle); armPlusFlywheel.setPosition(angle); } diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java index 478deada..801b3c82 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java @@ -21,7 +21,7 @@ public void initialize() {} @Override public void execute() { - DogLog.log("Running to target angle: ", angle); + DogLog.log("Commands/ArmToAngleCmd/TargetAngle", angle); arm.setPosition(angle); } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java index c19e9668..aaedf985 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java @@ -10,7 +10,7 @@ public class ZeroElevatorHardStop extends Command { public ZeroElevatorHardStop(ElevatorSubsystem subsystem) { elevatorSubsystem = subsystem; - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); + DogLog.log("Commands/ZeroElevatorHardStop/Running", false); addRequirements(elevatorSubsystem); } @@ -18,7 +18,7 @@ public ZeroElevatorHardStop(ElevatorSubsystem subsystem) { @Override public void initialize() { elevatorSubsystem.reduceCurrentLimits(); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", true); + DogLog.log("Commands/ZeroElevatorHardStop/Running", true); timesExceededCurrent = 0; } @@ -32,7 +32,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); + DogLog.log("Commands/ZeroElevatorHardStop/Running", false); if (!interrupted) { elevatorSubsystem.resetElevatorPositionToZero(); } @@ -43,9 +43,9 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/timesExceededCount", timesExceededCurrent); + DogLog.log("Commands/ZeroElevatorHardStop/timesExceededCurrent", timesExceededCurrent); boolean checkCurrent = elevatorSubsystem.checkCurrent(); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/checkcurrent", checkCurrent); + DogLog.log("Commands/ZeroElevatorHardStop/checkCurrent", checkCurrent); if (checkCurrent) { timesExceededCurrent++; } else { diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java index 4b265808..28cfe065 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java @@ -11,14 +11,14 @@ public class ZeroElevatorToFFiltered extends Command { public ZeroElevatorToFFiltered(ElevatorSubsystem subsystem) { elevatorSubsystem = subsystem; - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", false); addRequirements(elevatorSubsystem); } @Override public void initialize() { ticksAtPosition = 0; - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", true); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", true); elevatorSubsystem.elevateTo(ElevatorPositions.Intake); } @@ -27,15 +27,15 @@ public void execute() {} @Override public void end(boolean interrupted) { - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", false); elevatorSubsystem.resetPositionFiltered(); } @Override public boolean isFinished() { - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); + DogLog.log("Commands/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); boolean inPosition = elevatorSubsystem.isAtPosition() && elevatorSubsystem.atIntake(); - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition); + DogLog.log("Commands/ZeroElevatorToFFiltered/inPosition", inPosition); if (inPosition) { ticksAtPosition++; diff --git a/src/main/java/frc/robot/commands/EndWhenCloseEnough.java b/src/main/java/frc/robot/commands/EndWhenCloseEnough.java index 9aebf3a2..2b0c097e 100644 --- a/src/main/java/frc/robot/commands/EndWhenCloseEnough.java +++ b/src/main/java/frc/robot/commands/EndWhenCloseEnough.java @@ -34,7 +34,7 @@ public EndWhenCloseEnough( // Called when the command is initially scheduled. @Override public void initialize() { - DogLog.log("END_WHEN_CLOSE_ENOUGH", true); + DogLog.log("Commands/CloseEnough", false); } // Called every time the scheduler runs while the command is scheduled. @@ -44,7 +44,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("END_WHEN_CLOSE_ENOUGH", false); + DogLog.log("Commands/CloseEnough", true); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java b/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java index 0420890c..af88577d 100644 --- a/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java +++ b/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java @@ -9,13 +9,13 @@ public class CoralCheckedIn extends Command { public CoralCheckedIn(FunnelSubsystem funnelSubsystem) { this.funnelSubsystem = funnelSubsystem; - DogLog.log("CoralCheckedIn", false); + DogLog.log("CoralPosition/CoralCheckedIn", false); } // Called when the command is initially scheduled. @Override public void initialize() { - DogLog.log("CoralCheckedIn", true); + DogLog.log("CoralPosition/CoralCheckedIn", true); // Store the position of the coral when it was first checked out. } @@ -27,7 +27,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("CoralCheckedIn", false); + DogLog.log("CoralPosition/CoralCheckedIn", false); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java index ca4cc04b..a11cdcf5 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java @@ -1,7 +1,6 @@ package frc.robot.commands.SwerveCommands; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.SwerveSubsystem; @@ -25,7 +24,6 @@ public void initialize() { @Override public void execute() { - DogLog.log("restarted", counter); swerveDrivetrain.getModule(0).getDriveMotor().setVoltage(0.2425 + applyVoltage); swerveDrivetrain.getModule(1).getDriveMotor().setVoltage(0.2425 + applyVoltage); swerveDrivetrain.getModule(2).getDriveMotor().setVoltage(0.2425 + applyVoltage); diff --git a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java index 334ec461..2da6a0dd 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java @@ -99,21 +99,31 @@ public void execute() { ChassisSpeeds speeds = swerve.calculateRequiredEdwardChassisSpeeds(targetPose, initialPathDistance); - DogLog.log("JamesHardenMovement/TargetPose", targetPose); - DogLog.log("JamesHardenMovement/TargetPoseX(m)", targetPose.getX()); - DogLog.log("JamesHardenMovement/TargetPoseY(m)", targetPose.getY()); - DogLog.log("JamesHardenMovement/TargetPoseHeading(deg)", targetPose.getRotation().getRadians()); + DogLog.log("Commands/JamesHarden/TargetPose", targetPose); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseX(m)", targetPose.getX()); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseY(m)", targetPose.getY()); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", + targetPose.getRotation().getRadians()); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsX(radps)", speeds.omegaRadiansPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", + speeds.vxMetersPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", + speeds.vyMetersPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", + speeds.omegaRadiansPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", + swerve.getFieldSpeeds().vxMetersPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", + swerve.getFieldSpeeds().vyMetersPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsX(radps)", + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(radps)", swerve.getFieldSpeeds().omegaRadiansPerSecond); swerve.setFieldSpeeds(speeds); } @@ -127,21 +137,21 @@ public boolean isFinished() { currRot = ((2.0 * Math.PI) + (currRot % (2.0 * Math.PI))) % (2.0 * Math.PI); double targetRot = targetPose.getRotation().getRadians(); targetRot = ((2.0 * Math.PI) + (targetRot % (2.0 * Math.PI))) % (2.0 * Math.PI); - DogLog.log("JamesHardenMovement/currRot(rad)", currRot); - DogLog.log("JamesHardenMovement/targetRot(rad)", targetRot); + DogLog.log("Commands/JamesHarden/IsFinished/currRot(rad)", currRot); + DogLog.log("Commands/JamesHarden/IsFinished/targetRot(rad)", targetRot); DogLog.log( - "JamesHardenMovement/xTolMet", + "Commands/JamesHarden/IsFinished/xTolMet", (Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) < Constants.HardenConstants.RegularCommand.xyIndividualTolerance)); DogLog.log( - "JamesHardenMovement/yTolMet", + "Commands/JamesHarden/IsFinished/yTolMet", (Math.abs(swerve.getCurrentState().Pose.getY() - targetPose.getY()) < Constants.HardenConstants.RegularCommand.xyIndividualTolerance)); DogLog.log( - "JamesHardenMovement/rotTolMet", + "Commands/JamesHarden/IsFinished/rotTolMet", (Math.min(Math.abs(targetRot - currRot), (Math.PI * 2) - Math.abs(targetRot - currRot)) < Constants.HardenConstants.RegularCommand.headingTolerance)); - DogLog.log("JamesHardenMovement/End", false); + DogLog.log("Commands/JamesHarden/End", false); // translational is met if ((Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) @@ -170,7 +180,7 @@ public boolean isFinished() { + (swerve.getFieldSpeeds().vyMetersPerSecond * swerve.getFieldSpeeds().vyMetersPerSecond)) <= 0.2)) { - DogLog.log("JamesHardenMovement/End", true); + DogLog.log("Commands/JamesHarden/End", true); return true; } else return false; } @@ -258,10 +268,6 @@ public static JamesHardenMovement toClosestRightBranch( Supplier targetBranch = () -> { Translation2d currPosition = swerve.getCurrentState().Pose.getTranslation(); - DogLog.log("currPositionX", currPosition.getX()); - DogLog.log("currPositionY", currPosition.getY()); - DogLog.log("currPositionTheta", currPosition.getAngle().getRadians()); - DogLog.log("hardenRed", redSide.getAsBoolean()); if (redSide.getAsBoolean()) { double minDist = currPosition.getDistance(RedLandmarkPose.R0.getPose().getTranslation()); diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index ebe874fe..e9a02bb1 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -193,10 +193,10 @@ public void execute() { final double y = ySpeed; double turn = turningSpeed; - DogLog.log("joystickCommand/xSpeed", xSpeed); - DogLog.log("joystickCommand/ySpeed", ySpeed); - DogLog.log("joystickCommand/turningSpeed", turningSpeed); - DogLog.log("fieldCentric", fieldRelativeFunction.getAsBoolean()); + DogLog.log("Commands/joystickCommand/xSpeed", xSpeed); + DogLog.log("Commands/joystickCommand/ySpeed", ySpeed); + DogLog.log("Commands/joystickCommand/turningSpeed", turningSpeed); + DogLog.log("Information/fieldCentric", fieldRelativeFunction.getAsBoolean()); // 5. Applying the drive request on the swerve drivetrain // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) if (fixedRotation.getAsBoolean()) { diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java new file mode 100644 index 00000000..3cbf93df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -0,0 +1,356 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import dev.doglog.DogLog; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import java.util.List; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.stream.Collectors; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** + * VisionSystem fuses multi-tag AprilTag measurements from PhotonVision with swerve odometry by + * dynamically computing measurement noise based on tag count, distance, viewing angle, and robot + * speed. + */ +public class AnthonyVision extends SubsystemBase { + // static member of AnthonyVision that contains array of all existing AnthonyVision systems + private static AnthonyVision[] systemList = + new AnthonyVision[Constants.Vision.Cameras.values().length]; + + // Data type is "Cameras", an enum defined in Constants.java with only two options (left, right) + private final Constants.Vision.Cameras cameraId; + + private String camTitle; + // Reef tag IDs for each side of the field + private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); + private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); + + // Noise parameters + private double calibrationFactor = 1.0; // constant multiplier to everything + private double baseNoiseX = 0.0008; // meters + private double baseNoiseY = 0.0008; + private double baseNoiseTheta = 0.5; // radians + + // private double distanceCoefficientX = 0.06; + // private double distanceCoefficientY = 0.06; + + private double distanceExponentialCoefficientX = 0.00046074; + private double distanceExponentialBaseX = 2.97294; + private double distanceExponentialCoefficientY = 0.00046074; + private double distanceExponentialBaseY = 2.97294; + + private double distanceCoefficientTheta = 0.9; + + private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle + private double angleCoefficientY = 0.5; + private double angleCoefficientTheta = 0.5; + + private double speedCoefficientX = 0.5; // noise growth per fraction of max speed + private double speedCoefficientY = 0.5; + private double speedCoefficientTheta = 0.5; + + // Maximums for normalization + private double maximumRobotSpeed = 5; // meters per second + private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped + + // PhotonVision and odometry references + private final PhotonCamera photonCamera; + private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR + private PhotonPipelineResult latestVisionResult; + private final BooleanSupplier isRedSide; + private SwerveSubsystem swerveDrive; + private final AprilTagFieldLayout fieldLayout; + + public AnthonyVision(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + this.isRedSide = isRedSide; + this.cameraId = cameraId; + photonCamera = new PhotonCamera(cameraId.toString()); + Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); + + // Initialize field layout + this.fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + // Initialize both pose estimators + poseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + + camTitle = cameraId.getLoggingName(); + + latestVisionResult = null; + } + + // Get instance of AnthonyVision + public static AnthonyVision getInstance( + Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + int idx = cameraId.ordinal(); + if (systemList[idx] == null) { + systemList[idx] = new AnthonyVision(cameraId, isRedSide); + } + return systemList[idx]; + } + + @Override + public void periodic() { + // Initialize swerve drive if not already done + if (swerveDrive == null) { + swerveDrive = SwerveSubsystem.getInstance(); + } + + // Check camera connection + boolean cameraConnected = photonCamera.isConnected(); + + // If the current camera isn't connected, there's nothing to do here + if (!cameraConnected) { + DogLog.log("Vision/" + camTitle + "/CameraConnected", false); + return; + } + + DogLog.log("Vision/" + camTitle + "/CameraConnected", true); + // Get all unread results + List results = photonCamera.getAllUnreadResults(); + + // Go through all results (if there are any) and update the latest result with the last + for (var result : results) { + latestVisionResult = result; + } + } + + public void addFilteredPose() { + PhotonPoseEstimator selectedEstimator = poseEstimator; + if (latestVisionResult == null || !latestVisionResult.hasTargets()) { + DogLog.log("Vision/" + camTitle + "/HasTargets", false); + return; + } + DogLog.log("Vision/" + camTitle + "/HasTargets", true); + + double averageDistance = + latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .average() + .orElse(Double.NaN); + + double minDistance = + latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .min() + .orElse(Double.NaN); + + // Filter tags to only those on the active side + List validTags = + latestVisionResult.getTargets().stream() + .filter(t -> isTagOnActiveSide(t.getFiducialId())) + .filter(t -> isNotChopped(t.getYaw())) + .collect(Collectors.toList()); + + DogLog.log("Vision/" + camTitle + "/numValidTags", validTags.size()); + + for (PhotonTrackedTarget tag : validTags) { + DogLog.log("Vision/" + camTitle + "/Area", tag.getArea()); + DogLog.log("Vision/" + camTitle + "/Yaw", tag.getYaw()); + } + + // Log all detected tags for debugging + String allTagIds = + latestVisionResult.getTargets().stream() + .map(t -> Integer.toString(t.getFiducialId())) + .collect(Collectors.joining(",")); + + DogLog.log("Vision/" + camTitle + "/allTagIds", allTagIds); + + if (validTags.isEmpty()) { + DogLog.log("Vision/" + camTitle + "/ValidTags", false); + return; + } + DogLog.log("Vision/" + camTitle + "/ValidTags", true); + + // Log all tags that haven't been thrown out + int tagCount = validTags.size(); + + // Compute effective metrics for solution + // Use camera→target distance from PV (avoids odometry dependence) + + // nothing to do if rejected based on the minDistance or if no min dist has been found + if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { + DogLog.log("Vision/" + camTitle + "/ThrownOutDistance", true); + return; + } + DogLog.log("Vision/" + camTitle + "/ThrownOutDistance", false); + + // find the current speed + double currentSpeed = + Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); + DogLog.log("Vision/" + camTitle + "/VisionDrivebaseSpeed", currentSpeed); + + // Get the pose from PhotonVision + Optional maybePose = selectedEstimator.update(latestVisionResult); + if (maybePose.isEmpty()) { + DogLog.log("Vision/" + camTitle + "/AvailablePose", false); + return; + } + DogLog.log("Vision/" + camTitle + "/AvailablePose", true); + + EstimatedRobotPose estimatedPose = maybePose.get(); + Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); + + double nX = + computeNoiseXY( + baseNoiseX, + distanceExponentialCoefficientX, + distanceExponentialBaseX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + currentSpeed, + tagCount); + double nY = + computeNoiseXY( + baseNoiseY, + distanceExponentialCoefficientY, + distanceExponentialBaseY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + currentSpeed, + tagCount); + double nTH = + computeNoiseHeading( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + currentSpeed, + tagCount); + + DogLog.log("Vision/" + camTitle + "/speed", currentSpeed); + DogLog.log("Vision/" + camTitle + "/nX", nX); + DogLog.log("Vision/" + camTitle + "/nY", nY); + DogLog.log("Vision/" + camTitle + "/nTH", nTH); + DogLog.log("Vision/" + camTitle + "/Pose", measuredPose); + DogLog.log("Vision/" + camTitle + "/averageDistance", averageDistance); + + Matrix noiseVector = VecBuilder.fill(nX, nY, nTH); + // Process locally (no cross-camera comparison) + processPoseEstimate( + measuredPose, + averageDistance, + currentSpeed, + tagCount, + latestVisionResult.getTimestampSeconds(), + noiseVector); + } + + /** Final processing and addition of pose estimate to odometry. */ + private void processPoseEstimate( + Pose2d measuredPose, + double averageDistance, + double currentSpeed, + int tagCount, + double timestamp, + Matrix noiseVector) { + // Choose timestamp: use vision timestamp unless it differs too much from FPGA + double fpgaTimestamp = Timer.getFPGATimestamp(); + double timestampDifference = Math.abs(timestamp - fpgaTimestamp); + double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp - 0.03 : timestamp; + + // Build the noise vector and add the vision measurement + + swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); + } + + private boolean isTagOnActiveSide(int tagId) { + return isRedSide.getAsBoolean() + ? RED_SIDE_TAG_IDS.contains(tagId) + : BLUE_SIDE_TAG_IDS.contains(tagId); + } + + private boolean isNotChopped(double yaw) { + return (Math.abs(yaw) < 60d); + } + + private double computeNoiseXY( + double baseNoise, + double distanceExponentialCoefficient, + double distanceExponentialBase, + double angleCoefficient, + double speedCoefficient, + double distance, + double robotSpeed, + int tagCount) { + + // Tag count factor (diminishing returns; cap at 4) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1.0 / Math.sqrt(effectiveTags); + + // Distance term (keep as d^2) + // double distanceFactor = baseNoise + + // distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + + double distanceFactor = + (distance < (17.548 + 0.67)) + ? Math.min( + baseNoise + + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance), + 1.167) + : (baseNoise + + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance)); + + // Speed term (quadratic, saturated) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); + DogLog.log("Vision/calibrationFactor", calibrationFactor); + DogLog.log("Vision/tagFactor", tagFactor); + DogLog.log("Vision/distanceFactor", distanceFactor); + DogLog.log("Vision/speedFactor", speedFactor); + + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } + + private double computeNoiseHeading( + double baseNoise, + double distanceCoefficient, + double angleCoefficient, + double speedCoefficient, + double distance, + double robotSpeed, + int tagCount) { + + // Tag count factor (diminishing returns; cap at 4) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1.0 / Math.sqrt(effectiveTags); + + // Distance term (keep as d^2) + double distanceFactor = baseNoise + distanceCoefficient * distance * distance; + + // Speed term (quadratic, saturated) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); + + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 31289751..366ec512 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -159,7 +159,7 @@ public boolean checkCurrent() { private void runFlywheelAtRPS(double flywheelSpeed) { double motor_speed = flywheelSpeed / Constants.Flywheel.GEAR_RATIO; motor_speed = MathUtil.clamp(motor_speed, -100, 100); - DogLog.log("subsystems/Dale/Target Motor Speed", motor_speed); + DogLog.log("Subsystems/Dale/Target Motor Speed", motor_speed); flywheelMotor.setControl(controlRequestFlywheel.withVelocity(motor_speed)); } @@ -194,15 +194,15 @@ public void periodic() { encoderDegrees = Constants.Arm.ROTATIONS_TO_DEGEREES(armMotor.getPosition(false).getValueAsDouble()); // This method will be called once per scheduler run - DogLog.log("subsystems/Dale/Arm at target", atTarget(5)); - DogLog.log("subsystems/Dale/Arm Degrees", encoderDegrees); + DogLog.log("Subsystems/Dale/Arm at target", atTarget(5)); + DogLog.log("Subsystems/Dale/Arm Degrees", encoderDegrees); DogLog.log( - "subsystems/Dale/Arm Target Rotations", Constants.Arm.DEGREES_TO_ROTATIONS(targetDegrees)); - DogLog.log("subsystems/Dale/Arm Target Degrees", targetDegrees); + "Subsystems/Dale/Arm Target Rotations", Constants.Arm.DEGREES_TO_ROTATIONS(targetDegrees)); + DogLog.log("Subsystems/Dale/Arm Target Degrees", targetDegrees); // DogLog.log( // "subsystems/Dale/Flywheel Speed", flywheelMotor.getVelocity(false).getValueAsDouble()); DogLog.log( - "subsystems/Dale/command", + "Subsystems/Dale/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a00efcb2..8a3827ac 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -15,6 +15,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANrange; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import dev.doglog.DogLog; @@ -66,10 +67,10 @@ private ElevatorSubsystem() { ElevatorConstants.MOTOR2_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - currentLevel = ElevatorPositions.Intake; + currentLevel = ElevatorPositions.Intake; // Set up motor followers and deal with inverted motors - Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, MotorAlignmentValue.Opposed); motor2.setControl(follower); Slot1Configs s1c = @@ -143,7 +144,7 @@ public void resetPositionFiltered() { master.setPosition( currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -153,7 +154,7 @@ public void resetPosition() { this.getToFDistance() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", this.getToFDistance() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -164,7 +165,7 @@ public void resetPosition(double posInHeight) { master.setPosition( posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -194,8 +195,8 @@ public void resetElevatorPositionToZero() { public boolean checkCurrent() { double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); + DogLog.log("Subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); + DogLog.log("Subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); if (Supplycurrent > 1.0 && Statorcurrent > 20) { return true; @@ -232,14 +233,14 @@ public void setPosition(double height) { / ElevatorConstants.CARRAIGE_UPDUCTION) .withSlot(0)); DogLog.log( - "subsystems/Elevator/elevatorSetpoint(rot)", + "Subsystems/Elevator/elevatorSetpoint(rot)", height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / ElevatorConstants.CARRAIGE_UPDUCTION); } public void ElevatorTorqueMode() { - DogLog.log("subsystems/Elevator/usingTorqueMode", true); + DogLog.log("Subsystems/Elevator/usingTorqueMode", true); master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); } @@ -261,7 +262,7 @@ public boolean canFunnelTransferCoralToScoring() { public double getToFDistance() { // 0.11 is the sensor offset DogLog.log( - "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); + "Subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; } @@ -277,38 +278,38 @@ public void elevatorHasBeenZeroed() { public void periodic() { currentHeightToF = elevatorFilter.calculate(getToFDistance()); // Time of Flight Sensor - DogLog.log("subsystems/Elevator/getError", getError()); - DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); - DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); - DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); - - DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); - DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); - DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); + DogLog.log("Subsystems/Elevator/getError", getError()); + DogLog.log("Subsystems/Elevator/ToF/Distance", getToFDistance()); + DogLog.log("Subsystems/Elevator/ToF/Connected", distance.isConnected()); + DogLog.log("Subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); + + DogLog.log("Subsystems/Elevator/isAtPosition", this.isAtPosition()); + DogLog.log("Subsystems/Elevator/targetPosition", currentLevel.getPosition()); + DogLog.log("Subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); DogLog.log( - "subsystems/Elevator/targetHeightRot", + "Subsystems/Elevator/targetHeightRot", currentLevel.getHeight() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); DogLog.log( - "subsystems/Elevator/currentHeightDist", + "Subsystems/Elevator/currentHeightDist", master.getPosition().getValueAsDouble() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); + DogLog.log("Subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); DogLog.log( - "subsystems/Elevator/command", + "Subsystems/Elevator/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); DogLog.log( - "subsystems/Elevator/resetPositionBoolean", + "Subsystems/Elevator/resetPositionBoolean", this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); DogLog.log( - "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); + "Subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); + DogLog.log("Subsystems/Elevator/targetLevel", this.getLevel().toString()); DogLog.log( - "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); + "Subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); DogLog.log( - "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); + "Subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); } @Override @@ -321,9 +322,5 @@ public void simulationPeriodic() { double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop master.setPosition( newPosition); // Alarming to have this since running this on the robot will lead to - - // Log simulation data for debugging - DogLog.log("Simulated Position", newPosition); - DogLog.log("Simulated Speed", simulatedSpeed); } } diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index 543bb5c7..4582c49f 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -15,6 +15,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.signals.ControlModeValue; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import dev.doglog.DogLog; import edu.wpi.first.wpilibj.DigitalInput; @@ -47,7 +48,7 @@ private FunnelSubsystem() { checkOutSensor = new DigitalInput(Constants.FunnelConstants.CHECK_OUT_PORT); checkInSensor = new DigitalInput(Constants.FunnelConstants.CHECK_IN_PORT); - Follower invertedfollower = new Follower(FunnelConstants.RIGHT_MOTOR_PORT, true); + Follower invertedfollower = new Follower(FunnelConstants.RIGHT_MOTOR_PORT, MotorAlignmentValue.Aligned); leftMotor.setControl(invertedfollower); TalonFXConfigurator m1Config = rightMotor.getConfigurator(); @@ -195,13 +196,13 @@ public boolean drakeTripped() { @Override public void periodic() { // This method will be called once per scheduler run - DogLog.log("subsystems/Funnel/CheckedInStatus", isCoralCheckedIn()); - DogLog.log("subsystems/Funnel/CheckedOutStatus", isCoralCheckedOut()); - DogLog.log("subsystems/Funnel/DrakeStatus", drakeTripped()); + DogLog.log("Subsystems/Funnel/CheckedInStatus", isCoralCheckedIn()); + DogLog.log("Subsystems/Funnel/CheckedOutStatus", isCoralCheckedOut()); + DogLog.log("Subsystems/Funnel/DrakeStatus", drakeTripped()); // DogLog.log("subsystems/Funnel/FunnelVelocity", rightMotor.getVelocity().getValueAsDouble()); - DogLog.log("subsystems/Funnel/AbsPositionalError", getAbsolutePositionalError()); + DogLog.log("Subsystems/Funnel/AbsPositionalError", getAbsolutePositionalError()); DogLog.log( - "subsystems/Funnel/command", + "Subsystems/Funnel/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 72b21422..25f88f6e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -69,22 +69,21 @@ public SwerveSubsystem( // 1.7, 0.345, 0.0015 qProfiledPIDController = new ProfiledPIDController( - 3.4, // 3.4 not bad // [3.4 good for 0.2-1.2, 0.425 I] - 0.45, // 345 - 0.0005, // 0.0015 + Constants.HardenConstants.QKP, // 3.4 not bad // [3.4 good for 0.2-1.2, 0.425 I] + Constants.HardenConstants.QKI, // 345 + Constants.HardenConstants.QKD, // 0.0015 new TrapezoidProfile.Constraints( - Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND - 0.5, - 8)); // 8.25 // 5 accel and 0.75 p was good + Constants.HardenConstants.QCRUISE, + Constants.HardenConstants.QACCEL)); // 8.25 // 5 accel and 0.75 p was good headingProfiledPIDController = new ProfiledPIDController( - 3.7, // 4 was good - 0.4, // - 0, + Constants.HardenConstants.HKP, // 4 was good + Constants.HardenConstants.HKI, // + Constants.HardenConstants.HKD, new TrapezoidProfile.Constraints( - Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_RATE - 1.5, // -1 was good - Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_ACCELERATION_UNITS_PER_SECOND - - 16)); // -13 was good + Constants.HardenConstants.HCRUISE, // -1 was good + Constants.HardenConstants.HACCEL)); // -13 was good // headingProfiledPIDController = // new ProfiledPIDController( // 1, // 4 was good @@ -95,11 +94,14 @@ public SwerveSubsystem( // Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_ACCELERATION_UNITS_PER_SECOND // - 16)); // -13 was good - qProfiledPIDController.setIZone(0.35); - headingProfiledPIDController.setIZone(0.14); + qProfiledPIDController.setIZone(Constants.HardenConstants.QIZONE); + headingProfiledPIDController.setIZone(Constants.HardenConstants.HIZONE); - qProfiledPIDController.setIntegratorRange(0, 0.2); - headingProfiledPIDController.setIntegratorRange(0.0, Math.PI / 4); // 0.3 before + qProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); + headingProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.HIRANGE_LOWER, + Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before headingProfiledPIDController.enableContinuousInput(-Math.PI, Math.PI); // configureAutoBuilder(); @@ -344,39 +346,39 @@ public ChassisSpeeds calculateRequiredEdwardChassisSpeeds( Rotation2d travelAngle = travelAngleTo(targetPose); DogLog.log( - "EdwardCalculation/qProfiledPID/CurrentPositionMeasurement", + "Commands/JamesHarden/Edward/qPositionMeasurement", completePathDistance - distanceToTarget); DogLog.log( - "EdwardCalculation/qProfiledPID/PositionSetpoint", + "Commands/JamesHarden/Edward/qPositionSetpoint", qProfiledPIDController.getSetpoint().position); DogLog.log( - "EdwardCalculation/qProfiledPID/CurrentVelocityMeasurement", + "Commands/JamesHarden/Edward/qVelocityMeasurement", getDirectionalChassisSpeeds(travelAngle)); DogLog.log( - "EdwardCalculation/qProfiledPID/VelocitySetpoint", + "Commands/JamesHarden/Edward/qVelocitySetpoint", qProfiledPIDController.getSetpoint().velocity); DogLog.log( - "EdwardCalculation/qProfiledPID/PositionError", qProfiledPIDController.getPositionError()); + "Commands/JamesHarden/Edward/qPositionError", qProfiledPIDController.getPositionError()); DogLog.log( - "EdwardCalculation/qProfiledPID/VelocityError", qProfiledPIDController.getVelocityError()); + "Commands/JamesHarden/Edward/qVelocityError", qProfiledPIDController.getVelocityError()); DogLog.log( - "EdwardCalculation/headingProfiledPID/CurrentPositionMeasurement", + "Commands/JamesHarden/Rotational/PositionMeasurement", currentState.Pose.getRotation().getRadians()); DogLog.log( - "EdwardCalculation/headingProfiledPID/PositionSetpoint", + "Commands/JamesHarden/Rotational/PositionSetpoint", headingProfiledPIDController.getSetpoint().position); DogLog.log( - "EdwardCalculation/headingProfiledPID/CurrentVelocityMeasurement", + "Commands/JamesHarden/Rotational/VelocityMeasurement", currentState.Speeds.omegaRadiansPerSecond); DogLog.log( - "EdwardCalculation/headingProfiledPID/VelocitySetpoint", + "Commands/JamesHarden/Rotational/VelocitySetpoint", headingProfiledPIDController.getSetpoint().velocity); DogLog.log( - "EdwardCalculation/headingProfiledPID/PositionError", + "Commands/JamesHarden/Rotational/PositionError", headingProfiledPIDController.getPositionError()); DogLog.log( - "EdwardCalculation/headingProfiledPID/VelocityError", + "Commands/JamesHarden/Rotational/VelocityError", headingProfiledPIDController.getVelocityError()); return new ChassisSpeeds( @@ -455,25 +457,6 @@ public void addVisionMeasurement( @Override public void periodic() { - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - currentState = getState(); if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { @@ -488,16 +471,10 @@ public void periodic() { }); } - DogLog.log("subsystems/swerve/fl_speed", currentState.ModuleStates[0].speedMetersPerSecond); - DogLog.log("subsystems/swerve/fl_angle", currentState.ModuleStates[0].angle.getDegrees()); - DogLog.log("subsystems/swerve/fr_speed", currentState.ModuleStates[1].speedMetersPerSecond); - DogLog.log("subsystems/swerve/fr_angle", currentState.ModuleStates[1].angle.getDegrees()); - DogLog.log("subsystems/swerve/bl_speed", currentState.ModuleStates[2].speedMetersPerSecond); - DogLog.log("subsystems/swerve/bl_angle", currentState.ModuleStates[2].angle.getDegrees()); - DogLog.log("subsystems/swerve/br_speed", currentState.ModuleStates[3].speedMetersPerSecond); - DogLog.log("subsystems/swerve/br_angle", currentState.ModuleStates[3].angle.getDegrees()); + DogLog.log("PoseEstimation/DrivetrainPose", currentState.Pose); + DogLog.log( - "subsystems/swerve/command", + "Subsystems/SwerveSubsystem/CurrentCommand", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } } diff --git a/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java b/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java index fc705c73..ea9e4677 100644 --- a/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java @@ -77,7 +77,7 @@ public static TootsieSlideSubsystem getInstance() { private void runTootsieAtRPS(double flywheelSpeed) { double motor_speed = flywheelSpeed / Constants.TootsieSlide.GEAR_RATIO; - DogLog.log("subsystems/tootsieslide/Target Motor Speed", motor_speed); + DogLog.log("Subsystems/tootsieslide/Target Motor Speed", motor_speed); master.setControl(m_velocity.withVelocity(motor_speed)); m_flywheelSim.setInputVoltage(master.getSupplyVoltage().getValueAsDouble()); @@ -113,7 +113,7 @@ public void periodic() { // DogLog.log("subsystems/tootsieslide/tootsieVelocity", // master.getVelocity().getValueAsDouble()); DogLog.log( - "subsystems/tootsieslide/command", + "Subsystems/tootsieslide/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java deleted file mode 100644 index 5004edc8..00000000 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ /dev/null @@ -1,274 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import dev.doglog.DogLog; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -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.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.util.MiscUtils; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** Creates a new VisionSystem. */ -public class VisionSystem extends SubsystemBase { - - public static int numThrowaways = 0; - public static int numNotThrowaways = 0; - List reefIDs = - new ArrayList(Arrays.asList(19, 20, 21, 22, 17, 18, 6, 7, 8, 9, 10, 11)); - List blueReefID = new ArrayList(Arrays.asList(19, 20, 21, 22, 17, 18)); - List redReefID = new ArrayList(Arrays.asList(6, 7, 8, 9, 10, 11)); - - Pose2d savedResult = new Pose2d(0, 0, new Rotation2d(0.01, 0.01)); - private static VisionSystem[] systemList = - new VisionSystem[Constants.Vision.Cameras.values().length]; - private Transform3d[] camToRobots = { - // right Camera transform - new Transform3d( - new Translation3d( - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_X, - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_Y, - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_Z), - new Rotation3d( - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_ROLL, - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_PITCH, - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_YAW)), - // left Camera - new Transform3d( - new Translation3d( - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_X, - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_Y, - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_Z), - new Rotation3d( - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_ROLL, - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_PITCH, - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_YAW)), - }; - private PhotonCamera camera; - private Constants.Vision.Cameras cameraEnum; - private PhotonPipelineResult pipeline; - AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - PhotonPoseEstimator photonPoseEstimator; - private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance(); - private BooleanSupplier redSide; - - public VisionSystem(Constants.Vision.Cameras cameraEnum, BooleanSupplier redSide) { - this.cameraEnum = cameraEnum; - String name = cameraEnum.toString(); - int index = cameraEnum.ordinal(); - camera = new PhotonCamera(name); - photonPoseEstimator = - new PhotonPoseEstimator( - aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camToRobots[index]); - this.redSide = redSide; - for (var x : camera.getAllUnreadResults()) { - pipeline = x; - } - } - - public Double getDistance() { - return this.getAprilTagFieldLayout() - .getTagPose(getPipelineResult().getBestTarget().getFiducialId()) - .get() - .getTranslation() - .getDistance( - new Translation3d( - driveTrain.getState().Pose.getX(), driveTrain.getState().Pose.getY(), 0.0)); - } - - public static VisionSystem getInstance(Constants.Vision.Cameras name, BooleanSupplier redSide) { - if (systemList[name.ordinal()] == null) { - systemList[name.ordinal()] = new VisionSystem(name, redSide); - } - - return systemList[name.ordinal()]; - } - - // use this when feeding into drivetrain - public Optional getMultiTagPose3d(Pose2d previousRobotPose) { - photonPoseEstimator.setReferencePose(previousRobotPose); - return photonPoseEstimator.update(pipeline); - } - - public Pose2d getPose2d() { - Optional pose3d = getMultiTagPose3d(savedResult); - if (pose3d.isEmpty()) return null; - savedResult = pose3d.get().estimatedPose.toPose2d(); - return savedResult; - } - - public Pose2d getSaved() { - return savedResult; - } - - public boolean hasTarget(PhotonPipelineResult pipeline) { - if (pipeline == null) { - return false; - } - return pipeline.hasTargets(); - } - - public void setReference(Pose2d newPose) { - if (newPose == null) { - return; - } - savedResult = newPose; - } - - public static Pose2d getAverageForOffBotTesting(Pose2d one, Pose2d two) { - if (one == null) return two; - if (two == null) return one; - - return new Pose2d( - (one.getX() + two.getX()) / 2, (one.getY() + two.getY()) / 2, one.getRotation()); - } - - public AprilTagFieldLayout getAprilTagFieldLayout() { - return this.aprilTagFieldLayout; - } - - public PhotonPipelineResult getPipelineResult() { - return pipeline; - } - - public void addFilteredPose() { - String camName = cameraEnum.getLoggingName(); - PhotonPipelineResult pipelineResult = getPipelineResult(); - - DogLog.log("KalmanDebug/" + camName + "PiplineNull", pipelineResult == null); - DogLog.log("KalmanDebug/" + camName + "PipelineHasTarget", hasTarget(pipelineResult)); - - if (pipelineResult != null && hasTarget(pipelineResult)) { - - List targets = pipelineResult.getTargets(); - - boolean hasReefTag = true; - double poseAmbiguity = pipelineResult.getBestTarget().poseAmbiguity; - double distance = getDistance(); - - DogLog.log("KalmanDebug/" + camName + "PoseAmbiguity", poseAmbiguity); - DogLog.log("KalmanDebug/" + camName + "DistToAprilTag", distance); - - for (PhotonTrackedTarget target : targets) { - DogLog.log("redside in vision system", redSide.getAsBoolean()); - if (redSide.getAsBoolean()) { - if (!redReefID.contains(target.getFiducialId())) { - hasReefTag = false; - } - } else { - if (!blueReefID.contains(target.getFiducialId())) { - hasReefTag = false; - } - } - // if (!reefIDs.contains(target.getFiducialId())) { - // hasReefTag = false; - // } - } - - if (hasReefTag) { - double speedMultiplier = - (Math.sqrt( - Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2))) - / 2 - + 1.0; - double xKalman = MiscUtils.lerp((distance - 0.62) / 3, 0.03, 0.3, 1.0) * speedMultiplier; - double yKalman = MiscUtils.lerp((distance - 0.62) / 3, 0.03, 0.3, 1.0) * speedMultiplier; - // double speedMultiplier = 1; - // if (Math.sqrt( - // Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - // + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2)) - // > 1) { - // speedMultiplier = 2; - // } - // speedMultiplier = - // (Math.sqrt( - // Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - // + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2))) - // / 2 - // + 1.0; - // double xKalman = MiscUtils.lerp((distance - 0.6) / 2.4, 0.04, 0.5, 1.0) * - // speedMultiplier; - // double yKalman = MiscUtils.lerp((distance - 0.6) / 2.4, 0.04, 0.5, 1.0) * - // speedMultiplier; - double rotationKalman = MiscUtils.lerp((distance - 0.6) / 1.4, 0.4, 5, 30) / 10; - - DogLog.log("KalmanDebug/" + camName + "TranslationStandardDeviation", xKalman); - DogLog.log("KalmanDebug/" + camName + "RotationStandardDeviation", rotationKalman); - - Matrix visionMatrix = VecBuilder.fill(xKalman, yKalman, rotationKalman); - Pose2d bestRobotPose2d = getPose2d(); - - if (bestRobotPose2d == null) { - VisionSystem.numThrowaways++; - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - DogLog.log("KalmanDebug/" + camName + "throwaways", VisionSystem.numThrowaways); - return; - } - Pose2d rotationLess = - new Pose2d(bestRobotPose2d.getTranslation(), driveTrain.getState().Pose.getRotation()); - DogLog.log("KalmanDebug/" + camName + "Rotationless", rotationLess); - DogLog.log("KalmanDebug/" + camName + "RobotPoseIsPresent", bestRobotPose2d != null); - DogLog.log("KalmanDebug/" + camName + "VisionPose", bestRobotPose2d); - - double xDifference = Math.abs(driveTrain.getPose().getX() - bestRobotPose2d.getX()); - double yDifference = Math.abs(driveTrain.getPose().getY() - bestRobotPose2d.getY()); - double rotDifference = - Math.abs( - driveTrain.getPose().getRotation().getDegrees() - - bestRobotPose2d.getRotation().getDegrees()); - Translation2d transDifference = new Translation2d(xDifference, yDifference); - Rotation2d rot2dDifference = new Rotation2d(rotDifference); - - Pose2d visionDiff = new Pose2d(transDifference, rot2dDifference); - double timeDiff = Math.abs(pipelineResult.getTimestampSeconds() - Timer.getTimestamp()); - DogLog.log("KalmanDebug/" + camName + "visionDiff", visionDiff); - DogLog.log("KalmanDebug/" + camName + "diffInTimestamps", timeDiff); - - driveTrain.addVisionMeasurement( - bestRobotPose2d, - timeDiff > 5 ? Timer.getTimestamp() - 0.070 : pipelineResult.getTimestampSeconds(), - visionMatrix); - VisionSystem.numNotThrowaways++; - DogLog.log("KalmanDebug/" + camName + "numNotThrowaway", VisionSystem.numNotThrowaways); - DogLog.log("KalmanDebug/" + camName + "visionUsed", true); - } else { - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - } - } else { - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - } - } - - @Override - public void periodic() { - for (var x : camera.getAllUnreadResults()) { - pipeline = x; - } - } -} diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json index b410eb1d..05e93535 100644 --- a/vendordeps/DogLog.json +++ b/vendordeps/DogLog.json @@ -3,11 +3,11 @@ { "groupId": "com.github.jonahsnider", "artifactId": "doglog", - "version": "2025.3.0" + "version": "2026.2.0" } ], "fileName": "DogLog.json", - "frcYear": "2025", + "frcYear": "2026", "jsonUrl": "https://doglog.dev/vendordep.json", "name": "DogLog", "jniDependencies": [], @@ -15,6 +15,6 @@ "https://jitpack.io" ], "cppDependencies": [], - "version": "2025.3.0", + "version": "2026.2.0", "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" } \ No newline at end of file diff --git a/vendordeps/Phoenix5-5.35.1.json b/vendordeps/Phoenix5-5.36.0.json similarity index 87% rename from vendordeps/Phoenix5-5.35.1.json rename to vendordeps/Phoenix5-5.36.0.json index 69df8b53..9a27e478 100644 --- a/vendordeps/Phoenix5-5.35.1.json +++ b/vendordeps/Phoenix5-5.36.0.json @@ -1,50 +1,50 @@ { - "fileName": "Phoenix5-5.35.1.json", + "fileName": "Phoenix5-5.36.0.json", "name": "CTRE-Phoenix (v5)", - "version": "5.35.1", - "frcYear": "2025", + "version": "5.36.0", + "frcYear": "2026", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-latest.json", "requires": [ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6-frc2025-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + "offlineFileName": "Phoenix6-frc2026-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json" } ], "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" }, { "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - "offlineFileName": "Phoenix5-replay-frc2025-latest.json" + "offlineFileName": "Phoenix5-replay-frc2026-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.35.1" + "version": "5.36.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.35.1" + "version": "5.36.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.35.1", + "version": "5.36.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.35.1", + "version": "5.36.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.35.1", + "version": "5.36.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.35.1", + "version": "5.36.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -106,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.35.1", + "version": "5.36.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -122,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.35.1", + "version": "5.36.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.35.1", + "version": "5.36.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -154,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.35.1", + "version": "5.36.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-26.1.0.json similarity index 84% rename from vendordeps/Phoenix6-frc2025-latest.json rename to vendordeps/Phoenix6-26.1.0.json index acc78dba..dc5dc62b 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-26.1.0.json @@ -1,32 +1,32 @@ { - "fileName": "Phoenix6-frc2025-latest.json", + "fileName": "Phoenix6-26.1.0.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", - "frcYear": "2025", + "version": "26.1.0", + "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "26.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -123,8 +123,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.2", + "artifactId": "simProTalonFX", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -137,8 +137,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.2", + "artifactId": "simProTalonFXS", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -151,8 +151,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.2", + "artifactId": "simProCANcoder", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,8 +165,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.2", + "artifactId": "simProPigeon2", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -179,8 +179,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.2", + "artifactId": "simProCANrange", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -193,8 +193,22 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.2", + "artifactId": "simProCANdi", + "version": "26.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "26.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -321,9 +335,9 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.2", - "libName": "CTRE_SimCANCoder", + "artifactId": "simProTalonFX", + "version": "26.1.0", + "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -337,9 +351,9 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.2", - "libName": "CTRE_SimProTalonFX", + "artifactId": "simProTalonFXS", + "version": "26.1.0", + "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -353,9 +367,9 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.2", - "libName": "CTRE_SimProTalonFXS", + "artifactId": "simProCANcoder", + "version": "26.1.0", + "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -369,9 +383,9 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.2", - "libName": "CTRE_SimProCANcoder", + "artifactId": "simProPigeon2", + "version": "26.1.0", + "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -385,9 +399,9 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.2", - "libName": "CTRE_SimProPigeon2", + "artifactId": "simProCANrange", + "version": "26.1.0", + "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -401,9 +415,25 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.2", - "libName": "CTRE_SimProCANrange", + "artifactId": "simProCANdi", + "version": "26.1.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.0", + "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 3718e0ac..d90630e9 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ @@ -25,6 +25,7 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "linuxsystemcore", "linuxathena", "linuxarm32", "linuxarm64", diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 2d7b1d8e..9f40a208 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,9 +1,10 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.3.1", + "version": "v2026.0.1-beta", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", + "frcYear": "2026", + "_truefrcYear": "2026beta", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" @@ -13,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", + "version": "v2026.0.1-beta", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.3.1", + "version": "v2026.0.1-beta", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +44,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", + "version": "v2026.0.1-beta", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +61,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.3.1" + "version": "v2026.0.1-beta" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.3.1" + "version": "v2026.0.1-beta" } ] } \ No newline at end of file From 8f14b96bc85869c0f433df86499567da003f6814 Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Mon, 12 Jan 2026 19:05:07 -0800 Subject: [PATCH 2/3] fix elevator/funnel motors set in wrong direction. --- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/FunnelSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 8a3827ac..814f6f38 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -70,7 +70,7 @@ private ElevatorSubsystem() { currentLevel = ElevatorPositions.Intake; // Set up motor followers and deal with inverted motors - Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, MotorAlignmentValue.Opposed); + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, MotorAlignmentValue.Aligned); motor2.setControl(follower); Slot1Configs s1c = diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index 4582c49f..59c4f835 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -48,7 +48,7 @@ private FunnelSubsystem() { checkOutSensor = new DigitalInput(Constants.FunnelConstants.CHECK_OUT_PORT); checkInSensor = new DigitalInput(Constants.FunnelConstants.CHECK_IN_PORT); - Follower invertedfollower = new Follower(FunnelConstants.RIGHT_MOTOR_PORT, MotorAlignmentValue.Aligned); + Follower invertedfollower = new Follower(FunnelConstants.RIGHT_MOTOR_PORT, MotorAlignmentValue.Opposed); leftMotor.setControl(invertedfollower); TalonFXConfigurator m1Config = rightMotor.getConfigurator(); From 585a4cafeef4b49067b5d269e0053a118a3a5c9c Mon Sep 17 00:00:00 2001 From: Sid Senthil Date: Mon, 12 Jan 2026 19:10:00 -0800 Subject: [PATCH 3/3] update photonlib to 2026 full release --- vendordeps/photonlib.json | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 9f40a208..0a948bd3 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,10 +1,9 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.0.1-beta", + "version": "v2026.1.1-rc-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", - "_truefrcYear": "2026beta", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" @@ -14,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2026.1.1-rc-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -29,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.0.1-beta", + "version": "v2026.1.1-rc-2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -44,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2026.1.1-rc-2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -61,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.0.1-beta" + "version": "v2026.1.1-rc-2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.0.1-beta" + "version": "v2026.1.1-rc-2" } ] } \ No newline at end of file