diff --git a/.gitignore b/.gitignore index 5528d4f..34cbaac 100644 --- a/.gitignore +++ b/.gitignore @@ -169,6 +169,8 @@ out/ .fleet # Simulation GUI and other tools window save file +networktables.json +simgui.json *-window.json # Simulation data log directory @@ -176,3 +178,10 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json deleted file mode 100644 index 2aaab01..0000000 --- a/.pathplanner/settings.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "robotWidth": 0.9, - "robotLength": 0.9, - "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b..612cdd0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,36 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "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.*", + ] } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 9cca484..f90367b 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025", "teamNumber": 1899 } \ No newline at end of file diff --git a/README.md b/README.md index 71b124f..4a28141 100644 --- a/README.md +++ b/README.md @@ -1,21 +1,26 @@ -# swerve-base -Code for a basic swerve drive base +# Reefscape2025 +FRC team Saints Robotics 1899's code for the 2025 season. -## Features: -* Swerve drive -* Heading correction -* WPILib Sim support -* AdvantageScope support -* Pose estimation -## TODO/WIP: -* Auton setup -* Test heading correction +## Commit Message Prefixes +All commit messages must start with one of the following prefixes. -### Sim Controls -* W, A, S, D - Basic field-relative translation movement -* Left Arrow, Right Arrow - Rotational movement -* R, E - Increase/Decrease speed scaling (needs to be held down) -* N (Holding down) + W, A, S, D - Robot Relative driving -* Comma - Reset heading \ No newline at end of file +* **"docs:"** - Documentation changes +* **"feat:"** - New features +* **"fix:"** - Bug fixes +* **"refactor:"** - Changes that affect code organization +* **"style:"** - Changes that are formatting related (white-space, formatting, missing semi-colons, etc) +* **"test:"** - New tests or correcting existing tests +* **"perf:"** - Improves performance +* **"chore:"** - Miscellaneous changes + +### Examples + +* feat: created swerve functions +* fix: autonomous turning bug +* refactor: decoupled functions into new file +* docs: added more info about the codebase +* style: formatted the gradle file +* test: fixed deadzone test +* chore: licensing and credits diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec..645e542 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors +Copyright (c) 2009-2024 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 07feb12..05fce2d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" } java { @@ -33,6 +33,8 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' + deleteOldFiles = true // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project } } } @@ -42,7 +44,7 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava // Set to true to use debug for JNI. -// wpi.java.debugJni = false +wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true @@ -50,6 +52,7 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -67,13 +70,16 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - // testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - // testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } test { - // useJUnitPlatform() - // systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + testLogging { + exceptionFormat = 'full' + } } // Simulation configuration (e.g. environment variables). @@ -99,3 +105,4 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index d64cd49..a4b76b9 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67..34bd9ce 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a4..f5feea6 100755 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59..9d21a21 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail diff --git a/settings.gradle b/settings.gradle index d94f73c..c493958 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' 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 index d68dc6b..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -12,30 +12,18 @@ }, { "decKey": 69, - "decayRate": 0.009999999776482582, + "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 - }, - { - "decKey": 264, - "incKey": 265 - }, - { - "decKey": 262, - "incKey": 263 } ], - "axisCount": 5, - "buttonCount": 8, + "axisCount": 3, + "buttonCount": 4, "buttonKeys": [ 90, 88, 67, - 86, - 66, - 78, - 77, - 44 + 86 ], "povConfig": [ { @@ -65,14 +53,24 @@ "axisCount": 2, "buttonCount": 4, "buttonKeys": [ - -1, - -1, + 77, + 44, 46, 47 ], "povCount": 0 }, { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], "axisCount": 2, "buttonCount": 6, "buttonKeys": [ @@ -93,10 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" - }, - { - "guid": "Keyboard1" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json deleted file mode 100644 index 0f24e0c..0000000 --- a/simgui.json +++ /dev/null @@ -1,32 +0,0 @@ -{ - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", - "/SmartDashboard/Field": "Field2d" - }, - "windows": { - "/SmartDashboard/Field": { - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "transitory": { - "SmartDashboard": { - "Field": { - "open": true - }, - "open": true - } - } - } -} diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..f5432c9 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,40 @@ +{ + "robotWidth": 0.9144, + "robotLength": 0.9144, + "holonomicMode": true, + "pathFolders": [ + "LeftPaths", + "MiddlePath", + "RightPaths" + ], + "autoFolders": [ + "LeftAutons", + "MiddleAutons", + "RightAutons" + ], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 62.5957, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.05, + "driveGearing": 8.14, + "maxDriveSpeed": 4.4196, + "driveMotorType": "vortex", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7a147a0..aa95e56 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; @@ -26,37 +27,47 @@ */ public final class Constants { + public static final double kFastPeriodicPeriod = 0.01; // 100Hz, 10ms + public static final double kFastPeriodicOfset = 1e-6; // 1 micro second, no noticable effect other than order + /** * Input/Output constants */ public static final class IOConstants { public static final int kDriverControllerPort = 0; + public static final int kOperatorControllerPort = 1; - public static final double kControllerDeadband = 0.2; + public static final double kControllerDeadband = 0.15; public static final double kSlowModeScalar = 0.8; + + public static final double kElevatorAxisScalar = 0.05; //TODO: tune + public static final double kPivotAxisScalar = -0.25; //TODO: tune + + public static final int kDPadUp = 0; + public static final int kDPadRight = 90; + public static final int kDPadDown = 180; + public static final int kDPadLeft = 270; + + public static final double kHapticTime = 0.3; + public static final double kHapticStrength = 1; } public static final class DriveConstants { // TODO: set motor and encoder constants - public static final int kFrontLeftDriveMotorPort = 1; - public static final int kRearLeftDriveMotorPort = 3; - public static final int kFrontRightDriveMotorPort = 5; - public static final int kRearRightDriveMotorPort = 7; - - public static final int kFrontLeftTurningMotorPort = 2; - public static final int kRearLeftTurningMotorPort = 4; - public static final int kFrontRightTurningMotorPort = 6; - public static final int kRearRightTurningMotorPort = 8; - - public static final int kFrontLeftTurningEncoderPort = 9; - public static final int kRearLeftTurningEncoderPort = 10; - public static final int kFrontRightTurningEncoderPort = 11; - public static final int kRearRightTurningEncoderPort = 12; - - public static final double kFrontLeftTurningEncoderOffset = 0; - public static final double kRearLeftTurningEncoderOffset = 0; - public static final double kFrontRightTurningEncoderOffset = 0; - public static final double kRearRightTurningEncoderOffset = 0; + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 36; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 5; + public static final int kRearLeftTurningEncoderPort = 6; + public static final int kFrontRightTurningEncoderPort = 3; + public static final int kRearRightTurningEncoderPort = 4; // TODO: Test motor orientations before driving on an actual robot public static final boolean kFrontLeftDriveMotorReversed = false; @@ -65,55 +76,80 @@ public static final class DriveConstants { public static final boolean kRearRightDriveMotorReversed = true; /** Distance between centers of right and left wheels on robot (in meters). */ - public static final double kTrackWidth = 0.5; + public static final double kTrackWidth = 0.57785; /** Distance between front and back wheels on robot (in meters). */ - public static final double kWheelBase = 0.5; + public static final double kWheelBase = 0.57785; /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ public static final double kWheelDiameterMeters = 0.1; /** Gear ratio between the motor and the wheel. */ - public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 configuration + public static final double kDrivingGearRatio = 6.12; // SDS MK4i's in L2 configuration // TODO: Tune this PID before running on a robot on the ground - public static final double kPModuleTurningController = -0.3; + public static final double kPModuleTurningController = 0.3; - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + public static final Translation2d[] kModulePositions = new Translation2d[] { + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2) + }; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(kModulePositions); /** For a a SDS Mk4i L1 swerve base with Neos */ - public static final double kMaxSpeedMetersPerSecond = 3.6576; + public static final double kMaxSpeedMetersPerSecond = 4.4196; + + // TODO: Set max acceleration constants + public static final double kMaxAccelerationMetersPerSecondSquared = 1; + /** For a a SDS Mk4i L1 swerve base with Neos */ - public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3; + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; /** Heading Correction */ public static final double kHeadingCorrectionTurningStopTime = 0.2; // TODO: Tune this PID before running on a robot on the ground public static final double kPHeadingCorrectionController = 5; + + public static final boolean kAutoDriving = true; + + // TODO: set these on real robot + public static final double kMaxAccelerationUnitsPerSecond = 100; + public static final double kMaxAngularAccelerationUnitsPerSecond = 100; + + // max distance for auto driving in meters + public static final double kMaxAutoDistance = 5; } public static final class VisionConstants { // TODO: Update cam pose relative to center of bot - public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final double[] kLimelightCamPose = { - kCamPose.getX(), - kCamPose.getY(), - kCamPose.getZ(), - kCamPose.getRotation().getX(), - kCamPose.getRotation().getY(), - kCamPose.getRotation().getZ() }; + public static final Pose3d kCamPosLeft = new Pose3d( + // new Translation3d(0.3048,0.254,0), + new Translation3d(0.3429, -0.2413, 0.2413), + new Rotation3d(0,10,0) + ); + + public static final Pose3d kCamPosRight = new Pose3d( + new Translation3d(0.3429, 0.2413, 0.2413), + new Rotation3d(0,0,0) + ); + + + + public static final String kLimelightNameLeft = "limelight"; + public static final String kLimelightNameRight = "limelight-sr"; + + // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2 + public static final int kIMUMode = 0; // TODO: Experiment with different std devs in the pose estimator public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); - public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.7, 0.7, 999999); - // Field size in meters - public static final double kFieldWidth = 8.21055; - public static final double kFieldLength = 16.54175; + public static final boolean kUseVision = true; + public static final boolean kUseLeftLL = true; + public static final boolean kUseRightLL = true; } - } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..46f1bf7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,7 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + this.addPeriodic(m_robotContainer::fastPeriodic, Constants.kFastPeriodicPeriod, Constants.kFastPeriodicOfset); } /** @@ -53,16 +54,6 @@ public void disabledInit() {} @Override public void disabledPeriodic() {} - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } /** This function is called periodically during autonomous. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bdbb6ed..59fdb3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,11 @@ package frc.robot; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -26,11 +27,14 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + // Configure the trigger bindings configureBindings(); @@ -41,45 +45,61 @@ public RobotContainer() { -m_driverController.getLeftY(), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) + * (1 - (m_driverController + .getRightBumperButton() ? IOConstants.kSlowModeScalar : 0)) * 0.8, MathUtil.applyDeadband( -m_driverController.getLeftX(), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() + * (1 - (m_driverController + .getRightBumperButton() ? 1 : 0) * IOConstants.kSlowModeScalar) * 0.8, MathUtil.applyDeadband( m_driverController.getRightX(), IOConstants.kControllerDeadband) * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() + * (1 - (m_driverController + .getRightBumperButton() ? 1 : 0) * IOConstants.kSlowModeScalar) - / 2, - !m_driverController.getRightBumper()), - m_robotDrive)); - } + * -1, + true), + m_robotDrive)); +} + /** - * Use this method to define your button->command mappings. + * Driver Controls: + * + * Driving: + * left axis X/Y axis Translation + * right axis X axis Rotation + * start press Reset heading + * back press Reset position + */ private void configureBindings() { + // -------- driving bindings -------- // + + // driver reset heading new JoystickButton(m_driverController, Button.kStart.value) .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); + + // driver reset odometry + new JoystickButton(m_driverController, Button.kBack.value) + .onTrue(new InstantCommand(() -> m_robotDrive.resetOdometry(new Pose2d()), m_robotDrive)); } /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous + * This periodic loop runs every 10ms (100Hz) + * + *

+ * Should be used for any code that needs to be run more frequently than the + * default 20ms loop (50Hz) such as PID Controllers. + *

*/ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return null; + public void fastPeriodic() { + m_robotDrive.fastPeriodic(); } } diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 0000000..6f13b37 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,98 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.utils.AllianceFlipUtil; + +public class DriveToPose extends Command { + + private final DriveSubsystem m_driveSubsystem; + private Pose2d currentPose; + private final Pose2d m_targetPose; + private Timer m_timer = new Timer(); + + private final TrapezoidProfile.Constraints constraints = new Constraints(DriveConstants.kMaxSpeedMetersPerSecond / 3, 5 / 2); + private final TrapezoidProfile.Constraints angularConstraints = new Constraints( + DriveConstants.kMaxAngularSpeedRadiansPerSecond / 10, 5 / 10); + + private final ProfiledPIDController xController = new ProfiledPIDController(3.0, 0.01, 0, constraints); + private final ProfiledPIDController yController = new ProfiledPIDController(3.0, 0.01, 0, constraints); + private final ProfiledPIDController thetaController = new ProfiledPIDController(8.0, 0.0, 0.0, angularConstraints); + + /** Creates a new DriveToPose. */ + public DriveToPose(DriveSubsystem driveSubsystem, Pose2d targetPose) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveSubsystem); + + xController.setTolerance(0.005, 0.05); + yController.setTolerance(0.005, 0.05); + thetaController.setTolerance(Math.toRadians(1.5), 0.05); + + thetaController.enableContinuousInput(0, Math.PI * 2); + + m_driveSubsystem = driveSubsystem; + m_targetPose = targetPose; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_timer.restart(); + + // TODO: reset state with current chassis speeds + currentPose = m_driveSubsystem.getPose(); + xController.reset(new State(currentPose.getX(), 0)); + yController.reset(new State(currentPose.getY(), 0)); + thetaController.reset(new State(currentPose.getRotation().getRadians(), 0)); + + SmartDashboard.putNumber("drive target X", m_targetPose.getX()); + SmartDashboard.putNumber("drive target Y", m_targetPose.getY()); + SmartDashboard.putNumber("drive target Rot", m_targetPose.getRotation().getDegrees()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + currentPose = m_driveSubsystem.getPose(); + + double xSpeed = xController.calculate(currentPose.getX(), m_targetPose.getX()); + double ySpeed = yController.calculate(currentPose.getY(), m_targetPose.getY()); + double thetaSpeed = thetaController.calculate(currentPose.getRotation().getRadians(), + m_targetPose.getRotation().getRadians()); + + if (AllianceFlipUtil.shouldFlip()){ + xSpeed = -xSpeed; + ySpeed = -ySpeed; + } + + if (DriveConstants.kAutoDriving) { + m_driveSubsystem.drive(xSpeed, ySpeed, thetaSpeed, true); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return !DriveConstants.kAutoDriving || (xController.atGoal() && + yController.atGoal() && + thetaController.atGoal()); + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f606d5b..f017a75 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -4,13 +4,15 @@ package frc.robot.subsystems; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -21,6 +23,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.VisionConstants; +import frc.robot.Constants; +import frc.robot.utils.AllianceFlipUtil; +import frc.robot.utils.LimelightHelpers; +import frc.robot.utils.SlewRateLimiter; import frc.robot.Robot; public class DriveSubsystem extends SubsystemBase { @@ -28,31 +34,27 @@ public class DriveSubsystem extends SubsystemBase { DriveConstants.kFrontLeftDriveMotorPort, DriveConstants.kFrontLeftTurningMotorPort, DriveConstants.kFrontLeftTurningEncoderPort, - DriveConstants.kFrontLeftDriveMotorReversed, - DriveConstants.kFrontLeftTurningEncoderOffset); + DriveConstants.kFrontLeftDriveMotorReversed); private final SwerveModule m_rearLeft = new SwerveModule( DriveConstants.kRearLeftDriveMotorPort, DriveConstants.kRearLeftTurningMotorPort, DriveConstants.kRearLeftTurningEncoderPort, - DriveConstants.kRearLeftDriveMotorReversed, - DriveConstants.kRearLeftTurningEncoderOffset); + DriveConstants.kRearLeftDriveMotorReversed); private final SwerveModule m_frontRight = new SwerveModule( DriveConstants.kFrontRightDriveMotorPort, DriveConstants.kFrontRightTurningMotorPort, DriveConstants.kFrontRightTurningEncoderPort, - DriveConstants.kFrontRightDriveMotorReversed, - DriveConstants.kFrontRightTurningEncoderOffset); + DriveConstants.kFrontRightDriveMotorReversed); private final SwerveModule m_rearRight = new SwerveModule( DriveConstants.kRearRightDriveMotorPort, DriveConstants.kRearRightTurningMotorPort, DriveConstants.kRearRightTurningEncoderPort, - DriveConstants.kRearRightDriveMotorReversed, - DriveConstants.kRearRightTurningEncoderOffset); + DriveConstants.kRearRightDriveMotorReversed); - private final AHRS m_gyro = new AHRS(); + private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI); private double m_gyroAngle; private final Timer m_headingCorrectionTimer = new Timer(); @@ -65,19 +67,67 @@ public class DriveSubsystem extends SubsystemBase { m_rearRight.getPosition() }; + private SwerveModuleState[] m_desiredStates; + private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics, m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs, VisionConstants.kVisionSTDDevs); private final Field2d m_field = new Field2d(); + private final Field2d m_flippedField = new Field2d(); + + private final SlewRateLimiter m_xSpeedLimiter = new SlewRateLimiter(DriveConstants.kMaxAccelerationUnitsPerSecond); + private final SlewRateLimiter m_ySpeedLimiter = new SlewRateLimiter(DriveConstants.kMaxAccelerationUnitsPerSecond); + private final SlewRateLimiter m_rotationSpeedLimiter = new SlewRateLimiter(DriveConstants.kMaxAngularAccelerationUnitsPerSecond); /** Creates a new DriveSubsystem. */ + @SuppressWarnings("unused") public DriveSubsystem() { + this.zeroHeading(); + this.resetOdometry(new Pose2d()); SmartDashboard.putData("Field", m_field); + SmartDashboard.putData("flipped field", m_flippedField); m_headingCorrectionTimer.restart(); m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI); + + // TODO: Set a custom crop window for improved performance (the bot only needs + // to see april tags on the reef) + m_poseEstimator.setVisionMeasurementStdDevs(VisionConstants.kVisionSTDDevs); + + if (VisionConstants.kUseVision && Robot.isReal()) { + if (VisionConstants.kUseLeftLL) { + LimelightHelpers.setCameraPose_RobotSpace( + VisionConstants.kLimelightNameLeft, + VisionConstants.kCamPosLeft.getX(), + VisionConstants.kCamPosLeft.getY(), + VisionConstants.kCamPosLeft.getZ(), + VisionConstants.kCamPosLeft.getRotation().getX(), + VisionConstants.kCamPosLeft.getRotation().getY(), + VisionConstants.kCamPosLeft.getRotation().getZ()); + LimelightHelpers.SetIMUMode(VisionConstants.kLimelightNameLeft, VisionConstants.kIMUMode); + + LimelightHelpers.setCropWindow(VisionConstants.kLimelightNameLeft, -1, 1, -1, 0.4); + } + + if (VisionConstants.kUseRightLL) { + LimelightHelpers.setCameraPose_RobotSpace( + VisionConstants.kLimelightNameRight, + VisionConstants.kCamPosRight.getX(), + VisionConstants.kCamPosRight.getY(), + VisionConstants.kCamPosRight.getZ(), + VisionConstants.kCamPosRight.getRotation().getX(), + VisionConstants.kCamPosRight.getRotation().getY(), + VisionConstants.kCamPosRight.getRotation().getZ()); + LimelightHelpers.SetIMUMode(VisionConstants.kLimelightNameRight, VisionConstants.kIMUMode); + + LimelightHelpers.setCropWindow(VisionConstants.kLimelightNameRight, -1, 1, -1, 0.4); + } + } + + m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(new ChassisSpeeds()); } + @SuppressWarnings("unused") @Override public void periodic() { // This method will be called once per scheduler run @@ -92,6 +142,9 @@ public void periodic() { m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), m_swerveModulePositions); + measureLimelight(VisionConstants.kLimelightNameLeft, VisionConstants.kUseLeftLL); + measureLimelight(VisionConstants.kLimelightNameRight, VisionConstants.kUseRightLL); + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); SmartDashboard.putNumber("gyro angle", m_gyro.getAngle()); @@ -99,13 +152,54 @@ public void periodic() { SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY()); // AdvantageScope Logging + // max speed = 1 (for ease of use in AdvantageScope) double[] logData = { m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput, m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput, m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput, m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput, }; + + double[] logDataDesired = { + m_desiredStates[0].angle.getDegrees(), m_desiredStates[0].speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond, + m_desiredStates[1].angle.getDegrees(), m_desiredStates[1].speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond, + m_desiredStates[2].angle.getDegrees(), m_desiredStates[2].speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond, + m_desiredStates[3].angle.getDegrees(), m_desiredStates[3].speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond, + }; + + SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logDataDesired); SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData); + + Pose2d flippedPose = AllianceFlipUtil.apply(getPose()); + m_flippedField.setRobotPose(flippedPose); + SmartDashboard.putNumber("flip X", flippedPose.getX()); + SmartDashboard.putNumber("flip Y", flippedPose.getY()); + SmartDashboard.putNumber("flip Rot", flippedPose.getRotation().getDegrees()); + } + + public void measureLimelight(String name, boolean useLimelight) { + + if (!useLimelight) { + return; + } + + boolean LLreal = LimelightHelpers.getLatency_Pipeline(name) != 0.0; + if (VisionConstants.kUseVision && Robot.isReal() && LLreal) { + // Update LimeLight with current robot orientation + LimelightHelpers.SetRobotOrientation(name, m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0); + + // Get the pose estimate + LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + + // Add it to your pose estimator if it is a valid measurement + if (limelightMeasurement != null && limelightMeasurement.tagCount != 0 && m_gyro.getRate() < 720) { + m_poseEstimator.addVisionMeasurement( + limelightMeasurement.pose, + limelightMeasurement.timestampSeconds); + } + } + + SmartDashboard.putBoolean(name + " valid", LLreal); } /** @@ -163,15 +257,37 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe calculatedRotation = m_headingCorrectionPID.calculate(currentAngle); } + // TODO: set speed limiter rate based on elevator height using interlocks/constraints + // m_xSpeedLimiter.setRateLimit(DriveConstants.kMaxAccelerationUnitsPerSecond); + // m_ySpeedLimiter.setRateLimit(DriveConstants.kMaxAccelerationUnitsPerSecond); + // m_rotationSpeedLimiter.setRateLimit(DriveConstants.kMaxAngularAccelerationUnitsPerSecond); + + /** + * To prevent jerks when swapping to robot relative driving, + * we convert our robot relative speeds to field relative early + * so the previous value stored in the slew rate limiter filter is always valid + */ + + // If we are not driving in field relative, then convert our robot relative speeds to field relative + if (!fieldRelative) { + Translation2d fieldRelativeTranslation = new Translation2d(xSpeed, ySpeed).rotateBy(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle)); + + xSpeed = fieldRelativeTranslation.getX(); + ySpeed = fieldRelativeTranslation.getY(); + // rotation doesn't need to be updated because it is the same in both field and robot relative + } + + xSpeed = m_xSpeedLimiter.calculate(xSpeed); + ySpeed = m_ySpeedLimiter.calculate(ySpeed); + calculatedRotation = m_rotationSpeedLimiter.calculate(calculatedRotation); + // Depending on whether the robot is being driven in field relative, calculate // the desired states for each of the modules - SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation, - Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle)) - : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation)); + m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation, + Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))); - setModuleStates(swerveModuleStates); + SwerveDriveKinematics.desaturateWheelSpeeds(m_desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); } /** @@ -180,53 +296,73 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { + resetOdometry(pose, false); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + * @param ignoreRotation True if rotation in pose should be ignored (i.e. use gyro) + */ + public void resetOdometry(Pose2d pose, boolean ignoreRotation) { + Rotation2d rot = Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle); + + if (AllianceFlipUtil.shouldFlip()) { + rot = new Rotation2d(rot.getRadians() + Math.PI); + } + m_poseEstimator.resetPosition( - Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), + rot, new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), m_rearLeft.getPosition(), m_rearRight.getPosition() }, - pose); + ignoreRotation ? new Pose2d(pose.getTranslation(), rot) : pose); } - /** Zeroes the heading of the robot. */ + /** + * Sets the current heading to zero + * @param heading heading in radians + */ public void zeroHeading() { - m_gyro.reset(); - m_gyroAngle = 0; + zeroHeading(0); + } + + /** + * Sets the current heading to heading + * @param heading heading in radians + */ + public void zeroHeading(double heading) { + m_gyro.reset(); // does not actually need to be the correct value. all uses only care about rate + m_gyroAngle = heading; + resetOdometry(new Pose2d(getPose().getX(), getPose().getY(), new Rotation2d(heading)), false); } public void addVisionMeasurement(Pose2d pose, double timestamp) { m_poseEstimator.addVisionMeasurement(pose, timestamp); } - /** - * Sets the swerve ModuleStates. - * - * @param desiredStates The desired SwerveModule states. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(desiredStates[0]); - m_frontRight.setDesiredState(desiredStates[1]); - m_rearLeft.setDesiredState(desiredStates[2]); - m_rearRight.setDesiredState(desiredStates[3]); - - // AdvantageScope Logging - double[] logData = { - desiredStates[0].angle.getDegrees(), desiredStates[0].speedMetersPerSecond, - desiredStates[1].angle.getDegrees(), desiredStates[1].speedMetersPerSecond, - desiredStates[2].angle.getDegrees(), desiredStates[2].speedMetersPerSecond, - desiredStates[3].angle.getDegrees(), desiredStates[3].speedMetersPerSecond, - }; - SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logData); + /** Sets the module states every 10ms (100Hz), faster than the regular periodic loop */ + public void fastPeriodic() { + m_frontLeft.setDesiredState(m_desiredStates[0]); + m_frontRight.setDesiredState(m_desiredStates[1]); + m_rearLeft.setDesiredState(m_desiredStates[2]); + m_rearRight.setDesiredState(m_desiredStates[3]); // Takes the integral of the rotation speed to find the current angle for the // simulator - m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond - * Robot.kDefaultPeriod; + m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates).omegaRadiansPerSecond + * Constants.kFastPeriodicPeriod; } + public void autonDrive(ChassisSpeeds speeds) { + m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds); + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates); + } } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index f4de950..87c1f06 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -4,29 +4,33 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants; import frc.robot.Robot; public class SwerveModule { - private final CANSparkMax m_driveMotor; - private final CANSparkMax m_turningMotor; + private final SparkFlex m_driveMotor; + private final SparkFlex m_turningMotor; + + private final SparkFlexConfig m_driveMotorConfig = new SparkFlexConfig(); + private final SparkFlexConfig m_turningMotorConfig = new SparkFlexConfig(); private final CANcoder m_turningEncoder; - private final CANcoderConfigurator m_turningEncoderConfigurator; - private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, - 0); + private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, 0, Constants.kFastPeriodicPeriod); private SwerveModuleState m_state = new SwerveModuleState(); private double m_distance; @@ -37,34 +41,29 @@ public class SwerveModule { /** * Constructs a {@link SwerveModule}. * - * @param driveMotorPort The port of the drive motor. - * @param turningMotorPort The port of the turning motor. - * @param turningEncoderPort The port of the turning encoder. - * @param driveMotorReversed Whether the drive motor is reversed. - * @param turningEncoderOffset Offset of the turning encoder. + * @param driveMotorPort The port of the drive motor. + * @param turningMotorPort The port of the turning motor. + * @param turningEncoderPort The port of the turning encoder. + * @param driveMotorReversed Whether the drive motor is reversed. */ public SwerveModule( int driveMotorPort, int turningMotorPort, int turningEncoderPort, - boolean driveMotorReversed, - double turningEncoderOffset) { - m_driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless); - m_turningMotor = new CANSparkMax(turningMotorPort, MotorType.kBrushless); + boolean driveMotorReversed) { + m_driveMotor = new SparkFlex(driveMotorPort, MotorType.kBrushless); + m_turningMotor = new SparkFlex(turningMotorPort, MotorType.kBrushless); m_turningEncoder = new CANcoder(turningEncoderPort); - m_turningEncoderConfigurator = m_turningEncoder.getConfigurator(); // converts default units to meters per second - m_driveMotor.getEncoder().setVelocityConversionFactor( - DriveConstants.kWheelDiameterMeters * Math.PI / 60 / DriveConstants.kDrivingGearRatio); - - m_driveMotor.setInverted(driveMotorReversed); + m_driveMotorConfig.encoder.positionConversionFactor( + DriveConstants.kWheelDiameterMeters * Math.PI / DriveConstants.kDrivingGearRatio); + m_driveMotorConfig.inverted(driveMotorReversed); - m_turningMotor.setIdleMode(IdleMode.kBrake); + m_turningMotorConfig.idleMode(IdleMode.kBrake); - // TODO: CANcoder offsets are now set on the device manually using Pheonix Tuner - // (or maybe Pheonix X) - m_turningEncoderConfigurator.apply(new MagnetSensorConfigs().withMagnetOffset(-turningEncoderOffset)); + m_driveMotor.configure(m_driveMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + m_turningMotor.configure(m_turningMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); } @@ -93,14 +92,19 @@ public SwerveModulePosition getPosition() { * @param desiredState Desired state with speed and angle. */ public void setDesiredState(SwerveModuleState desiredState) { - m_state = SwerveModuleState.optimize(desiredState, getEncoderAngle(m_turningEncoder)); + m_state = desiredState; + m_state.optimize(getEncoderAngle(m_turningEncoder)); driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond; - turnOutput = m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(), + turnOutput = -m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(), m_state.angle.getRadians()); m_driveMotor.set(driveOutput); m_turningMotor.set(turnOutput); + + if (m_driveMotor.getDeviceId() == DriveConstants.kFrontLeftDriveMotorPort) { + SmartDashboard.putNumber("drive output", m_driveMotor.get()); + } } /** diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java deleted file mode 100644 index daeea25..0000000 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ /dev/null @@ -1,128 +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 java.util.Arrays; -import java.util.Optional; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -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; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.VisionConstants; - -/** - *

- * In 3D poses and transforms: - *

    - *
  • +X is north/forward, - *
  • +Y is west/left, - *
  • +Z is up. - *
- * - *

- * On the field (0, 0, 0) in 3D space is the right corner of the blue alliance - * driver station - * Therefore, from the blue driver station: +X is forward, +Y is left, +Z is up - * - *

- * In 2D poses and transforms: - *

    - *
  • +X is away from the driver, - *
  • +Y is toward the blue alliance driver's left and to the red alliance - * driver's right - *
  • +Rotation is clockwise - *
- */ -public class VisionSubsystem extends SubsystemBase { - - NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight"); - - private final DoubleArraySubscriber m_botPose; - private final DoubleSubscriber m_cl; - private final DoubleSubscriber m_tl; - - /** Creates a new Limelight. */ - public VisionSubsystem() { - // Provide the limelight with the camera pose relative to the center of the - // robot - m_visionNetworkTable.getEntry("camerapose_robotspace_set").setDoubleArray(VisionConstants.kLimelightCamPose); - - // Create subscribers to get values from the limelight - m_botPose = m_visionNetworkTable.getDoubleArrayTopic("botpose_wpiblue").subscribe(null); - m_cl = m_visionNetworkTable.getDoubleTopic("cl").subscribe(0); - m_tl = m_visionNetworkTable.getDoubleTopic("tl").subscribe(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - public Optional getMeasurement() { - TimestampedDoubleArray[] updates = m_botPose.readQueue(); - - // If we have had no updates since the last time this method ran then return - // nothing - if (updates.length == 0) { - return Optional.empty(); - } - - TimestampedDoubleArray update = updates[updates.length - 1]; - - // If the latest update is empty then return nothing - if (Arrays.equals(update.value, new double[6])) { - return Optional.empty(); - } - - double x = update.value[0]; - double y = update.value[1]; - double z = update.value[2]; - double roll = Units.degreesToRadians(update.value[3]); - double pitch = Units.degreesToRadians(update.value[4]); - double yaw = Units.degreesToRadians(update.value[5]); - - double latency = m_cl.get() + m_tl.get(); - - double timestamp = (update.timestamp * 1e-6) - (latency * 1e-3); - Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw)); - - /* - * The limelight returns 3D field poses where (0, 0, 0) is located at the center - * of the field - * - * So to input this pose into our pose estimator we need to tranform so that (0, - * 0, 0) is the right corner of the blue driver stations - */ - // TODO: Check if we actually need to do this... - // pose.transformBy(new Transform3d(new Translation3d(VisionConstants.kFieldLength, VisionConstants.kFieldWidth, 0.0), new Rotation3d())); - - return Optional.of(new Measurement( - timestamp, - pose, - VisionConstants.kVisionSTDDevs)); - } - - public static class Measurement { - public double timestamp; - public Pose3d pose; - public Matrix stdDeviation; - - public Measurement(double timestamp, Pose3d pose, Matrix stdDeviation) { - this.timestamp = timestamp; - this.pose = pose; - this.stdDeviation = stdDeviation; - } - } -} diff --git a/src/main/java/frc/robot/utils/AllianceFlipUtil.java b/src/main/java/frc/robot/utils/AllianceFlipUtil.java new file mode 100644 index 0000000..60fde1b --- /dev/null +++ b/src/main/java/frc/robot/utils/AllianceFlipUtil.java @@ -0,0 +1,44 @@ +// 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.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; + +/** Add your docs here. */ +public class AllianceFlipUtil { + public static double fieldWidth = Units.feetToMeters(26.0) + Units.inchesToMeters(5.0); + public static double fieldLength = Units.feetToMeters(57.0) + Units.inchesToMeters(6.875); + + public static double applyX(double x) { + return shouldFlip() ? fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } +} diff --git a/src/main/java/frc/robot/utils/FindNearest.java b/src/main/java/frc/robot/utils/FindNearest.java new file mode 100644 index 0000000..d5f5387 --- /dev/null +++ b/src/main/java/frc/robot/utils/FindNearest.java @@ -0,0 +1,82 @@ +// 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.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.Constants.DriveConstants; + +/** Finds the nearest scoring location on the reef */ +public class FindNearest { + + // Scoring locations for the blue alliance + public static final Pose2d[] blueScoringLocations = { + new Pose2d(new Translation2d(5.759, 3.952), Rotation2d.fromDegrees(180)), + new Pose2d(new Translation2d(5.36, 3.03), Rotation2d.fromDegrees(120)), + new Pose2d(new Translation2d(5.042, 2.88), Rotation2d.fromDegrees(120)), + new Pose2d(new Translation2d(4.035, 2.794), Rotation2d.fromDegrees(60)), + new Pose2d(new Translation2d(3.80, 2.95), Rotation2d.fromDegrees(60)), + new Pose2d(new Translation2d(3.222, 3.738), Rotation2d.fromDegrees(0)), + new Pose2d(new Translation2d(3.218, 4.015), Rotation2d.fromDegrees(0)), + new Pose2d(new Translation2d(3.638, 5.004), Rotation2d.fromDegrees(-60)), + new Pose2d(new Translation2d(3.94, 5.14), Rotation2d.fromDegrees(-60)), + new Pose2d(new Translation2d(4.95, 5.227), Rotation2d.fromDegrees(-120)), + new Pose2d(new Translation2d(5.247, 4.993), Rotation2d.fromDegrees(-120)), + new Pose2d(new Translation2d(5.759, 4.269), Rotation2d.fromDegrees(180)) + }; + + // Scoring locations for the red alliance + public static final Pose2d[] redScoringLocations = new Pose2d[blueScoringLocations.length]; + static { + for (int i = 0; i < blueScoringLocations.length; i++) { + redScoringLocations[i] = AllianceFlipUtil.apply(blueScoringLocations[i]); + } + } + + public static final Pose2d[] blueSources = { + new Pose2d(new Translation2d(1.7, 0.65), Rotation2d.fromDegrees(-127.5 + 180)), + new Pose2d(new Translation2d(1.7, 7.38), Rotation2d.fromDegrees(127.5 - 180)) + }; + + public static final Pose2d[] redSources = new Pose2d[blueSources.length]; + static { + for (int i = 0; i < blueSources.length; i++) { + redSources[i] = AllianceFlipUtil.apply(blueSources[i]); + } + } + + public static Pose2d getNearestScoringLocation(Pose2d currentPose) { + Pose2d[] scoringLocations = AllianceFlipUtil.shouldFlip() ? redScoringLocations : blueScoringLocations; + Pose2d nearestLocation = null; + double nearestDistance = Double.MAX_VALUE; + + for (Pose2d location : scoringLocations) { + double distance = currentPose.getTranslation().getDistance(location.getTranslation()); + if (distance < nearestDistance) { + nearestDistance = distance; + nearestLocation = location; + } + } + + return nearestLocation; + } + + public static Pose2d getNearestSource(Pose2d currentPose) { + Pose2d[] sources = AllianceFlipUtil.shouldFlip() ? redSources : blueSources; + Pose2d nearestSource = null; + double nearestDistance = DriveConstants.kMaxAutoDistance; + + for (Pose2d source : sources) { + double distance = currentPose.getTranslation().getDistance(source.getTranslation()); + if (distance < nearestDistance) { + nearestDistance = distance; + nearestSource = source; + } + } + + return nearestSource; + } +} diff --git a/src/main/java/frc/robot/utils/LimelightHelpers.java b/src/main/java/frc/robot/utils/LimelightHelpers.java new file mode 100644 index 0000000..9d8eed1 --- /dev/null +++ b/src/main/java/frc/robot/utils/LimelightHelpers.java @@ -0,0 +1,1645 @@ +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot.utils; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/SlewRateLimiter.java b/src/main/java/frc/robot/utils/SlewRateLimiter.java new file mode 100644 index 0000000..46cefa0 --- /dev/null +++ b/src/main/java/frc/robot/utils/SlewRateLimiter.java @@ -0,0 +1,117 @@ +// 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.utils; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUtil; + +/** + * A class that limits the rate of change of an input value. Useful for + * implementing voltage, + * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the + * quantity being + * controlled is a velocity or a voltage; when controlling a position, consider + * using a {@link + * edu.wpi.first.math.trajectory.TrapezoidProfile} instead. + */ +public class SlewRateLimiter { + private double m_positiveRateLimit; + private double m_negativeRateLimit; + private double m_prevVal; + private double m_prevTime; + + /** + * Creates a new SlewRateLimiter with the given positive and negative rate + * limits and initial + * value. + * + * @param positiveRateLimit The rate-of-change limit in the positive direction, + * in units per + * second. This is expected to be positive. + * @param negativeRateLimit The rate-of-change limit in the negative direction, + * in units per + * second. This is expected to be negative. + * @param initialValue The initial value of the input. + */ + public SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) { + m_positiveRateLimit = positiveRateLimit; + m_negativeRateLimit = negativeRateLimit; + m_prevVal = initialValue; + m_prevTime = MathSharedStore.getTimestamp(); + } + + /** + * Creates a new SlewRateLimiter with the given positive rate limit and negative + * rate limit of + * -rateLimit. + * + * @param rateLimit The rate-of-change limit, in units per second. + */ + public SlewRateLimiter(double rateLimit) { + this(rateLimit, -rateLimit, 0); + } + + /** + * Filters the input to limit its slew rate. + * + * @param input The input value whose slew rate is to be limited. + * @return The filtered value, which will not change faster than the slew rate. + */ + public double calculate(double input) { + double currentTime = MathSharedStore.getTimestamp(); + double elapsedTime = currentTime - m_prevTime; + m_prevVal += MathUtil.clamp( + input - m_prevVal, + m_negativeRateLimit * elapsedTime, + m_positiveRateLimit * elapsedTime); + m_prevTime = currentTime; + return m_prevVal; + } + + /** + * Returns the value last calculated by the SlewRateLimiter. + * + * @return The last value. + */ + public double lastValue() { + return m_prevVal; + } + + /** + * Resets the slew rate limiter to the specified value; ignores the rate limit + * when doing so. + * + * @param value The value to reset to. + */ + public void reset(double value) { + m_prevVal = value; + m_prevTime = MathSharedStore.getTimestamp(); + } + + /** + * Sets the rate limits of the SlewRateLimiter. + * + * @param positiveRateLimit The rate-of-change limit in the positive direction, + * in units per + * second. This is expected to be positive. + * @param negativeRateLimit The rate-of-change limit in the negative direction, + * in units per + * second. This is expected to be negative. + */ + public void setRateLimits(double positiveRateLimit, double negativeRateLimit) { + m_positiveRateLimit = positiveRateLimit; + m_negativeRateLimit = negativeRateLimit; + } + + /** + * Sets the positive and negative rate limits of the SlewRateLimiter. + * + * @param rateLimit The rate-of-change limit, in units per second. + */ + public void setRateLimit(double rateLimit) { + m_positiveRateLimit = rateLimit; + m_negativeRateLimit = -rateLimit; + } +} diff --git a/src/test/java/RobotTest.java b/src/test/java/RobotTest.java new file mode 100644 index 0000000..63bca30 --- /dev/null +++ b/src/test/java/RobotTest.java @@ -0,0 +1,54 @@ +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; + +import java.io.OutputStream; +import java.io.PrintStream; +import java.time.Duration; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.TimeoutException; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import frc.robot.Robot; + + +public class RobotTest { + @BeforeEach + void setup() {} + + /** + * Tests for robot code crashing within the first 5 seconds of the code starting + */ + @Test + void test_Robot() { + try (Robot robot = new Robot()) { + assertDoesNotThrow(() -> { + final Duration timeout = Duration.ofMillis(5000); + final ExecutorService executor = Executors.newSingleThreadExecutor(); + final Future future = executor.submit(() -> { + final PrintStream nullstream = new PrintStream(new OutputStream() { + public void write(int b) {} + }); + System.setOut(nullstream); + System.setErr(nullstream); + robot.startCompetition(); + }); + + try { + future.get(timeout.toMillis(), TimeUnit.MILLISECONDS); + } + catch (TimeoutException e) { + future.cancel(true); + } + executor.shutdown(); + }, "Robot code crashed"); + } + } + + @AfterEach + void shutdown() {} +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index e978a5f..0000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" - ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 85% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.1.json index cae1363..71e25f3 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,9 +1,9 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-25.2.0.json similarity index 71% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-25.2.0.json index 69a4079..38747fb 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-25.2.0.json @@ -1,76 +1,94 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-25.2.0.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", - "frcYear": 2024, + "version": "25.2.0", + "frcYear": "2025", "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-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "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" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "25.2.0" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.1.0", + "artifactId": "api-cpp-sim", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.1.0", + "artifactId": "tools-sim", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", + "artifactId": "simTalonSRX", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +138,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +180,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +218,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +234,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +250,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +266,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,81 +282,87 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimTalonFX", + "artifactId": "simVictorSPX", + "version": "25.2.0", + "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.1.0", - "libName": "CTRE_SimVictorSPX", + "artifactId": "simPigeonIMU", + "version": "25.2.0", + "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.1.0", - "libName": "CTRE_SimPigeonIMU", + "artifactId": "simCANCoder", + "version": "25.2.0", + "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.1.0", - "libName": "CTRE_SimCANCoder", + "artifactId": "simProTalonFX", + "version": "25.2.0", + "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimProTalonFX", + "artifactId": "simProTalonFXS", + "version": "25.2.0", + "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +378,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "25.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +394,23 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/REVLib-2024.json b/vendordeps/REVLib-2025.0.1.json similarity index 84% rename from vendordeps/REVLib-2024.json rename to vendordeps/REVLib-2025.0.1.json index f7c8225..c998054 100644 --- a/vendordeps/REVLib-2024.json +++ b/vendordeps/REVLib-2025.0.1.json @@ -1,30 +1,29 @@ { - "fileName": "REVLib.json", + "fileName": "REVLib-2025.0.1.json", "name": "REVLib", - "version": "2024.2.0", - "frcYear": "2024", + "version": "2025.0.1", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2025.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2025.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -71,4 +68,4 @@ ] } ] -} +} \ No newline at end of file diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 0000000..5010be0 --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf389..3718e0a 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": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [