diff --git a/.github/dependabot.yml b/.github/dependabot.yml index ffd8ba16..436a2a8b 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -10,8 +10,10 @@ updates: - package-ecosystem: "github-actions" # Workflow files stored in the default location of `.github/workflows`. (You don't need to specify `/.github/workflows` for `directory`. You can use `directory: "/"`.) directory: "/" + labels: + - "github_actions" schedule: - interval: "weekly" + interval: "monthly" # Maintain dependencies for gradle - package-ecosystem: "gradle" @@ -20,4 +22,4 @@ updates: labels: - "gradle dependencies" schedule: - interval: "monthly" + interval: "weekly" diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index af1c2e1b..0c9bc405 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -6,9 +6,9 @@ name: CI # events but only for the main branch. on: push: - branches: [ main ] + branches: [ main, develop ] pull_request: - branches: [ main ] + branches: [ main, develop ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: diff --git a/.vscode/settings.json b/.vscode/settings.json index ed950a7f..39a65d96 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -36,6 +36,36 @@ "spotlessGradle.format.enable": true, "spotlessGradle.diagnostics.enable": false, "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.*" + ], "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", "[json]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index a1591102..51005bf5 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025beta", + "projectYear": "2025", "teamNumber": 0 } diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json index e3c2d54e..f412ed27 100644 --- a/AdvantageScope Swerve Calibration.json +++ b/AdvantageScope Swerve Calibration.json @@ -122,7 +122,7 @@ } } ], - "game": "2024 Field", + "game": "2025 Field", "origin": "blue" }, "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", diff --git a/INSTALL.md b/INSTALL.md index d6087910..97a07847 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -24,13 +24,26 @@ recommended to **not** select the "Include all branches" checkbox. -------- -### Setting up your new project +### Software Requirements + +The Az-RBSI requires the [2025 WPILib Installer]( +https://github.com/wpilibsuite/allwpilib/releases) (VSCode and associated +tools), 2025 firmware installed on all hardware (motors, encoders, power +distribution, etc.), the [2025 NI FRC Game Tools]( +https://www.ni.com/en/support/downloads/drivers/download.frc-game-tools.html) +(Driver Station and associated tools), and the [2025 CTRE Phoenix Tuner X]( +https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/index.html). Take a +moment to update all software and firmware before attempting to load your new +robot project. + +Please note that you need these _minimum_ versions of the following components: + +* WPILib `2025.1.1` +* RoboRIO image `FRC_roboRIO_2025_v2.0` -**The Az-RBSI requires the 2025 WPILib Installer (VSCode and associated tools), -2025 firmware installed on all hardware (motors, encoders, power distribution, -etc.), the 2025 NI FRC Game Tools (Driver Station and associated tools), and -the 2025 CTRE Phoenix Tuner X. Take a moment to update all software and -firmware before attempting to load your new robot project.** +-------- + +### Setting up your new project When your new robot code respository is created, it will have a single commit that contains the entire Az-RBSI template for the current release. (See the @@ -42,9 +55,19 @@ steps you need to complete: 1. Add your team number to the `.wpilib/wpilib_preferences.json` file. The generic Az-RBSI template contains a team number "0", and your code will not - deploy properly if this variable is not set (*e.g.*, VSCode looks for the - RoboRIO on IP address `10.TE.AM.2`, and it will not find anything if it - tries to contact `10.0.0.2`.) + deploy properly if this variable is not set (*i.e.*, since VSCode looks for + the RoboRIO on IP address `10.TE.AM.2`, it will not find anything if it + tries to contact `10.0.0.2`.) If you forget to change this value, you will + get an error message when deploying code to your robot like: + + ``` + Missing Target! + ============================================= + Are you connected to the robot, and is it on? + ============================================= + GradleRIO detected this build failed due to not being able to find "roborio"! + Scroll up in this error log for more information. + ``` 2. If you have an all-CTRE swerve base (*i.e.*, 8x TalonFX-controlled motors, 4x CANCoders, and 1x Pigeon2), use Phoenix Tuner X to create a swerve @@ -76,14 +99,24 @@ method are encouraged to submit bug reports and code fixes to the [Az-RBSI repository](https://github.com/AZ-First/Az-RBSI). 6. The Az-RBSI expects an Xbox-style controller -- if you have a PS4 or other, - change this at the top of the `RobotContainer.java` file in the - `src/main/java/frc/robot` directory. - -7. Power ports... + substitute the proper command-based controller class for + `CommandXboxController` near the top of the `RobotContainer.java` file in + the `src/main/java/frc/robot` directory. + +7. Power monitoring by subsystem is included in the Az-RBSI. In order to + properly match subsystems to ports on your Power Distribution Module, + carefully edit the `CANandPowerPorts` of `Constants.java` to include the + proper power ports for each motor in your drivetrain, and include any + motors from additional subsystems you add to your robot. To include + additional subsystems in the monitoring, add them to the [`m_power` + instantiation]( + https://github.com/AZ-First/Az-RBSI/blob/38f6391cb70c4caa90502710f591682815064677/src/main/java/frc/robot/RobotContainer.java#L154-L157) in the `RobotContainer.java` file. 8. All of the constants for needed for tuning your robot should be in the `Constants.java` file in the `src/main/java/frc/robot` directory. This file - should be thoroughly edited to match the particulars of your robot. + should be thoroughly edited to match the particulars of your robot. Be sure + to work through each section of this file and include the proper values for + your robot. -------- diff --git a/README.md b/README.md index af25f09d..e923c7fd 100644 --- a/README.md +++ b/README.md @@ -53,3 +53,8 @@ effective logging for troubleshooting. * [PhotonVision](https://docs.photonvision.org/en/latest/) / [Limelight]( https://docs.limelightvision.io/docs/docs-limelight/getting-started/summary) -- Robot vision / tracking + +## Further Reading + +For tips on command-based programming, see this post: +https://bovlb.github.io/frc-tips/commands/best-practices.html diff --git a/WPILib-License.md b/WPILib-License.md index 645e5425..d744196f 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors +Copyright (c) 2009-2025 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 8a3142e4..f1e53c3d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" + id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "6.25.0" id "io.freefair.lombok" version "8.11" diff --git a/settings.gradle b/settings.gradle index d94f73c6..969c7b09 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') diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 690f5db2..bbd91940 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,7 +1,7 @@ { "field_size": { - "x": 16.54, - "y": 8.21 + "x": 17.548, + "y": 8.052 }, "nodeSizeMeters": 0.3, "grid": [ @@ -61,6 +61,9 @@ true, true, true, + true, + true, + true, true ], [ @@ -119,6 +122,9 @@ true, true, true, + true, + true, + true, true ], [ @@ -127,7 +133,6 @@ true, true, true, - true, false, false, false, @@ -171,8 +176,12 @@ false, false, false, - true, - true, + false, + false, + false, + false, + false, + false, true, true, true, @@ -184,7 +193,6 @@ true, true, true, - true, false, false, false, @@ -231,7 +239,11 @@ false, false, false, - true, + false, + false, + false, + false, + false, true, true, true, @@ -291,6 +303,9 @@ false, false, false, + false, + false, + false, true, true, true @@ -350,6 +365,9 @@ false, false, false, + false, + false, + false, true, true ], @@ -408,6 +426,9 @@ false, false, false, + false, + false, + false, true, true ], @@ -429,14 +450,6 @@ false, false, false, - true, - true, - true, - false, - false, - false, - false, - false, false, false, false, @@ -447,9 +460,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -466,12 +476,6 @@ false, false, false, - true, - true - ], - [ - true, - true, false, false, false, @@ -487,8 +491,9 @@ false, false, true, - true, - true, + true + ], + [ true, true, false, @@ -503,15 +508,8 @@ false, false, false, - false, - true, - true, true, true, - true, - false, - false, - false, false, false, false, @@ -524,12 +522,6 @@ false, false, false, - true, - true - ], - [ - true, - true, false, false, false, @@ -547,9 +539,6 @@ true, true, true, - true, - true, - false, false, false, false, @@ -563,15 +552,11 @@ false, false, true, + true + ], + [ true, true, - true, - true, - false, - false, - false, - false, - false, false, false, false, @@ -583,9 +568,9 @@ false, false, true, - true - ], - [ + true, + true, + true, true, true, false, @@ -602,15 +587,6 @@ false, false, false, - true, - true, - true, - true, - true, - false, - false, - false, - false, false, false, false, @@ -636,10 +612,6 @@ false, false, false, - false, - false, - false, - false, true, true ], @@ -654,13 +626,12 @@ false, false, false, + false, + true, + true, true, true, true, - false, - false, - false, - false, true, true, true, @@ -679,16 +650,20 @@ false, false, false, - true, - true, - true, false, false, false, false, + false, + false, + true, true, true, true, + true, + true, + true, + false, false, false, false, @@ -711,6 +686,12 @@ false, false, false, + false, + true, + true, + true, + true, + true, true, true, true, @@ -724,6 +705,9 @@ false, false, false, + true, + true, + true, false, false, false, @@ -732,6 +716,15 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -741,11 +734,10 @@ false, false, false, - false, - false, - true, - true, true, + true + ], + [ true, true, false, @@ -757,9 +749,13 @@ false, false, true, - true - ], - [ + true, + true, + true, + true, + true, + true, + true, true, true, false, @@ -781,6 +777,15 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -790,6 +795,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -798,9 +809,11 @@ false, false, false, - false, - false, - false, + true, + true, + true, + true, + true, true, true, true, @@ -815,9 +828,6 @@ false, true, true, - true - ], - [ true, true, true, @@ -827,6 +837,11 @@ false, false, false, + false, + true, + true, + true, + true, true, true, true, @@ -841,6 +856,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -849,9 +870,16 @@ false, false, false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -870,12 +898,12 @@ false, false, false, + false, + true, + true, true, true, true, - true - ], - [ true, true, true, @@ -886,14 +914,13 @@ false, false, false, - true, - true, - true, - false, false, false, false, true, + true + ], + [ true, true, false, @@ -904,6 +931,17 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, false, false, false, @@ -918,11 +956,6 @@ false, false, false, - true, - true, - true, - false, - false, false, false, false, @@ -931,9 +964,6 @@ true, true, true, - true - ], - [ true, true, true, @@ -948,13 +978,37 @@ false, false, false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, false, false, + false, + false, + true, + true, true, true, true, true, true, + true, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -973,6 +1027,8 @@ true, true, true, + true, + true, false, false, false, @@ -983,18 +1039,10 @@ false, false, false, - false, - true, - true, - true, - true, true, true ], [ - true, - true, - true, true, true, false, @@ -1007,12 +1055,22 @@ false, false, false, - false, true, true, true, true, true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1043,16 +1101,9 @@ false, false, true, - true, - true, - true, - true, true ], [ - true, - true, - true, true, true, false, @@ -1066,9 +1117,7 @@ false, false, false, - true, - true, - true, + false, true, true, false, @@ -1084,11 +1133,8 @@ false, false, false, - true, - true, - true, - true, - true, + false, + false, false, false, false, @@ -1103,14 +1149,22 @@ true, true, true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], [ - true, - true, - true, true, true, false, @@ -1125,9 +1179,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -1143,9 +1194,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -1158,17 +1206,26 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], [ - true, - true, - true, true, true, false, @@ -1216,16 +1273,20 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], [ - true, - true, true, true, false, @@ -1275,9 +1336,14 @@ false, false, false, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, true, true ], @@ -1334,12 +1400,17 @@ false, false, false, - true, + false, + false, + false, + false, true, true, true ], [ + true, + true, true, true, false, @@ -1395,9 +1466,13 @@ false, true, true, + true, true ], [ + true, + true, + true, true, true, false, @@ -1449,64 +1524,6 @@ false, false, false, - false, - false, - false, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, true, true, true, @@ -1569,6 +1586,9 @@ true, true, true, + true, + true, + true, true ], [ @@ -1627,6 +1647,9 @@ true, true, true, + true, + true, + true, true ] ] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8daa1dce..6d6eeac6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or @@ -17,24 +17,30 @@ package frc.robot; -import static frc.robot.util.RBSIEnum.*; - import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; import com.pathplanner.lib.config.PIDConstants; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.generated.TunerConstants; +import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.CTREPro; +import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.RBSIEnum.MotorIdleMode; +import frc.robot.util.RBSIEnum.RobotType; +import frc.robot.util.RBSIEnum.SwerveType; +import frc.robot.util.RBSIEnum.VisionType; +import frc.robot.util.RobotDeviceId; import java.io.IOException; import java.nio.file.Path; import lombok.Getter; @@ -63,7 +69,7 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static boolean phoenixPro = false; // CTRE Pro License? true, false + private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE @@ -93,61 +99,45 @@ public static void disableHAL() { /** Physical Constants for Robot Operation ******************************* */ public static final class PhysicalConstants { - public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final double kRobotMassKg = Units.lbsToKilograms(100.); public static final Matter kChassis = - new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), kRobotMass); - public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag + new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), kRobotMassKg); + // Robot moment of intertial; this can be obtained from a CAD model of your drivetrain. Usually, + // this is between 3 and 8 kg*m^2. + public static final double kRobotMOI = 6.8; + + // Wheel coefficient of friction + public static final double kWheelCOF = 1.2; } /** Power Distribution Constants ********************************** */ public static final class PowerConstants { - // Set this to either kRev or kCTRE for the type of Power Distribution Module - public static final ModuleType kPowerModule = ModuleType.kRev; // Current Limits public static final double kTotalMaxCurrent = 120.; public static final double kMotorPortMaxCurrent = 40.; public static final double kSmallPortMaxCurrent = 20.; } - /** Accelerometer Constants ********************************************** */ - public static class AccelerometerConstants { - - // Insert here the orientation (CCW == +) of the Rio and IMU from the robot - // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference - // frame. - // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP - public static final Rotation2d kRioOrientation = - switch (getRobot()) { - case COMPBOT -> Rotation2d.fromDegrees(0.); - case DEVBOT -> Rotation2d.fromDegrees(0.); - default -> Rotation2d.fromDegrees(0.); - }; - // IMU can be one of Pigeon2 or NavX - public static final Rotation2d kIMUOrientation = - switch (getRobot()) { - case COMPBOT -> Rotation2d.fromDegrees(0.); - case DEVBOT -> Rotation2d.fromDegrees(0.); - default -> Rotation2d.fromDegrees(0.); - }; - } - /** Drive Base Constants ************************************************* */ public static final class DrivebaseConstants { // Theoretical free speed (m/s) at 12v applied output; - // This needs to be tuned to your individual robot - // NOTE: If using SwerveType.PHOENIX6, adjust this in the Phoenix X Tuner Swerve Generator - public static final LinearVelocity kMaxLinearSpeed = TunerConstants.kSpeedAt12Volts; - // Otherwise, set the maximum linear speed here - // public static final double kMaxLinearSpeed = 5.21; + // IMPORTANT: Follow the AdvantageKit instructions for measuring the ACTUAL maximum linear speed + // of YOUR ROBOT, and replace the estimate here with your measured value! + public static final double kMaxLinearSpeed = Units.feetToMeters(18); - // Set 3/4 of a rotation per second as the max angular velocity + // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; // Maximum chassis accelerations desired for robot motion -- metric / radians + // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT! public static final double kMaxLinearAccel = 4.0; // m/s/s - public static final double kMaxAngularAccel = Units.degreesToRadians(720); // deg/s/s + public static final double kMaxAngularAccel = Units.degreesToRadians(720); + + // Drive and Turn PID constants + public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); + public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4); // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds @@ -157,11 +147,20 @@ public static final class DrivebaseConstants { public static final double kDelay = 3.0; // seconds public static final double kQuasiTimeout = 5.0; // seconds public static final double kDynamicTimeout = 3.0; // seconds + + // Not sure what to do with these, yet... + // kDriveF = 0.13; + // kDriveIZ = 0.0; + // kSteerF = 0.0; + // kSteerIZ = 0.0; } /** Example Flywheel Mechanism Constants ********************************* */ public static final class FlywheelConstants { + // Mechanism idle mode + public static final MotorIdleMode kFlywheelIdleMode = MotorIdleMode.COAST; // BRAKE, COAST + // Mechanism motor gear ratio public static final double kFlywheelGearRatio = 1.5; @@ -170,54 +169,76 @@ public static final class FlywheelConstants { public static final double kStaticGainReal = 0.1; public static final double kVelocityGainReal = 0.05; // Feedback (PID) constants - public static final double kPReal = 1.0; - public static final double kIReal = 0.0; - public static final double kDReal = 0.0; + public static final PIDConstants pidReal = new PIDConstants(1.0, 0.0, 0.0); // MODE == SIM // Feedforward constants public static final double kStaticGainSim = 0.0; public static final double kVelocityGainSim = 0.03; // Feedback (PID) constants - public static final double kPSim = 1.0; - public static final double kISim = 0.0; - public static final double kDSim = 0.0; + public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0); + } + + /** Accelerometer Constants ********************************************** */ + public static class AccelerometerConstants { + + // Insert here the orientation (CCW == +) of the Rio and IMU from the robot + // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference + // frame. + // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP + public static final Rotation2d kRioOrientation = + switch (getRobot()) { + case COMPBOT -> Rotation2d.fromDegrees(0.); + case DEVBOT -> Rotation2d.fromDegrees(0.); + default -> Rotation2d.fromDegrees(0.); + }; + // IMU can be one of Pigeon2 or NavX + public static final Rotation2d kIMUOrientation = + switch (getRobot()) { + case COMPBOT -> Rotation2d.fromDegrees(0.); + case DEVBOT -> Rotation2d.fromDegrees(0.); + default -> Rotation2d.fromDegrees(0.); + }; } /** Operator Constants *************************************************** */ public static class OperatorConstants { - // Joystick Deadband - public static final double kLeftDeadband = 0.1; - public static final double kRightDeadband = 0.1; + // Joystick Functions + // Set to TRUE for Drive = Left Stick, Turn = Right Stick; else FALSE + public static final boolean kDriveLeftTurnRight = true; + + // Joystick Deadbands + public static final double kDeadband = 0.1; public static final double kTurnConstant = 6; - } - /** Autonomous Action Constants ****************************************** */ - public static final class AutonConstants { + // Joystick slew rate limiters to smooth erratic joystick motions, measured in units per second + public static final double kJoystickSlewLimit = 0.5; - // Translation PID constants - public static final PIDConstants kAutoTranslatePID = new PIDConstants(0.7, 0, 0); - // Rotation PID constants - public static final PIDConstants kAutoAnglePID = new PIDConstants(0.4, 0, 0.01); - } + // Override and Console Toggle Switches + // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK + // Example from: + // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 + public static final int DRIVER_SWITCH_0 = 1; + public static final int DRIVER_SWITCH_1 = 2; + public static final int DRIVER_SWITCH_2 = 3; - /** Choreo Autonomous Action Constants *********************************** */ - public static final class AutoConstants { + public static final int OPERATOR_SWITCH_0 = 8; + public static final int OPERATOR_SWITCH_1 = 9; + public static final int OPERATOR_SWITCH_2 = 10; + public static final int OPERATOR_SWITCH_3 = 11; + public static final int OPERATOR_SWITCH_4 = 12; - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final int[] MULTI_TOGGLE = {4, 5}; + } - public static final double kPXController = 1; - public static final double kPYController = 1; - public static final double kPThetaController = 1; + /** Autonomous Action Constants ****************************************** */ + public static final class AutoConstants { - // Constraint for the motion profiled robot angle controller - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = - new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + // PathPlanner Translation PID constants + public static final PIDConstants kAutoDrivePID = new PIDConstants(0.7, 0, 0); + // PathPlanner Rotation PID constants + public static final PIDConstants kAutoSteerPID = new PIDConstants(0.4, 0, 0.01); } /** Vision Constants (Assuming PhotonVision) ***************************** */ @@ -230,6 +251,100 @@ public static class VisionConstants { public static final double kZMargin = 0.75; public static final double kXYZStdDevCoefficient = 0.005; public static final double kThetaStdDevCoefficient = 0.01; + + // Basic filtering thresholds + public static final double maxAmbiguity = 0.3; + public static final double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static final double linearStdDevBaseline = 0.02; // Meters + public static final double angularStdDevBaseline = 0.06; // Radians + + // Multipliers to apply for MegaTag 2 observations + public static final double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static final double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available + } + + /** Vision Camera Posses ************************************************* */ + public static class Cameras { + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + // ... And more, if needed + + // Robot to camera transforms + // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + } + + /** List of Device CAN and Power Distribution Circuit IDs **************** */ + public static class CANandPowerPorts { + + /* DRIVETRAIN CAN DEVICE IDS */ + // Input the correct Power Distribution Module port for each motor!!!! + // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) + + // Front Left + public static final RobotDeviceId FL_DRIVE = + new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18); + public static final RobotDeviceId FL_ROTATION = + new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19); + public static final RobotDeviceId FL_CANCODER = + new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null); + // Front Right + public static final RobotDeviceId FR_DRIVE = + new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17); + public static final RobotDeviceId FR_ROTATION = + new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16); + public static final RobotDeviceId FR_CANCODER = + new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null); + // Back Left + public static final RobotDeviceId BL_DRIVE = + new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1); + public static final RobotDeviceId BL_ROTATION = + new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0); + public static final RobotDeviceId BL_CANCODER = + new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null); + // Back Right + public static final RobotDeviceId BR_DRIVE = + new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2); + public static final RobotDeviceId BR_ROTATION = + new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3); + public static final RobotDeviceId BR_CANCODER = + new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null); + // Pigeon + public static final RobotDeviceId PIGEON = + new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kCANbusName, null); + + /* SUBSYSTEM CAN DEVICE IDS */ + // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) + // Example: + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); + + /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ + // This is where digital I/O feedback devices are defined + // Example: + // public static final int ELEVATOR_BOTTOM_LIMIT = 3; + + /* LINEAR SERVO PWM CHANNELS */ + // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) + // are defined + // Example: + // public static final int INTAKE_SERVO = 0; } /** AprilTag Field Layout ************************************************ */ @@ -238,10 +353,11 @@ public static class VisionConstants { public static class AprilTagConstants { public static final double aprilTagWidth = Units.inchesToMeters(6.50); + public static final String aprilTagFamily = "36h11"; public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; - public static final AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @Getter public enum AprilTagLayoutType { @@ -324,7 +440,12 @@ public static VisionType getVisionType() { } /** Get the current CTRE/Phoenix Pro License state */ - public static boolean getPhoenixPro() { + public static CTREPro getPhoenixPro() { return phoenixPro; } + + /** Get the current AprilTag layout type. */ + public static AprilTagLayoutType getAprilTagLayoutType() { + return AprilTagConstants.defaultAprilTagType; + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 750b769a..3526e0ab 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7389245a..f5bf2c26 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or @@ -13,18 +13,14 @@ package frc.robot; -import choreo.auto.AutoRoutine; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants.PowerConstants; -import frc.robot.RobotContainer.Ports; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; @@ -37,7 +33,6 @@ */ public class Robot extends LoggedRobot { private Command m_autoCommandPathPlanner; - private AutoRoutine m_autoCommandChoreo; private RobotContainer m_robotContainer; private Timer m_disabledTimer; @@ -74,8 +69,6 @@ public void robotInit() { // Running on a real robot, log to a USB stick ("/U/logs") Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); - LoggedPowerDistribution.getInstance( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); break; case SIM: @@ -150,6 +143,8 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + + // TODO: Make sure Gyro inits here with whatever is in the path planning thingie m_robotContainer.setMotorBrake(true); switch (Constants.getAutoType()) { case PATHPLANNER: @@ -160,11 +155,7 @@ public void autonomousInit() { } break; case CHOREO: - m_autoCommandChoreo = m_robotContainer.getAutonomousCommandChoreo(); - // schedule the autonomous command (example) - if (m_autoCommandChoreo != null) { - CommandScheduler.getInstance().schedule(m_autoCommandChoreo.cmd()); - } + m_robotContainer.getAutonomousCommandChoreo(); break; default: throw new RuntimeException( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a4047227..7cd9608a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -19,33 +19,26 @@ package frc.robot; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.Constants.Cameras.*; -import choreo.Choreo; import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; -import choreo.auto.AutoFactory.AutoBindings; import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.AprilTagConstants; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; -import frc.robot.Constants.PowerConstants; -import frc.robot.commands.ChoreoAutoController; +import frc.robot.Constants.OperatorConstants; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; @@ -59,38 +52,42 @@ import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; +import frc.robot.util.GetJoystickValue; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.PowerMonitoring; import frc.robot.util.RBSIEnum; -import frc.robot.util.RobotDeviceId; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { - // Define the Driver and, optionally, the Operator/Co-Driver Controllers + /** Define the Driver and, optionally, the Operator/Co-Driver Controllers */ // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - final CommandXboxController operatorXbox = new CommandXboxController(1); - final OverrideSwitches overrides = new OverrideSwitches(2); + final CommandXboxController driverController = new CommandXboxController(0); // Main Driver - // Autonomous Things - Field2d m_field = new Field2d(); + final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator + final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches - // Declare the robot subsystems here + /** Declare the robot subsystems here ************************************ */ + // These are the "Active Subsystems" that the robot controlls private final Drive m_drivebase; + private final Flywheel m_flywheel; + // These are "Virtual Subsystems" that report information but have no motors private final Accelerometer m_accel; private final Vision m_vision; private final PowerMonitoring m_power; - // Dashboard inputs + /** Dashboard inputs ***************************************************** */ // AutoChoosers for both supported path planning types private final LoggedDashboardChooser autoChooserPathPlanner; + private final AutoChooser autoChooserChoreo; private final AutoFactory autoFactoryChoreo; - private final ChoreoAutoController choreoController; + // Input estimated battery capacity (if full, use printed value) + private final LoggedTunableNumber batteryCapacity = + new LoggedTunableNumber("Battery Amp-Hours", 18.0); // EXAMPLE TUNABLE FLYWHEEL SPEED INPUT FROM DASHBOARD private final LoggedTunableNumber flywheelSpeedInput = new LoggedTunableNumber("Flywheel Speed", 1500.0); @@ -98,17 +95,10 @@ public class RobotContainer { // Alerts private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO); - /** Returns the current AprilTag layout type. */ - public AprilTagLayoutType getAprilTagLayoutType() { - return AprilTagConstants.defaultAprilTagType; - } - - public static AprilTagLayoutType staticGetAprilTagLayoutType() { - return AprilTagConstants.defaultAprilTagType; - } - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - @SuppressWarnings("unchecked") + /** + * Constructor for the Robot Container. This container holds subsystems, opertator interface + * devices, and commands. + */ public RobotContainer() { // Instantiate Robot Subsystems based on RobotType @@ -160,10 +150,10 @@ public RobotContainer() { break; } - // ``PowerMonitoring`` takes all the non-drivebase subsystems for which - // you wish to have power monitoring; DO NOT include ``m_drivebase``, - // as that is automatically monitored. - m_power = null; // new PowerMonitoring(m_flywheel); + // In addition to the initial battery capacity from the Dashbaord, ``PowerMonitoring`` takes all + // the non-drivebase subsystems for which you wish to have power monitoring; DO NOT include + // ``m_drivebase``, as that is automatically monitored. + m_power = new PowerMonitoring(batteryCapacity, m_flywheel); // Set up the SmartDashboard Auto Chooser based on auto type switch (Constants.getAutoType()) { @@ -173,21 +163,19 @@ public RobotContainer() { // Set the others to null autoChooserChoreo = null; autoFactoryChoreo = null; - choreoController = null; break; case CHOREO: - choreoController = new ChoreoAutoController(m_drivebase); autoFactoryChoreo = - Choreo.createAutoFactory( + new AutoFactory( m_drivebase::getPose, // A function that returns the current robot pose - choreoController, // The controller for the drive subsystem - this::isRedAlliance, // A function that returns true if the robot is on the red - // alliance - m_drivebase, - new AutoBindings() // An empty `AutoBindings` object, you can learn more below + m_drivebase::resetOdometry, // A function that resets the current robot pose to the + // provided Pose2d + m_drivebase::followTrajectory, // The drive subsystem trajectory follower + true, // If alliance flipping should be enabled + m_drivebase // The drive subsystem ); - autoChooserChoreo = new AutoChooser(autoFactoryChoreo, ""); - autoChooserChoreo.addAutoRoutine("twoPieceAuto", this::twoPieceAuto); + autoChooserChoreo = new AutoChooser(); + autoChooserChoreo.addRoutine("twoPieceAuto", this::twoPieceAuto); // Set the others to null autoChooserPathPlanner = null; break; @@ -211,49 +199,6 @@ private void defineAutoCommands() { NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); } - /** - * Set up the SysID routines from AdvantageKit - * - *

NOTE: These are currently only accessible with Constants.AutoType.PATHPLANNER - */ - private void definesysIdRoutines() { - if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { - // Drivebase characterization - autoChooserPathPlanner.addOption( - "Drive Wheel Radius Characterization", - DriveCommands.wheelRadiusCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( - "Drive Simple FF Characterization", - DriveCommands.feedforwardCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( - "Drive SysId (Quasistatic Forward)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Drive SysId (Quasistatic Reverse)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Drive SysId (Dynamic Forward)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Drive SysId (Dynamic Reverse)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - // Example Flywheel SysId Characterization - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Forward)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Reverse)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Forward)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Reverse)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - } - } - /** * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link GenericHID} or one of its subclasses ({@link @@ -262,40 +207,63 @@ private void definesysIdRoutines() { */ private void configureBindings() { + // Send the proper joystick input based on driver preference -- Set this in `Constants.java` + GetJoystickValue driveStickY; + GetJoystickValue driveStickX; + GetJoystickValue turnStickX; + if (OperatorConstants.kDriveLeftTurnRight) { + driveStickY = driverController::getLeftY; + driveStickX = driverController::getLeftX; + turnStickX = driverController::getRightX; + } else { + driveStickY = driverController::getRightY; + driveStickX = driverController::getRightX; + turnStickX = driverController::getLeftX; + } + // SET STANDARD DRIVING AS DEFAULT COMMAND FOR THE DRIVEBASE - // NOTE: Left joystick controls lateral translation, right joystick (X) controls rotation m_drivebase.setDefaultCommand( DriveCommands.fieldRelativeDrive( m_drivebase, - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX(), - () -> driverXbox.getRightX())); + () -> -driveStickY.value(), + () -> -driveStickX.value(), + () -> -turnStickX.value())); - // Example Commands + // ** Example Commands -- Remap, remove, or change as desired ** // Press B button while driving --> ROBOT-CENTRIC - driverXbox + driverController .b() .onTrue( Commands.runOnce( () -> DriveCommands.robotRelativeDrive( m_drivebase, - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX(), - () -> driverXbox.getRightX()), + () -> -driveStickY.value(), + () -> -driveStickX.value(), + () -> turnStickX.value()), m_drivebase)); // Press A button -> BRAKE - driverXbox.a().whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase)); + driverController + .a() + .whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase)); // Press X button --> Stop with wheels in X-Lock position - driverXbox.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); + driverController.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); // Press Y button --> Manually Re-Zero the Gyro - driverXbox.y().onTrue(Commands.runOnce(() -> m_drivebase.zero())); + driverController + .y() + .onTrue( + Commands.runOnce( + () -> + m_drivebase.setPose( + new Pose2d(m_drivebase.getPose().getTranslation(), new Rotation2d())), + m_drivebase) + .ignoringDisable(true)); // Press RIGHT BUMPER --> Run the example flywheel - driverXbox + driverController .rightBumper() .whileTrue( Commands.startEnd( @@ -319,8 +287,12 @@ public Command getAutonomousCommandPathPlanner() { * * @return the command to run in autonomous */ - public AutoRoutine getAutonomousCommandChoreo() { - return autoChooserChoreo.getSelectedAutoRoutine(); + public void getAutonomousCommandChoreo() { + // Put the auto chooser on the dashboard + SmartDashboard.putData(autoChooserChoreo); + + // Schedule the selected auto during the autonomous period + RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler()); } public void setDriveMode() { @@ -335,104 +307,57 @@ public void setMotorBrake(boolean brake) { /** Updates the alerts. */ public void updateAlerts() { // AprilTag layout alert - boolean aprilTagAlertActive = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; + boolean aprilTagAlertActive = Constants.getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; aprilTagLayoutAlert.set(aprilTagAlertActive); if (aprilTagAlertActive) { aprilTagLayoutAlert.setText( - "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ")."); + "Non-official AprilTag layout in use (" + + Constants.getAprilTagLayoutType().toString() + + ")."); } } - /** List of Device CAN and Power Distribution Circuit IDs **************** */ - public static class Ports { - - /* DRIVETRAIN CAN DEVICE IDS */ - // This is the default setup for the Az-RBSI swerve base - // Swerve Modules go: - // FL,FR,BL,BR - // - // 0 1 - // 2 3 - public static final RobotDeviceId FL_DRIVE = new RobotDeviceId(1, "DriveTrain", 18); - public static final RobotDeviceId FL_ROTATION = new RobotDeviceId(2, "DriveTrain", 19); - public static final RobotDeviceId FL_CANCODER = new RobotDeviceId(3, "DriveTrain", null); - - public static final RobotDeviceId FR_DRIVE = new RobotDeviceId(4, "DriveTrain", 17); - public static final RobotDeviceId FR_ROTATION = new RobotDeviceId(5, "DriveTrain", 16); - public static final RobotDeviceId FR_CANCODER = new RobotDeviceId(6, "DriveTrain", null); - - public static final RobotDeviceId BL_DRIVE = new RobotDeviceId(7, "DriveTrain", 1); - public static final RobotDeviceId BL_ROTATION = new RobotDeviceId(8, "DriveTrain", 0); - public static final RobotDeviceId BL_CANCODER = new RobotDeviceId(9, "DriveTrain", null); - - public static final RobotDeviceId BR_DRIVE = new RobotDeviceId(10, "DriveTrain", 2); - public static final RobotDeviceId BR_ROTATION = new RobotDeviceId(11, "DriveTrain", 3); - public static final RobotDeviceId BR_CANCODER = new RobotDeviceId(12, "DriveTrain", null); - - public static final RobotDeviceId PIGEON = new RobotDeviceId(13, "DriveTrain", null); - - /* POWER DISTRIBUTION CAN ID (set by device type in PowerConstants) */ - public static final RobotDeviceId POWER_CAN_DEVICE_ID = - switch (PowerConstants.kPowerModule) { - case kRev -> new RobotDeviceId(1, null); - case kCTRE -> new RobotDeviceId(0, null); - default -> null; - }; - - /* SUBSYSTEM CAN DEVICE IDS */ - // This is where mechanism subsystem devices are defined - // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, 8); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, 9); - - /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ - // This is where digital I/O feedback devices are defined - // Example: - // public static final int ELEVATOR_BOTTOM_LIMIT = 3; - - /* LINEAR SERVO PWM CHANNELS */ - // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) - // are defined - // Example: - // public static final int INTAKE_SERVO = 0; - } - - /** Override and Console Toggle Switches ********************************* */ - public static class Overrides { - - // Assumes this controller: https://www.amazon.com/gp/product/B00UUROWWK - // Example from: - // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 - public static final int DRIVER_SWITCH_0 = 1; - public static final int DRIVER_SWITCH_1 = 2; - public static final int DRIVER_SWITCH_2 = 3; - - public static final int OPERATOR_SWITCH_0 = 8; - public static final int OPERATOR_SWITCH_1 = 9; - public static final int OPERATOR_SWITCH_2 = 10; - public static final int OPERATOR_SWITCH_3 = 11; - public static final int OPERATOR_SWITCH_4 = 12; - - public static final int[] MULTI_TOGGLE = {4, 5}; - } + /** + * Set up the SysID routines from AdvantageKit + * + *

NOTE: These are currently only accessible with Constants.AutoType.PATHPLANNER + */ + private void definesysIdRoutines() { + if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { + // Drivebase characterization + autoChooserPathPlanner.addOption( + "Drive Wheel Radius Characterization", + DriveCommands.wheelRadiusCharacterization(m_drivebase)); + autoChooserPathPlanner.addOption( + "Drive Simple FF Characterization", + DriveCommands.feedforwardCharacterization(m_drivebase)); + autoChooserPathPlanner.addOption( + "Drive SysId (Quasistatic Forward)", + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Drive SysId (Quasistatic Reverse)", + m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooserPathPlanner.addOption( + "Drive SysId (Dynamic Forward)", + m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Drive SysId (Dynamic Reverse)", + m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - /** Vision Camera Posses ************************************************* */ - public static class Cameras { - - public static final Pose3d[] cameraPoses = - switch (Constants.getRobot()) { - case COMPBOT -> - new Pose3d[] { - // Camera #1 - new Pose3d( - Units.inchesToMeters(-1.0), - Units.inchesToMeters(0), - Units.inchesToMeters(23.5), - new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)), - }; - case DEVBOT -> new Pose3d[] {}; - default -> new Pose3d[] {}; - }; + // Example Flywheel SysId Characterization + autoChooserPathPlanner.addOption( + "Flywheel SysId (Quasistatic Forward)", + m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Flywheel SysId (Quasistatic Reverse)", + m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooserPathPlanner.addOption( + "Flywheel SysId (Dynamic Forward)", + m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooserPathPlanner.addOption( + "Flywheel SysId (Dynamic Reverse)", + m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } } /** @@ -440,33 +365,28 @@ public static class Cameras { * *

NOTE: This would normally be in a spearate file. */ - private AutoRoutine twoPieceAuto(AutoFactory factory) { - final AutoRoutine routine = factory.newRoutine("twoPieceAuto"); + private AutoRoutine twoPieceAuto() { + AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto"); - final AutoTrajectory trajectory = routine.trajectory("twoPieceAuto"); + // Load the routine's trajectories + AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece"); + AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece"); - routine - .running() - .onTrue( - m_drivebase - .resetOdometry( - trajectory - .getInitialPose() - .orElseGet( - () -> { - routine.kill(); - return new Pose2d(); - })) - .andThen(trajectory.cmd()) - .withName("twoPieceAuto entry point")); - - // trajectory.atTime("intake").onTrue(intake.extend()); - // trajectory.atTime("shoot").onTrue(shooter.launch()); + // When the routine begins, reset odometry and start the first trajectory + routine.active().onTrue(Commands.sequence(pickupTraj.resetOdometry(), pickupTraj.cmd())); - return routine; - } + // Starting at the event marker named "intake", run the intake + // pickupTraj.atTime("intake").onTrue(intakeSubsystem.intake()); + + // When the trajectory is done, start the next trajectory + pickupTraj.done().onTrue(scoreTraj.cmd()); - private boolean isRedAlliance() { - return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue).equals(Alliance.Red); + // While the trajectory is active, prepare the scoring subsystem + // scoreTraj.active().whileTrue(scoringSubsystem.getReady()); + + // When the trajectory is done, score + // scoreTraj.done().onTrue(scoringSubsystem.score()); + + return routine; } } diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 20f7900b..852cd6b6 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright 2024 SleipnirGroup // https://choreo.autos/ @@ -15,7 +15,7 @@ package frc.robot.commands; -import static frc.robot.Constants.AutonConstants.*; +import static frc.robot.Constants.AutoConstants.*; import choreo.trajectory.SwerveSample; import edu.wpi.first.math.controller.PIDController; @@ -27,11 +27,11 @@ public class ChoreoAutoController implements Consumer { private final Drive drive; // drive subsystem private final PIDController xController = - new PIDController(kAutoTranslatePID.kP, 0.0, kAutoTranslatePID.kD); + new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); private final PIDController yController = - new PIDController(kAutoTranslatePID.kP, 0.0, kAutoTranslatePID.kD); + new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); private final PIDController headingController = - new PIDController(kAutoAnglePID.kP, 0.0, kAutoAnglePID.kD); + new PIDController(kAutoSteerPID.kP, 0.0, kAutoSteerPID.kD); public ChoreoAutoController(Drive drive) { this.drive = drive; @@ -50,10 +50,10 @@ public void accept(SwerveSample referenceState) { double rotationFeedback = headingController.calculate(pose.getRotation().getRadians(), referenceState.heading); + // Convert to field relative speeds & send command ChassisSpeeds out = ChassisSpeeds.fromFieldRelativeSpeeds( xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation()); - drive.runVelocity(out); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 9521ac09..7909b911 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -30,8 +30,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.SwerveConstants; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -40,11 +42,9 @@ import java.util.function.Supplier; public class DriveCommands { - private static final double DEADBAND = 0.1; - private static final double ANGLE_KP = 5.0; - private static final double ANGLE_KD = 0.4; - private static final double ANGLE_MAX_VELOCITY = 8.0; - private static final double ANGLE_MAX_ACCELERATION = 20.0; + + // Needed for Characterization routines, not normal robot operations + // DO NOT ADJUST private static final double FF_START_DELAY = 2.0; // Secs private static final double FF_RAMP_RATE = 0.1; // Volts/Sec private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec @@ -63,18 +63,22 @@ public static Command fieldRelativeDrive( return Commands.run( () -> { // Get the Linear Velocity & Omega from inputs - Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); - double omega = getOmega(omegaSupplier); + Translation2d linearVelocity = + getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = getOmega(omegaSupplier.getAsDouble()); // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), + speeds, isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); @@ -92,105 +96,17 @@ public static Command robotRelativeDrive( DoubleSupplier omegaSupplier) { return Commands.run( () -> { - // Get the Linear Velocity & Omega from inputs - Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier); - double omega = getOmega(omegaSupplier); - - // Convert to robot relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromRobotRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); - }, - drive); - } - - /** - * Compute the new linear velocity from inputs, including applying deadbands and squaring for - * smoothness - */ - private static Translation2d getLinearVelocity( - DoubleSupplier xSupplier, DoubleSupplier ySupplier) { - - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), - OperatorConstants.kLeftDeadband); - Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - - return new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - } - - /** - * Compute the new angular velocity from inputs, including applying deadbands and squaring for - * smoothness - */ - private static double getOmega(DoubleSupplier omegaSupplier) { - double omega = - MathUtil.applyDeadband(omegaSupplier.getAsDouble(), OperatorConstants.kRightDeadband); - return Math.copySign(omega * omega, omega); - } - - private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { - // Apply deadband - double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), DEADBAND); - Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); - - // Square magnitude for more precise control - linearMagnitude = linearMagnitude * linearMagnitude; - - // Return new linear velocity - return new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - } - - /** - * Field relative drive command using two joysticks (controlling linear and angular velocities). - */ - public static Command joystickDrive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { - return Commands.run( - () -> { - // Get linear velocity Translation2d linearVelocity = - getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - - // Apply rotation deadband - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - - // Square rotation value for more precise control - omega = Math.copySign(omega * omega, omega); + getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = getOmega(omegaSupplier.getAsDouble()); - // Convert to field relative speeds & send command - ChassisSpeeds speeds = + // Run with straight-up velocities w.r.t. the robot! + drive.runVelocity( new ChassisSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - speeds.toRobotRelativeSpeeds( - isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation()); - drive.runVelocity(speeds); + omega * drive.getMaxAngularSpeedRadPerSec())); }, drive); } @@ -200,7 +116,7 @@ public static Command joystickDrive( * Possible use cases include snapping to an angle, aiming at a vision target, or controlling * absolute rotation with a joystick. */ - public static Command joystickDriveAtAngle( + public static Command fieldRelativeDriveAtAngle( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, @@ -209,10 +125,11 @@ public static Command joystickDriveAtAngle( // Create PID controller ProfiledPIDController angleController = new ProfiledPIDController( - ANGLE_KP, - 0.0, - ANGLE_KD, - new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + new TrapezoidProfile.Constraints( + DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); angleController.enableContinuousInput(-Math.PI, Math.PI); // Construct command @@ -220,7 +137,7 @@ public static Command joystickDriveAtAngle( () -> { // Get linear velocity Translation2d linearVelocity = - getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble()); // Calculate angular speed double omega = @@ -236,11 +153,12 @@ public static Command joystickDriveAtAngle( boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - speeds.toRobotRelativeSpeeds( - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation()); - drive.runVelocity(speeds); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive) @@ -248,6 +166,36 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Compute the new linear velocity from inputs, including applying deadbands and squaring for + * smoothness. Also apply the linear velocity Slew Rate Limiter. + */ + private static Translation2d getLinearVelocity(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kDeadband); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + // NOTE: The x & y values range from -1 to +1, so their squares are as well + linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + } + + /** + * Compute the new angular velocity from inputs, including applying deadbands and squaring for + * smoothness. Also apply the angular Slew Rate Limiter. + */ + private static double getOmega(double omega) { + omega = MathUtil.applyDeadband(omega, OperatorConstants.kDeadband); + return Math.copySign(omega * omega, omega); + } + + /***************************************************************************/ + /** DRIVEBASE CHARACTERIZATION COMMANDS ********************************** */ /** * Measures the velocity feedforward constants for the drive motors. * @@ -362,7 +310,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { for (int i = 0; i < 4; i++) { wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; } - double wheelRadius = (state.gyroDelta * Drive.DRIVE_BASE_RADIUS) / wheelDelta; + double wheelRadius = + (state.gyroDelta * SwerveConstants.kDriveBaseRadiusMeters) / wheelDelta; NumberFormat formatter = new DecimalFormat("#0.000"); System.out.println( diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java old mode 100644 new mode 100755 index d1f48432..4c4239af --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -3,17 +3,14 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; -import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; // import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -29,10 +26,10 @@ public class TunerConstants { new Slot0Configs() .withKP(100) .withKI(0) - .withKD(1.0) - .withKS(0.2) - .withKV(0.0) - .withKA(0.0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -46,6 +43,13 @@ public class TunerConstants { // This affects the PID/FF gains for the drive motors private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; @@ -54,7 +58,7 @@ public class TunerConstants { // This needs to be tuned to your individual robot private static final Current kSlipCurrent = Amps.of(120.0); - // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); private static final TalonFXConfiguration steerInitialConfigs = @@ -66,7 +70,7 @@ public class TunerConstants { // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; @@ -80,10 +84,10 @@ public class TunerConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; + private static final double kCoupleRatio = 3.5714285714285716; private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 15.42857142857143; + private static final double kSteerGearRatio = 21.428571428571427; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; @@ -92,11 +96,11 @@ public class TunerConstants { private static final int kPigeonId = 13; // These are only used for simulation - private static final double kSteerInertia = 0.004; - private static final double kDriveInertia = 0.025; + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.25); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.25); + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() @@ -104,34 +108,39 @@ public class TunerConstants { .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withCANcoderInitialConfigs(cancoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.472900390625); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125); private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftCANcoderInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; private static final Distance kFrontLeftXPos = Inches.of(10.375); private static final Distance kFrontLeftYPos = Inches.of(10.375); @@ -140,9 +149,9 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.11474609375); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875); private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightCANcoderInverted = false; + private static final boolean kFrontRightEncoderInverted = false; private static final Distance kFrontRightXPos = Inches.of(10.375); private static final Distance kFrontRightYPos = Inches.of(-10.375); @@ -151,9 +160,9 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.4091796875); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125); private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftCANcoderInverted = false; + private static final boolean kBackLeftEncoderInverted = false; private static final Distance kBackLeftXPos = Inches.of(-10.375); private static final Distance kBackLeftYPos = Inches.of(10.375); @@ -162,65 +171,146 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.086181640625); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125); private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightCANcoderInverted = false; + private static final boolean kBackRightEncoderInverted = false; private static final Distance kBackRightXPos = Inches.of(-10.375); private static final Distance kBackRightYPos = Inches.of(-10.375); - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftCANcoderInverted); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightCANcoderInverted); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftCANcoderInverted); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightCANcoderInverted); - - // /** - // * Creates a CommandSwerveDrivetrain instance. - // * This should only be called once in your robot program,. - // */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - // ); - // } + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + // /** + // * Creates a CommandSwerveDrivetrain instance. + // * This should only be called once in your robot program,. + // */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + // ); + // } + + /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]ᵀ, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, + * y, theta]ᵀ, with units in meters and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + } + } } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 91519de3..b63d4ba8 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or @@ -16,7 +16,7 @@ import static frc.robot.Constants.AccelerometerConstants.*; import com.ctre.phoenix6.hardware.Pigeon2; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.BuiltInAccelerometer; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 26700e39..7d468d9a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -16,10 +16,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import choreo.trajectory.SwerveSample; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.ModuleConfig; @@ -54,6 +53,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.PhysicalConstants; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import java.util.concurrent.locks.Lock; @@ -63,23 +63,15 @@ public class Drive extends SubsystemBase { - // TunerConstants doesn't include these constants, so they are declared locally - public static final double DRIVE_BASE_RADIUS = kDriveBaseRadiusMeters; - // Are we on the CANivore or not? - static final double ODOMETRY_FREQUENCY = new CANBus(kCANbusName).isNetworkFD() ? 250.0 : 100.0; - // PathPlanner config constants - private static final double ROBOT_MASS_KG = 74.088; - private static final double ROBOT_MOI = 6.883; - private static final double WHEEL_COF = 1.2; private static final RobotConfig PP_CONFIG = new RobotConfig( - ROBOT_MASS_KG, - ROBOT_MOI, + PhysicalConstants.kRobotMassKg, + PhysicalConstants.kRobotMOI, new ModuleConfig( kWheelRadiusMeters, - DrivebaseConstants.kMaxLinearSpeed.magnitude(), - WHEEL_COF, + DrivebaseConstants.kMaxLinearSpeed, + PhysicalConstants.kWheelCOF, DCMotor.getKrakenX60Foc(1).withReduction(kDriveGearRatio), kDriveSlipCurrent, 1), @@ -105,6 +97,9 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + // Choreo drive controller + private final PIDController xController = new PIDController(10.0, 0.0, 0.0); + // Constructor public Drive() { switch (Constants.getSwerveType()) { @@ -134,8 +129,12 @@ public Drive() { for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE - throw new RuntimeException( - "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); + if (kImuType == "navx" || kImuType == "navx_spi") { + modules[i] = new Module(new ModuleIOTalonFX(i), i); + } else { + throw new RuntimeException( + "For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!"); + } case 0b00010000: // Blended Talon Drive / NEO Steer modules[i] = new Module(new ModuleIOBlended(i), i); break; @@ -282,13 +281,13 @@ public void setMotorBrake(boolean brake) { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - speeds.discretize(Constants.loopPeriodSecs); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.loopPeriodSecs); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -411,21 +410,21 @@ public void addVisionMeasurement( /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return DrivebaseConstants.kMaxLinearSpeed.in(MetersPerSecond); + return DrivebaseConstants.kMaxLinearSpeed; } /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; + return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; } /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(kFrontLeftXPosMeters, kFrontLeftYPosMeters), - new Translation2d(kFrontRightXPosMeters, kFrontRightYPosMeters), - new Translation2d(kBackLeftXPosMeters, kBackLeftYPosMeters), - new Translation2d(kBackRightXPosMeters, kBackRightYPosMeters) + new Translation2d(kFLXPosMeters, kFLYPosMeters), + new Translation2d(kFRXPosMeters, kFRYPosMeters), + new Translation2d(kBLXPosMeters, kBLYPosMeters), + new Translation2d(kBRXPosMeters, kBRYPosMeters) }; } @@ -468,6 +467,21 @@ public void choreoController(Pose2d pose, SwerveSample sample) { // .withWheelForceFeedforwardsY(sample.moduleForcesY())); } + public void followTrajectory(SwerveSample sample) { + // Get the current pose of the robot + Pose2d pose = getPose(); + + // Generate the next speeds for the robot + ChassisSpeeds speeds = + new ChassisSpeeds( + sample.vx + xController.calculate(pose.getX(), sample.x), + sample.vy + xController.calculate(pose.getX(), sample.y), + sample.omega + xController.calculate(pose.getRotation().getRadians(), sample.heading)); + + // Apply the generated speeds + runVelocity(speeds); + } + /** * Parse the module type given the type information for the FL module * @@ -478,7 +492,7 @@ private Byte parseModuleType() { Byte b_drive; // [x,x,-,-,-,-,-,-] Byte b_steer; // [-,-,x,x,-,-,-,-] Byte b_encoder; // [-,-,-,-,x,x,-,-] - switch (kFrontLeftDriveType) { + switch (kFLDriveType) { case "falcon": case "kraken": case "talonfx": @@ -493,7 +507,7 @@ private Byte parseModuleType() { default: throw new RuntimeException("Invalid drive motor type"); } - switch (kFrontLeftSteerType) { + switch (kFLSteerType) { case "falcon": case "kraken": case "talonfx": @@ -508,7 +522,7 @@ private Byte parseModuleType() { default: throw new RuntimeException("Invalid steer motor type"); } - switch (kFrontLeftEncoderType) { + switch (kFLEncoderType) { case "cancoder": // CTRE CANcoder b_encoder = 0b00; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java deleted file mode 100644 index c7aa0929..00000000 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ /dev/null @@ -1,396 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.drive; - -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.RobotConfig; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants; -import frc.robot.generated.TunerConstants; -import frc.robot.util.YagslConstants; - -/** - * Holds the proper set of drive constants given the type of drive - * - *

Does some translation of how the two keep values to return a completely unified API. This file - * should not be modified. - */ -public class DriveConstants { - - // Declare all the constants - public static final String kImuType; - public static final double kCoupleRatio; - public static final double kDriveGearRatio; - public static final double kSteerGearRatio; - public static final double kWheelRadiusInches; - public static final String kCANbusName; - public static final int kPigeonId; - public static final double kSteerInertia; - public static final double kDriveInertia; - public static final double kSteerFrictionVoltage; - public static final double kDriveFrictionVoltage; - public static final double kSteerCurrentLimit; - public static final double kDriveCurrentLimit; - public static final double kDriveSlipCurrent; - public static final double kOptimalVoltage; - public static final int kFrontLeftDriveMotorId; - public static final int kFrontLeftSteerMotorId; - public static final int kFrontLeftEncoderId; - public static final String kFrontLeftDriveCanbus; - public static final String kFrontLeftSteerCanbus; - public static final String kFrontLeftEncoderCanbus; - public static final String kFrontLeftDriveType; - public static final String kFrontLeftSteerType; - public static final String kFrontLeftEncoderType; - public static final double kFrontLeftEncoderOffset; // In Radians - public static final boolean kFrontLeftDriveInvert; - public static final boolean kFrontLeftSteerInvert; - public static final boolean kFrontLeftEncoderInvert; - public static final double kFrontLeftXPosMeters; - public static final double kFrontLeftYPosMeters; - public static final int kFrontRightDriveMotorId; - public static final int kFrontRightSteerMotorId; - public static final int kFrontRightEncoderId; - public static final String kFrontRightDriveCanbus; - public static final String kFrontRightSteerCanbus; - public static final String kFrontRightEncoderCanbus; - public static final String kFrontRightDriveType; - public static final String kFrontRightSteerType; - public static final String kFrontRightEncoderType; - public static final double kFrontRightEncoderOffset; // In Radians - public static final boolean kFrontRightDriveInvert; - public static final boolean kFrontRightSteerInvert; - public static final boolean kFrontRightEncoderInvert; - public static final double kFrontRightXPosMeters; - public static final double kFrontRightYPosMeters; - public static final int kBackLeftDriveMotorId; - public static final int kBackLeftSteerMotorId; - public static final int kBackLeftEncoderId; - public static final String kBackLeftDriveCanbus; - public static final String kBackLeftSteerCanbus; - public static final String kBackLeftEncoderCanbus; - public static final String kBackLeftDriveType; - public static final String kBackLeftSteerType; - public static final String kBackLeftEncoderType; - public static final double kBackLeftEncoderOffset; // In Radians - public static final boolean kBackLeftDriveInvert; - public static final boolean kBackLeftSteerInvert; - public static final boolean kBackLeftEncoderInvert; - public static final double kBackLeftXPosMeters; - public static final double kBackLeftYPosMeters; - public static final int kBackRightDriveMotorId; - public static final int kBackRightSteerMotorId; - public static final int kBackRightEncoderId; - public static final String kBackRightDriveCanbus; - public static final String kBackRightSteerCanbus; - public static final String kBackRightEncoderCanbus; - public static final String kBackRightDriveType; - public static final String kBackRightSteerType; - public static final String kBackRightEncoderType; - public static final double kBackRightEncoderOffset; // In Radians - public static final boolean kBackRightDriveInvert; - public static final boolean kBackRightSteerInvert; - public static final boolean kBackRightEncoderInvert; - public static final double kBackRightXPosMeters; - public static final double kBackRightYPosMeters; - public static final double kDriveP; - public static final double kDriveI; - public static final double kDriveD; - public static final double kDriveF; - public static final double kDriveIZ; - public static final double kSteerP; - public static final double kSteerI; - public static final double kSteerD; - public static final double kSteerF; - public static final double kSteerIZ; - - // Fill in the values from the proper source - static { - switch (Constants.getSwerveType()) { - case PHOENIX6: - kImuType = "pigeon2"; - kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; - kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; - kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kWheelRadiusInches = TunerConstants.FrontLeft.WheelRadius; - kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; - kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; - kSteerInertia = TunerConstants.FrontLeft.SteerInertia; - kDriveInertia = TunerConstants.FrontLeft.DriveInertia; - kSteerFrictionVoltage = 0.0; - kDriveFrictionVoltage = 0.1; - kSteerCurrentLimit = 40.0; // Example from CTRE documentation - kDriveCurrentLimit = 120.0; // Example from CTRE documentation - kDriveSlipCurrent = TunerConstants.FrontLeft.SlipCurrent; - kOptimalVoltage = 12.0; // Assumed Ideal - kFrontLeftDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; - kFrontLeftSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; - kFrontLeftEncoderId = TunerConstants.FrontLeft.CANcoderId; - kFrontLeftDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontLeftSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontLeftEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontLeftDriveType = "kraken"; - kFrontLeftSteerType = "kraken"; - kFrontLeftEncoderType = "cancoder"; - kFrontLeftEncoderOffset = - -Units.rotationsToRadians(TunerConstants.FrontLeft.CANcoderOffset) + Math.PI; - kFrontLeftDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted; - kFrontLeftSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted; - kFrontLeftEncoderInvert = false; - kFrontLeftXPosMeters = TunerConstants.FrontLeft.LocationX; - kFrontLeftYPosMeters = TunerConstants.FrontLeft.LocationY; - kFrontRightDriveMotorId = TunerConstants.FrontRight.DriveMotorId; - kFrontRightSteerMotorId = TunerConstants.FrontRight.SteerMotorId; - kFrontRightEncoderId = TunerConstants.FrontRight.CANcoderId; - kFrontRightDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontRightSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontRightEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFrontRightDriveType = "kraken"; - kFrontRightSteerType = "kraken"; - kFrontRightEncoderType = "cancoder"; - kFrontRightEncoderOffset = - -Units.rotationsToRadians(TunerConstants.FrontRight.CANcoderOffset); - kFrontRightDriveInvert = TunerConstants.FrontRight.DriveMotorInverted; - kFrontRightSteerInvert = TunerConstants.FrontRight.SteerMotorInverted; - kFrontRightEncoderInvert = false; - kFrontRightXPosMeters = TunerConstants.FrontRight.LocationX; - kFrontRightYPosMeters = TunerConstants.FrontRight.LocationY; - kBackLeftDriveMotorId = TunerConstants.BackLeft.DriveMotorId; - kBackLeftSteerMotorId = TunerConstants.BackLeft.SteerMotorId; - kBackLeftEncoderId = TunerConstants.BackLeft.CANcoderId; - kBackLeftDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackLeftSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackLeftEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackLeftDriveType = "kraken"; - kBackLeftSteerType = "kraken"; - kBackLeftEncoderType = "cancoder"; - kBackLeftEncoderOffset = - -Units.rotationsToRadians(TunerConstants.BackLeft.CANcoderOffset) + Math.PI; - kBackLeftDriveInvert = TunerConstants.BackLeft.DriveMotorInverted; - kBackLeftSteerInvert = TunerConstants.BackLeft.SteerMotorInverted; - kBackLeftEncoderInvert = false; - kBackLeftXPosMeters = TunerConstants.BackLeft.LocationX; - kBackLeftYPosMeters = TunerConstants.BackLeft.LocationY; - kBackRightDriveMotorId = TunerConstants.BackRight.DriveMotorId; - kBackRightSteerMotorId = TunerConstants.BackRight.SteerMotorId; - kBackRightEncoderId = TunerConstants.BackRight.CANcoderId; - kBackRightDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackRightSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackRightEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBackRightDriveType = "kraken"; - kBackRightSteerType = "kraken"; - kBackRightEncoderType = "cancoder"; - kBackRightEncoderOffset = - -Units.rotationsToRadians(TunerConstants.BackRight.CANcoderOffset); - kBackRightDriveInvert = TunerConstants.BackRight.DriveMotorInverted; - kBackRightSteerInvert = TunerConstants.BackRight.SteerMotorInverted; - kBackRightEncoderInvert = false; - kBackRightXPosMeters = TunerConstants.BackRight.LocationX; - kBackRightYPosMeters = TunerConstants.BackRight.LocationY; - // NOTE: The PIDF values from TunerConstants.java make WPILib/AK implemention go crazy - // These values are from the AK example sketches - kDriveP = 0.05; - kDriveI = 0.0; - kDriveD = 0.0; - kDriveF = 0.13; - kDriveIZ = 0.0; - kSteerP = 7.0; - kSteerI = 0.0; - kSteerD = 0.0; - kSteerF = 0.0; - kSteerIZ = 0.0; - break; - - case YAGSL: - kImuType = YagslConstants.swerveDriveJson.imu.type; - kCoupleRatio = YagslConstants.kCoupleRatio; - kDriveGearRatio = YagslConstants.kDriveGearRatio; - kSteerGearRatio = YagslConstants.kSteerGearRatio; - kWheelRadiusInches = YagslConstants.kWheelRadiusInches; - kCANbusName = YagslConstants.kCANbusName; - kPigeonId = YagslConstants.kPigeonId; - kSteerInertia = YagslConstants.kSteerInertia; - kDriveInertia = YagslConstants.kDriveInertia; - kSteerFrictionVoltage = YagslConstants.kSteerFrictionVoltage; - kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage; - kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit; - kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; - kDriveSlipCurrent = 120.0; - kOptimalVoltage = YagslConstants.kOptimalVoltage; - kFrontLeftDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; - kFrontLeftSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; - kFrontLeftEncoderId = YagslConstants.kFrontLeftEncoderId; - kFrontLeftDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; - kFrontLeftSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; - kFrontLeftEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; - kFrontLeftDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase(); - kFrontLeftSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase(); - kFrontLeftEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase(); - kFrontLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset); - kFrontLeftDriveInvert = YagslConstants.kFrontLeftDriveInvert; - kFrontLeftSteerInvert = YagslConstants.kFrontLeftSteerInvert; - kFrontLeftEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; - kFrontLeftXPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftXPosInches); - kFrontLeftYPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftYPosInches); - kFrontRightDriveMotorId = YagslConstants.kFrontRightDriveMotorId; - kFrontRightSteerMotorId = YagslConstants.kFrontRightSteerMotorId; - kFrontRightEncoderId = YagslConstants.kFrontRightEncoderId; - kFrontRightDriveCanbus = YagslConstants.kFrontRightDriveCanbus; - kFrontRightSteerCanbus = YagslConstants.kFrontRightSteerCanbus; - kFrontRightEncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; - kFrontRightDriveType = YagslConstants.kFrontRightDriveType.toLowerCase(); - kFrontRightSteerType = YagslConstants.kFrontRightSteerType.toLowerCase(); - kFrontRightEncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase(); - kFrontRightEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset); - kFrontRightDriveInvert = YagslConstants.kFrontRightDriveInvert; - kFrontRightSteerInvert = YagslConstants.kFrontRightSteerInvert; - kFrontRightEncoderInvert = YagslConstants.kFrontRightEncoderInvert; - kFrontRightXPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightXPosInches); - kFrontRightYPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightYPosInches); - kBackLeftDriveMotorId = YagslConstants.kBackLeftDriveMotorId; - kBackLeftSteerMotorId = YagslConstants.kBackLeftSteerMotorId; - kBackLeftEncoderId = YagslConstants.kBackLeftEncoderId; - kBackLeftDriveCanbus = YagslConstants.kBackLeftDriveCanbus; - kBackLeftSteerCanbus = YagslConstants.kBackLeftSteerCanbus; - kBackLeftEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; - kBackLeftDriveType = YagslConstants.kBackLeftDriveType.toLowerCase(); - kBackLeftSteerType = YagslConstants.kBackLeftSteerType.toLowerCase(); - kBackLeftEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase(); - kBackLeftEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset); - kBackLeftDriveInvert = YagslConstants.kBackLeftDriveInvert; - kBackLeftSteerInvert = YagslConstants.kBackLeftSteerInvert; - kBackLeftEncoderInvert = YagslConstants.kBackLeftEncoderInvert; - kBackLeftXPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftXPosInches); - kBackLeftYPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftYPosInches); - kBackRightDriveMotorId = YagslConstants.kBackRightDriveMotorId; - kBackRightSteerMotorId = YagslConstants.kBackRightSteerMotorId; - kBackRightEncoderId = YagslConstants.kBackRightEncoderId; - kBackRightDriveCanbus = YagslConstants.kBackRightDriveCanbus; - kBackRightSteerCanbus = YagslConstants.kBackRightSteerCanbus; - kBackRightEncoderCanbus = YagslConstants.kBackRightEncoderCanbus; - kBackRightDriveType = YagslConstants.kBackRightDriveType.toLowerCase(); - kBackRightSteerType = YagslConstants.kBackRightSteerType.toLowerCase(); - kBackRightEncoderType = YagslConstants.kBackRightEncoderType.toLowerCase(); - kBackRightEncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset); - kBackRightDriveInvert = YagslConstants.kBackRightDriveInvert; - kBackRightSteerInvert = YagslConstants.kBackRightSteerInvert; - kBackRightEncoderInvert = YagslConstants.kBackRightEncoderInvert; - kBackRightXPosMeters = Units.inchesToMeters(YagslConstants.kBackRightXPosInches); - kBackRightYPosMeters = Units.inchesToMeters(YagslConstants.kBackRightYPosInches); - kDriveP = YagslConstants.kDriveP; - kDriveI = YagslConstants.kDriveI; - kDriveD = YagslConstants.kDriveD; - kDriveF = YagslConstants.kDriveF; - kDriveIZ = YagslConstants.kDriveIZ; - kSteerP = YagslConstants.kSteerP; - kSteerI = YagslConstants.kSteerI; - kSteerD = YagslConstants.kSteerD; - kSteerF = YagslConstants.kSteerF; - kSteerIZ = YagslConstants.kSteerIZ; - break; - - default: - throw new RuntimeException("Invalid Swerve Drive Type"); - } - } - - // Computed quantities - public static final double kDriveBaseRadiusMeters = - Math.max( - Math.max( - Math.hypot(kFrontLeftXPosMeters, kFrontLeftYPosMeters), - Math.hypot(kFrontRightXPosMeters, kFrontRightYPosMeters)), - Math.max( - Math.hypot(kBackLeftXPosMeters, kBackLeftYPosMeters), - Math.hypot(kBackRightXPosMeters, kBackRightYPosMeters))); - public static final double kDriveBaseRadiusInches = Units.metersToInches(kDriveBaseRadiusMeters); - public static final double kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); - - // Stuff to deal with from AK25's Spark Swerve Template - public static final double maxSpeedMetersPerSec = 4.8; - public static final double odometryFrequency = 100.0; // Hz - public static final double trackWidth = Units.inchesToMeters(26.5); - public static final double wheelBase = Units.inchesToMeters(26.5); - public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); - public static final Translation2d[] moduleTranslations = - new Translation2d[] { - new Translation2d(trackWidth / 2.0, wheelBase / 2.0), - new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), - new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), - new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) - }; - - // Drive motor configuration - public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.5); - public static final double driveMotorReduction = - (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth - public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); - - // Drive encoder configuration - public static final double driveEncoderPositionFactor = - 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians - public static final double driveEncoderVelocityFactor = - (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec - - // Drive PID configuration - public static final double driveKp = 0.0; - public static final double driveKd = 0.0; - public static final double driveKs = 0.0; - public static final double driveKv = 0.1; - public static final double driveSimP = 0.05; - public static final double driveSimD = 0.0; - public static final double driveSimKs = 0.0; - public static final double driveSimKv = 0.0789; - - // Turn motor configuration - public static final boolean turnInverted = false; - public static final int turnMotorCurrentLimit = 20; - public static final double turnMotorReduction = 9424.0 / 203.0; - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); - - // Turn encoder configuration - public static final boolean turnEncoderInverted = true; - public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians - public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec - - // Turn PID configuration - public static final double turnKp = 2.0; - public static final double turnKd = 0.0; - public static final double turnSimP = 8.0; - public static final double turnSimD = 0.0; - public static final double turnPIDMinInput = 0; // Radians - public static final double turnPIDMaxInput = 2 * Math.PI; // Radians - - // PathPlanner configuration - public static final double robotMassKg = 74.088; - public static final double robotMOI = 6.883; - public static final double wheelCOF = 1.2; - public static final RobotConfig ppConfig = - new RobotConfig( - robotMassKg, - robotMOI, - new ModuleConfig( - wheelRadiusMeters, - maxSpeedMetersPerSec, - wheelCOF, - driveGearbox.withReduction(driveMotorReduction), - driveMotorCurrentLimit, - 1), - moduleTranslations); -} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 89467c24..c8c4d16f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 7f8dcb76..6de5b723 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,17 +15,18 @@ package frc.robot.subsystems.drive; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.SPI; import java.util.Queue; /** IO implementation for Pigeon2 */ public class GyroIONavX implements GyroIO { - private final AHRS navx = new AHRS(SPI.Port.kMXP, (byte) Drive.ODOMETRY_FREQUENCY); + private final AHRS navx = + new AHRS(NavXComType.kMXP_SPI, (byte) SwerveConstants.kOdometryFrequency); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 79a5020d..a4ae3d30 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; @@ -42,7 +42,7 @@ public class GyroIOPigeon2 implements GyroIO { public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); + yaw.setUpdateFrequency(SwerveConstants.kOdometryFrequency); yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 97a485f2..c51680ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,7 +15,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 23b2f26b..bcd3bb3c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 87befca0..d7f6e9da 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2024 FRC 2486 +// Copyright 2024-2025 FRC 2486 // https://github.com/Coconuts2486-FRC // // This program is free software; you can redistribute it and/or @@ -15,11 +15,12 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -31,6 +32,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; @@ -50,6 +52,8 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.SparkUtil; import java.util.Queue; @@ -64,7 +68,9 @@ * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOBlended implements ModuleIO { - private final SwerveModuleConstants constants; + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; // CAN Devices private final TalonFX driveTalon; @@ -115,54 +121,54 @@ public ModuleIOBlended(int module) { switch (module) { case 0 -> ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPosMeters, - kFrontLeftYPosMeters, - kFrontLeftDriveInvert, - kFrontLeftSteerInvert, - kFrontLeftEncoderInvert); + kFLSteerMotorId, + kFLDriveMotorId, + kFLEncoderId, + kFLEncoderOffset, + kFLXPosMeters, + kFLYPosMeters, + kFLDriveInvert, + kFLSteerInvert, + kFLEncoderInvert); case 1 -> ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPosMeters, - kFrontRightYPosMeters, - kFrontRightDriveInvert, - kFrontRightSteerInvert, - kFrontRightEncoderInvert); + kFRSteerMotorId, + kFRDriveMotorId, + kFREncoderId, + kFREncoderOffset, + kFRXPosMeters, + kFRYPosMeters, + kFRDriveInvert, + kFRSteerInvert, + kFREncoderInvert); case 2 -> ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPosMeters, - kBackLeftYPosMeters, - kBackLeftDriveInvert, - kBackLeftSteerInvert, - kBackLeftEncoderInvert); + kBLSteerMotorId, + kBLDriveMotorId, + kBLEncoderId, + kBLEncoderOffset, + kBLXPosMeters, + kBLYPosMeters, + kBLDriveInvert, + kBLSteerInvert, + kBLEncoderInvert); case 3 -> ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPosMeters, - kBackRightYPosMeters, - kBackRightDriveInvert, - kBackRightSteerInvert, - kBackRightEncoderInvert); + kBRSteerMotorId, + kBRDriveMotorId, + kBREncoderId, + kBREncoderOffset, + kBRXPosMeters, + kBRYPosMeters, + kBRDriveInvert, + kBRSteerInvert, + kBREncoderInvert); default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, DriveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, SwerveConstants.kCANbusName); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); - cancoder = new CANcoder(constants.CANcoderId, DriveConstants.kCANbusName); + cancoder = new CANcoder(constants.EncoderId, SwerveConstants.kCANbusName); turnController = turnSpark.getClosedLoopController(); @@ -170,28 +176,26 @@ public ModuleIOBlended(int module) { var driveConfig = constants.DriveMotorInitialConfigs; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; - driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); // Configure turn motor var turnConfig = new SparkMaxConfig(); turnConfig - .inverted(turnInverted) + .inverted(constants.SteerMotorInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder - .inverted(turnEncoderInverted) + .inverted(constants.EncoderInverted) .positionConversionFactor(turnEncoderPositionFactor) .velocityConversionFactor(turnEncoderVelocityFactor) .averageDepth(2); @@ -200,11 +204,15 @@ public ModuleIOBlended(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); + .pidf( + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + 0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -218,12 +226,23 @@ public ModuleIOBlended(int module) { turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); // Configure CANCoder - CANcoderConfiguration cancoderConfig = constants.CANcoderInitialConfigs; - cancoderConfig.MagnetSensor.MagnetOffset = constants.CANcoderOffset; + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; cancoderConfig.MagnetSensor.SensorDirection = - constants.CANcoderInverted + constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; + + // Set motor Closed Loop Output type based on Phoenix Pro status + constants.DriveMotorClosedLoopOutput = + switch (Constants.getPhoenixPro()) { + case LICENSED -> ClosedLoopOutputType.TorqueCurrentFOC; + case UNLICENSED -> ClosedLoopOutputType.Voltage; + }; + + // Finally, apply the configs to the motor controllers + PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); cancoder.getConfigurator().apply(cancoderConfig); // Create timestamp queue @@ -244,7 +263,7 @@ public ModuleIOBlended(int module) { turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(cancoder.getPosition()); // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll(Drive.ODOMETRY_FREQUENCY, drivePosition); + BaseStatusSignal.setUpdateFrequencyForAll(SwerveConstants.kOdometryFrequency, drivePosition); BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, driveAppliedVolts, driveCurrent); ParentDevice.optimizeBusUtilizationForAll(driveTalon); } @@ -317,20 +336,23 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(Rotation2d.fromRotations(constants.CANcoderOffset)).getRadians(), + rotation.plus(Rotation2d.fromRotations(constants.EncoderOffset)).getRadians(), turnPIDMinInput, turnPIDMaxInput); turnController.setReference(setpoint, ControlType.kPosition); } - private SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadiusMeters) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + private SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadiusMeters) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 7d060c8b..52950e24 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,6 +15,8 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -53,7 +55,9 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim(SwerveModuleConstants constants) { + public ModuleIOSim( + SwerveModuleConstants + constants) { // Create drive and turn sim models driveSim = new DCMotorSim( diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 20c5be92..841ac551 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,11 +15,12 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import static frc.robot.util.SparkUtil.*; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -36,6 +37,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants.DrivebaseConstants; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -51,6 +53,8 @@ public class ModuleIOSpark implements ModuleIO { private final SparkBase turnSpark; private final RelativeEncoder driveEncoder; private final AbsoluteEncoder turnEncoder; + private final boolean turnInverted; + private final boolean turnEncoderInverted; // Closed loop controllers private final SparkClosedLoopController driveController; @@ -68,32 +72,48 @@ public class ModuleIOSpark implements ModuleIO { public ModuleIOSpark(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFrontLeftEncoderOffset); - case 1 -> new Rotation2d(kFrontRightEncoderOffset); - case 2 -> new Rotation2d(kBackLeftEncoderOffset); - case 3 -> new Rotation2d(kBackRightEncoderOffset); + case 0 -> new Rotation2d(kFLEncoderOffset); + case 1 -> new Rotation2d(kFREncoderOffset); + case 2 -> new Rotation2d(kBLEncoderOffset); + case 3 -> new Rotation2d(kBREncoderOffset); default -> new Rotation2d(); }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFrontLeftDriveMotorId; - case 1 -> kFrontRightDriveMotorId; - case 2 -> kBackLeftDriveMotorId; - case 3 -> kBackRightDriveMotorId; + case 0 -> kFLDriveMotorId; + case 1 -> kFRDriveMotorId; + case 2 -> kBLDriveMotorId; + case 3 -> kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFrontLeftSteerMotorId; - case 1 -> kFrontRightSteerMotorId; - case 2 -> kBackLeftSteerMotorId; - case 3 -> kBackRightSteerMotorId; + case 0 -> kFLSteerMotorId; + case 1 -> kFRSteerMotorId; + case 2 -> kBLSteerMotorId; + case 3 -> kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); + turnInverted = + switch (module) { + case 0 -> kFLSteerInvert; + case 1 -> kFRSteerInvert; + case 2 -> kBLSteerInvert; + case 3 -> kBRSteerInvert; + default -> false; + }; + turnEncoderInverted = + switch (module) { + case 0 -> kFLEncoderInvert; + case 1 -> kFREncoderInvert; + case 2 -> kBLEncoderInvert; + case 3 -> kBREncoderInvert; + default -> false; + }; driveEncoder = driveSpark.getEncoder(); turnEncoder = turnSpark.getAbsoluteEncoder(); driveController = driveSpark.getClosedLoopController(); @@ -103,7 +123,7 @@ public ModuleIOSpark(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit(driveMotorCurrentLimit) + .smartCurrentLimit((int) kDriveCurrentLimit) .voltageCompensation(12.0); driveConfig .encoder @@ -115,12 +135,14 @@ public ModuleIOSpark(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - driveKp, 0.0, - driveKd, 0.0); + DrivebaseConstants.drivePID.kP, + DrivebaseConstants.drivePID.kI, + DrivebaseConstants.drivePID.kD, + 0.0); driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -139,7 +161,7 @@ public ModuleIOSpark(int module) { turnConfig .inverted(turnInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -152,11 +174,15 @@ public ModuleIOSpark(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); + .pidf( + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + 0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -230,9 +256,15 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + double ffVolts = + SwerveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + + driveKv * velocityRadPerSec; driveController.setReference( - velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index ba9e1fb2..ce7744c3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,13 +15,14 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.subsystems.drive.SwerveConstants.*; import static frc.robot.util.SparkUtil.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -41,6 +42,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.DrivebaseConstants; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -56,6 +58,8 @@ public class ModuleIOSparkCANcoder implements ModuleIO { private final SparkBase turnSpark; private final RelativeEncoder driveEncoder; private final CANcoder cancoder; + private final boolean turnInverted; + private final boolean turnEncoderInverted; // Closed loop controllers private final SparkClosedLoopController driveController; @@ -79,40 +83,56 @@ public class ModuleIOSparkCANcoder implements ModuleIO { public ModuleIOSparkCANcoder(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFrontLeftEncoderOffset); - case 1 -> new Rotation2d(kFrontRightEncoderOffset); - case 2 -> new Rotation2d(kBackLeftEncoderOffset); - case 3 -> new Rotation2d(kBackRightEncoderOffset); + case 0 -> new Rotation2d(kFLEncoderOffset); + case 1 -> new Rotation2d(kFREncoderOffset); + case 2 -> new Rotation2d(kBLEncoderOffset); + case 3 -> new Rotation2d(kBREncoderOffset); default -> new Rotation2d(); }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFrontLeftDriveMotorId; - case 1 -> kFrontRightDriveMotorId; - case 2 -> kBackLeftDriveMotorId; - case 3 -> kBackRightDriveMotorId; + case 0 -> kFLDriveMotorId; + case 1 -> kFRDriveMotorId; + case 2 -> kBLDriveMotorId; + case 3 -> kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFrontLeftSteerMotorId; - case 1 -> kFrontRightSteerMotorId; - case 2 -> kBackLeftSteerMotorId; - case 3 -> kBackRightSteerMotorId; + case 0 -> kFLSteerMotorId; + case 1 -> kFRSteerMotorId; + case 2 -> kBLSteerMotorId; + case 3 -> kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); + turnInverted = + switch (module) { + case 0 -> kFLSteerInvert; + case 1 -> kFRSteerInvert; + case 2 -> kBLSteerInvert; + case 3 -> kBRSteerInvert; + default -> false; + }; + turnEncoderInverted = + switch (module) { + case 0 -> kFLEncoderInvert; + case 1 -> kFREncoderInvert; + case 2 -> kBLEncoderInvert; + case 3 -> kBREncoderInvert; + default -> false; + }; driveEncoder = driveSpark.getEncoder(); cancoder = new CANcoder( switch (module) { - case 0 -> kFrontLeftEncoderId; - case 1 -> kFrontRightEncoderId; - case 2 -> kBackLeftEncoderId; - case 3 -> kBackRightEncoderId; + case 0 -> kFLEncoderId; + case 1 -> kFREncoderId; + case 2 -> kBLEncoderId; + case 3 -> kBREncoderId; default -> 0; }, kCANbusName); @@ -123,7 +143,7 @@ public ModuleIOSparkCANcoder(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit(driveMotorCurrentLimit) + .smartCurrentLimit((int) kDriveCurrentLimit) .voltageCompensation(12.0); driveConfig .encoder @@ -135,12 +155,14 @@ public ModuleIOSparkCANcoder(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - driveKp, 0.0, - driveKd, 0.0); + DrivebaseConstants.drivePID.kP, + DrivebaseConstants.drivePID.kI, + DrivebaseConstants.drivePID.kD, + 0.0); driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -159,7 +181,7 @@ public ModuleIOSparkCANcoder(int module) { turnConfig .inverted(turnInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(turnMotorCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder @@ -172,11 +194,15 @@ public ModuleIOSparkCANcoder(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pidf(turnKp, 0.0, turnKd, 0.0); + .pidf( + DrivebaseConstants.steerPID.kP, + DrivebaseConstants.steerPID.kI, + DrivebaseConstants.steerPID.kD, + 0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -257,9 +283,15 @@ public void setTurnOpenLoop(double output) { @Override public void setDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveKs * Math.signum(velocityRadPerSec) + driveKv * velocityRadPerSec; + double ffVolts = + SwerveConstants.kDriveFrictionVoltage * Math.signum(velocityRadPerSec) + + driveKv * velocityRadPerSec; driveController.setReference( - velocityRadPerSec, ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage); + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index e6f82e31..20d5db3e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -35,6 +35,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -42,6 +43,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; import frc.robot.generated.TunerConstants; import java.util.Queue; @@ -52,7 +54,9 @@ *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. */ public class ModuleIOTalonFX implements ModuleIO { - private final SwerveModuleConstants constants; + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; // Hardware objects private final TalonFX driveTalon; @@ -107,57 +111,65 @@ public ModuleIOTalonFX(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, DriveConstants.kCANbusName); - turnTalon = new TalonFX(constants.SteerMotorId, DriveConstants.kCANbusName); - cancoder = new CANcoder(constants.CANcoderId, DriveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, SwerveConstants.kCANbusName); + turnTalon = new TalonFX(constants.SteerMotorId, SwerveConstants.kCANbusName); + cancoder = new CANcoder(constants.EncoderId, SwerveConstants.kCANbusName); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; - driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); // Configure turn motor var turnConfig = new TalonFXConfiguration(); turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; turnConfig.Slot0 = constants.SteerMotorGains; - turnConfig.Feedback.FeedbackRemoteSensorID = constants.CANcoderId; - turnConfig.Feedback.FeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; - case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; - case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; - }; - turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.Feedback.RotorToSensorRatio = SwerveConstants.kSteerGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / SwerveConstants.kSteerGearRatio; turnConfig.MotionMagic.MotionMagicAcceleration = turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; - turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * SwerveConstants.kSteerGearRatio; turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; turnConfig.ClosedLoopGeneral.ContinuousWrap = true; turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); // Configure CANCoder - CANcoderConfiguration cancoderConfig = constants.CANcoderInitialConfigs; - cancoderConfig.MagnetSensor.MagnetOffset = constants.CANcoderOffset; + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; cancoderConfig.MagnetSensor.SensorDirection = - constants.CANcoderInverted + constants.EncoderInverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; + + // Set motor Closed Loop Output type and CANCoder feedback type based on Phoenix Pro status + switch (Constants.getPhoenixPro()) { + case LICENSED: + turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + constants.DriveMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; + constants.SteerMotorClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; + case UNLICENSED: + turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + constants.DriveMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; + constants.SteerMotorClosedLoopOutput = ClosedLoopOutputType.Voltage; + } + + // Finally, apply the configs to the motor controllers + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); cancoder.getConfigurator().apply(cancoderConfig); // Create timestamp queue @@ -181,7 +193,7 @@ public ModuleIOTalonFX(int module) { // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( - Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + SwerveConstants.kOdometryFrequency, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index a42b4964..3b3a2e56 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -121,12 +121,12 @@ public void run() { signalsLock.lock(); try { if (isCANFD && phoenixSignals.length > 0) { - BaseStatusSignal.waitForAll(2.0 / Drive.ODOMETRY_FREQUENCY, phoenixSignals); + BaseStatusSignal.waitForAll(2.0 / SwerveConstants.kOdometryFrequency, phoenixSignals); } else { // "waitForAll" does not support blocking on multiple signals with a bus // that is not CAN FD, regardless of Pro licensing. No reasoning for this // behavior is provided by the documentation. - Thread.sleep((long) (1000.0 / Drive.ODOMETRY_FREQUENCY)); + Thread.sleep((long) (1000.0 / SwerveConstants.kOdometryFrequency)); if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); } } catch (InterruptedException e) { diff --git a/src/main/java/frc/robot/subsystems/drive/README b/src/main/java/frc/robot/subsystems/drive/README index 1e84dc77..2b457143 100644 --- a/src/main/java/frc/robot/subsystems/drive/README +++ b/src/main/java/frc/robot/subsystems/drive/README @@ -12,10 +12,10 @@ Existing IO files: - ``GyroIONavX.java``: NavX as the swerve IMU, connected via SPI - ``ModuleIOTalonFX.java``: 2x TalonFX motors + CANcoder - - ``ModuleIOSparkMax.java``: 2x Rev NEO motors + analog absolute encoder connected to the RIO + - ``ModuleIOSpark.java``: 2x Rev NEO motors + analog absolute encoder connected to the RIO - ``ModuleIOSparkCANcoder.java``: 2x Rv NEO motors + CANcoder - ``ModuleIOBlended.java``: TalonFX drive, Rev NEO steer motors + CANcoder -------------------- -Last Modified: 22 Nov 2024, TPEB +Last Modified: 2 Jan 2025, TPEB diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index 1f6e63e0..8ccb2edc 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -55,7 +55,7 @@ private SparkOdometryThread() { public void start() { if (timestampQueues.size() > 0) { - notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); + notifier.startPeriodic(1.0 / SwerveConstants.kOdometryFrequency); } } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java new file mode 100644 index 00000000..d5119272 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -0,0 +1,309 @@ +// Copyright (c) 2024-2025 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.generated.TunerConstants; +import frc.robot.util.YagslConstants; + +/** + * Holds the proper set of drive constants given the type of drive + * + *

Does some translation of how the two keep values to return a completely unified API. This file + * should not be modified. + */ +public class SwerveConstants { + + // Declare all the constants + public static final String kImuType; + public static final double kCoupleRatio; + public static final double kDriveGearRatio; + public static final double kSteerGearRatio; + public static final double kWheelRadiusInches; + public static final String kCANbusName; + public static final int kPigeonId; + public static final double kSteerInertia; + public static final double kDriveInertia; + public static final double kSteerFrictionVoltage; + public static final double kDriveFrictionVoltage; + public static final double kSteerCurrentLimit; + public static final double kDriveCurrentLimit; + public static final double kDriveSlipCurrent; + public static final double kOptimalVoltage; + public static final int kFLDriveMotorId; + public static final int kFLSteerMotorId; + public static final int kFLEncoderId; + public static final String kFLDriveCanbus; + public static final String kFLSteerCanbus; + public static final String kFLEncoderCanbus; + public static final String kFLDriveType; + public static final String kFLSteerType; + public static final String kFLEncoderType; + public static final double kFLEncoderOffset; // In Radians + public static final boolean kFLDriveInvert; + public static final boolean kFLSteerInvert; + public static final boolean kFLEncoderInvert; + public static final double kFLXPosMeters; + public static final double kFLYPosMeters; + public static final int kFRDriveMotorId; + public static final int kFRSteerMotorId; + public static final int kFREncoderId; + public static final String kFRDriveCanbus; + public static final String kFRSteerCanbus; + public static final String kFREncoderCanbus; + public static final String kFRDriveType; + public static final String kFRSteerType; + public static final String kFREncoderType; + public static final double kFREncoderOffset; // In Radians + public static final boolean kFRDriveInvert; + public static final boolean kFRSteerInvert; + public static final boolean kFREncoderInvert; + public static final double kFRXPosMeters; + public static final double kFRYPosMeters; + public static final int kBLDriveMotorId; + public static final int kBLSteerMotorId; + public static final int kBLEncoderId; + public static final String kBLDriveCanbus; + public static final String kBLSteerCanbus; + public static final String kBLEncoderCanbus; + public static final String kBLDriveType; + public static final String kBLSteerType; + public static final String kBLEncoderType; + public static final double kBLEncoderOffset; // In Radians + public static final boolean kBLDriveInvert; + public static final boolean kBLSteerInvert; + public static final boolean kBLEncoderInvert; + public static final double kBLXPosMeters; + public static final double kBLYPosMeters; + public static final int kBRDriveMotorId; + public static final int kBRSteerMotorId; + public static final int kBREncoderId; + public static final String kBRDriveCanbus; + public static final String kBRSteerCanbus; + public static final String kBREncoderCanbus; + public static final String kBRDriveType; + public static final String kBRSteerType; + public static final String kBREncoderType; + public static final double kBREncoderOffset; // In Radians + public static final boolean kBRDriveInvert; + public static final boolean kBRSteerInvert; + public static final boolean kBREncoderInvert; + public static final double kBRXPosMeters; + public static final double kBRYPosMeters; + + // Fill in the values from the proper source + static { + switch (Constants.getSwerveType()) { + case PHOENIX6: + kImuType = "pigeon2"; + kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; + kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; + kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; + kWheelRadiusInches = TunerConstants.FrontLeft.WheelRadius; + kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; + kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; + kSteerInertia = TunerConstants.FrontLeft.SteerInertia; + kDriveInertia = TunerConstants.FrontLeft.DriveInertia; + kSteerFrictionVoltage = 0.0; + kDriveFrictionVoltage = 0.1; + kSteerCurrentLimit = 40.0; // Example from CTRE documentation + kDriveCurrentLimit = 120.0; // Example from CTRE documentation + kDriveSlipCurrent = TunerConstants.FrontLeft.SlipCurrent; + kOptimalVoltage = 12.0; // Assumed Ideal + // Front Left + kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; + kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; + kFLEncoderId = TunerConstants.FrontLeft.EncoderId; + kFLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLDriveType = "kraken"; + kFLSteerType = "kraken"; + kFLEncoderType = "cancoder"; + kFLEncoderOffset = + -Units.rotationsToRadians(TunerConstants.FrontLeft.EncoderOffset) + Math.PI; + kFLDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted; + kFLSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted; + kFLEncoderInvert = TunerConstants.FrontLeft.EncoderInverted; + kFLXPosMeters = TunerConstants.FrontLeft.LocationX; + kFLYPosMeters = TunerConstants.FrontLeft.LocationY; + // Front Right + kFRDriveMotorId = TunerConstants.FrontRight.DriveMotorId; + kFRSteerMotorId = TunerConstants.FrontRight.SteerMotorId; + kFREncoderId = TunerConstants.FrontRight.EncoderId; + kFRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFRDriveType = "kraken"; + kFRSteerType = "kraken"; + kFREncoderType = "cancoder"; + kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.EncoderOffset); + kFRDriveInvert = TunerConstants.FrontRight.DriveMotorInverted; + kFRSteerInvert = TunerConstants.FrontRight.SteerMotorInverted; + kFREncoderInvert = TunerConstants.FrontRight.EncoderInverted; + kFRXPosMeters = TunerConstants.FrontRight.LocationX; + kFRYPosMeters = TunerConstants.FrontRight.LocationY; + // Back Left + kBLDriveMotorId = TunerConstants.BackLeft.DriveMotorId; + kBLSteerMotorId = TunerConstants.BackLeft.SteerMotorId; + kBLEncoderId = TunerConstants.BackLeft.EncoderId; + kBLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLDriveType = "kraken"; + kBLSteerType = "kraken"; + kBLEncoderType = "cancoder"; + kBLEncoderOffset = + -Units.rotationsToRadians(TunerConstants.BackLeft.EncoderOffset) + Math.PI; + kBLDriveInvert = TunerConstants.BackLeft.DriveMotorInverted; + kBLSteerInvert = TunerConstants.BackLeft.SteerMotorInverted; + kBLEncoderInvert = TunerConstants.BackLeft.EncoderInverted; + kBLXPosMeters = TunerConstants.BackLeft.LocationX; + kBLYPosMeters = TunerConstants.BackLeft.LocationY; + // Back Right + kBRDriveMotorId = TunerConstants.BackRight.DriveMotorId; + kBRSteerMotorId = TunerConstants.BackRight.SteerMotorId; + kBREncoderId = TunerConstants.BackRight.EncoderId; + kBRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBRDriveType = "kraken"; + kBRSteerType = "kraken"; + kBREncoderType = "cancoder"; + kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.EncoderOffset); + kBRDriveInvert = TunerConstants.BackRight.DriveMotorInverted; + kBRSteerInvert = TunerConstants.BackRight.SteerMotorInverted; + kBREncoderInvert = TunerConstants.BackRight.EncoderInverted; + kBRXPosMeters = TunerConstants.BackRight.LocationX; + kBRYPosMeters = TunerConstants.BackRight.LocationY; + break; + + case YAGSL: + kImuType = YagslConstants.swerveDriveJson.imu.type; + kCoupleRatio = YagslConstants.kCoupleRatio; + kDriveGearRatio = YagslConstants.kDriveGearRatio; + kSteerGearRatio = YagslConstants.kSteerGearRatio; + kWheelRadiusInches = YagslConstants.kWheelRadiusInches; + kCANbusName = YagslConstants.kCANbusName; + kPigeonId = YagslConstants.kPigeonId; + kSteerInertia = YagslConstants.kSteerInertia; + kDriveInertia = YagslConstants.kDriveInertia; + kSteerFrictionVoltage = YagslConstants.kSteerFrictionVoltage; + kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage; + kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit; + kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; + kDriveSlipCurrent = 120.0; + kOptimalVoltage = YagslConstants.kOptimalVoltage; + // Front Left + kFLDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; + kFLSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; + kFLEncoderId = YagslConstants.kFrontLeftEncoderId; + kFLDriveCanbus = YagslConstants.kFrontLeftDriveCanbus; + kFLSteerCanbus = YagslConstants.kFrontLeftSteerCanbus; + kFLEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus; + kFLDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase(); + kFLSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase(); + kFLEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase(); + kFLEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset); + kFLDriveInvert = YagslConstants.kFrontLeftDriveInvert; + kFLSteerInvert = YagslConstants.kFrontLeftSteerInvert; + kFLEncoderInvert = YagslConstants.kFrontLeftEncoderInvert; + kFLXPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftXPosInches); + kFLYPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftYPosInches); + // Front Right + kFRDriveMotorId = YagslConstants.kFrontRightDriveMotorId; + kFRSteerMotorId = YagslConstants.kFrontRightSteerMotorId; + kFREncoderId = YagslConstants.kFrontRightEncoderId; + kFRDriveCanbus = YagslConstants.kFrontRightDriveCanbus; + kFRSteerCanbus = YagslConstants.kFrontRightSteerCanbus; + kFREncoderCanbus = YagslConstants.kFrontRightEncoderCanbus; + kFRDriveType = YagslConstants.kFrontRightDriveType.toLowerCase(); + kFRSteerType = YagslConstants.kFrontRightSteerType.toLowerCase(); + kFREncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase(); + kFREncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset); + kFRDriveInvert = YagslConstants.kFrontRightDriveInvert; + kFRSteerInvert = YagslConstants.kFrontRightSteerInvert; + kFREncoderInvert = YagslConstants.kFrontRightEncoderInvert; + kFRXPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightXPosInches); + kFRYPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightYPosInches); + // Back Left + kBLDriveMotorId = YagslConstants.kBackLeftDriveMotorId; + kBLSteerMotorId = YagslConstants.kBackLeftSteerMotorId; + kBLEncoderId = YagslConstants.kBackLeftEncoderId; + kBLDriveCanbus = YagslConstants.kBackLeftDriveCanbus; + kBLSteerCanbus = YagslConstants.kBackLeftSteerCanbus; + kBLEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus; + kBLDriveType = YagslConstants.kBackLeftDriveType.toLowerCase(); + kBLSteerType = YagslConstants.kBackLeftSteerType.toLowerCase(); + kBLEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase(); + kBLEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset); + kBLDriveInvert = YagslConstants.kBackLeftDriveInvert; + kBLSteerInvert = YagslConstants.kBackLeftSteerInvert; + kBLEncoderInvert = YagslConstants.kBackLeftEncoderInvert; + kBLXPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftXPosInches); + kBLYPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftYPosInches); + // Back Right + kBRDriveMotorId = YagslConstants.kBackRightDriveMotorId; + kBRSteerMotorId = YagslConstants.kBackRightSteerMotorId; + kBREncoderId = YagslConstants.kBackRightEncoderId; + kBRDriveCanbus = YagslConstants.kBackRightDriveCanbus; + kBRSteerCanbus = YagslConstants.kBackRightSteerCanbus; + kBREncoderCanbus = YagslConstants.kBackRightEncoderCanbus; + kBRDriveType = YagslConstants.kBackRightDriveType.toLowerCase(); + kBRSteerType = YagslConstants.kBackRightSteerType.toLowerCase(); + kBREncoderType = YagslConstants.kBackRightEncoderType.toLowerCase(); + kBREncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset); + kBRDriveInvert = YagslConstants.kBackRightDriveInvert; + kBRSteerInvert = YagslConstants.kBackRightSteerInvert; + kBREncoderInvert = YagslConstants.kBackRightEncoderInvert; + kBRXPosMeters = Units.inchesToMeters(YagslConstants.kBackRightXPosInches); + kBRYPosMeters = Units.inchesToMeters(YagslConstants.kBackRightYPosInches); + break; + + default: + throw new RuntimeException("Invalid Swerve Drive Type"); + } + } + + // Computed quantities + public static final double kDriveBaseRadiusMeters = + Math.max( + Math.max( + Math.hypot(kFLXPosMeters, kFLYPosMeters), Math.hypot(kFRXPosMeters, kFRYPosMeters)), + Math.max( + Math.hypot(kBLXPosMeters, kBLYPosMeters), Math.hypot(kBRXPosMeters, kBRYPosMeters))); + public static final double kDriveBaseRadiusInches = Units.metersToInches(kDriveBaseRadiusMeters); + public static final double kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); + + // Are we on the CANivore or not? + public static final double kOdometryFrequency = + new CANBus(kCANbusName).isNetworkFD() ? 250.0 : 100.0; + + // SPARK Drive encoder configuration + // Rotor Rotations -> Wheel Radians + public static final double driveEncoderPositionFactor = 2 * Math.PI / kDriveGearRatio; + // Rotor RPM -> Wheel Rad/Sec + public static final double driveEncoderVelocityFactor = (2 * Math.PI) / 60.0 / kDriveGearRatio; + // Rotations -> Radians + public static final double turnEncoderPositionFactor = 2 * Math.PI; + // RPM -> Rad/Sec + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; + // Drive PID configuration + public static final double driveKv = 0.1; + // Turn PID configuration0 + public static final double turnPIDMinInput = 0; // Radians + public static final double turnPIDMaxInput = 2 * Math.PI; // Radians +} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index 6db39c5c..53e3b0d3 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -43,11 +43,11 @@ public Flywheel(FlywheelIO io) { case REAL: case REPLAY: ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); - io.configurePID(kPReal, kIReal, kDReal); + io.configurePID(pidReal.kP, pidReal.kI, pidReal.kD); break; case SIM: ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); - io.configurePID(kPSim, kISim, kDSim); + io.configurePID(pidSim.kP, pidSim.kI, pidSim.kD); break; default: ffModel = new SimpleMotorFeedforward(0.0, 0.0); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java index 8c7d0139..5ffbf398 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index fe3046d4..a4a82cd8 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -29,7 +29,7 @@ public class FlywheelIOSim implements FlywheelIO { // spins slower than the motors, this number should be greater than one. private static final double kFlywheelGearing = 1.0; - // 1/2 MR² + // 1/2 MR^2 private static final double kFlywheelMomentOfInertia = 0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index 725413de..871a9dea 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -19,6 +19,7 @@ import static frc.robot.util.SparkUtil.*; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -31,8 +32,8 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.util.Units; -import frc.robot.RobotContainer.Ports; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.Constants.CANandPowerPorts; +import frc.robot.subsystems.drive.SwerveConstants; /** * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with @@ -42,14 +43,15 @@ public class FlywheelIOSpark implements FlywheelIO { // Define the leader / follower motors from the Ports section of RobotContainer private final SparkBase leader = - new SparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + new SparkMax(CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final SparkBase follower = - new SparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); + new SparkMax(CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); private final RelativeEncoder encoder = leader.getEncoder(); private final SparkClosedLoopController pid = leader.getClosedLoopController(); // IMPORTANT: Include here all devices listed above that are part of this mechanism! public final int[] powerPorts = { - Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + CANandPowerPorts.FLYWHEEL_LEADER.getPowerPort(), + CANandPowerPorts.FLYWHEEL_FOLLOWER.getPowerPort() }; public FlywheelIOSpark() { @@ -57,8 +59,12 @@ public FlywheelIOSpark() { // Configure leader motor var leaderConfig = new SparkFlexConfig(); leaderConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(DriveConstants.driveMotorCurrentLimit) + .idleMode( + switch (kFlywheelIdleMode) { + case COAST -> IdleMode.kCoast; + case BRAKE -> IdleMode.kBrake; + }) + .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(12.0); leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2); leaderConfig @@ -70,7 +76,7 @@ public FlywheelIOSpark() { leaderConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / DriveConstants.odometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -104,7 +110,7 @@ public void setVelocity(double velocityRadPerSec, double ffVolts) { pid.setReference( Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio, ControlType.kVelocity, - 0, + ClosedLoopSlot.kSlot0, ffVolts, ArbFFUnits.kVoltage); } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 66c645ab..9364d321 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -31,18 +31,23 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.RobotContainer.Ports; +import frc.robot.Constants.CANandPowerPorts; public class FlywheelIOTalonFX implements FlywheelIO { // Define the leader / follower motors from the Ports section of RobotContainer private final TalonFX leader = - new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus()); + new TalonFX( + CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), + CANandPowerPorts.FLYWHEEL_LEADER.getBus()); private final TalonFX follower = - new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus()); + new TalonFX( + CANandPowerPorts.FLYWHEEL_FOLLOWER.getDeviceNumber(), + CANandPowerPorts.FLYWHEEL_FOLLOWER.getBus()); // IMPORTANT: Include here all devices listed above that are part of this mechanism! public final int[] powerPorts = { - Ports.FLYWHEEL_LEADER.getPowerPort(), Ports.FLYWHEEL_FOLLOWER.getPowerPort() + CANandPowerPorts.FLYWHEEL_LEADER.getPowerPort(), + CANandPowerPorts.FLYWHEEL_FOLLOWER.getPowerPort() }; private final StatusSignal leaderPosition = leader.getPosition(); @@ -55,9 +60,14 @@ public FlywheelIOTalonFX() { var config = new TalonFXConfiguration(); config.CurrentLimits.SupplyCurrentLimit = 30.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = + switch (kFlywheelIdleMode) { + case COAST -> NeutralModeValue.Coast; + case BRAKE -> NeutralModeValue.Brake; + }; leader.getConfigurator().apply(config); follower.getConfigurator().apply(config); + // If follower rotates in the opposite direction, set "OpposeMasterDirection" to true follower.setControl(new Follower(leader.getDeviceID(), false)); BaseStatusSignal.setUpdateFrequencyForAll( diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/README b/src/main/java/frc/robot/subsystems/flywheel_example/README index 7cad088f..b796c05d 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/README +++ b/src/main/java/frc/robot/subsystems/flywheel_example/README @@ -10,7 +10,7 @@ with AdvantageKit. The structure of the subsystem directory is: * FlywheelIO.java The base subsystem I/O (input/output) interface that contains the - structure and class varibales needed by the library-specific modules. + structure and class variables needed by the library-specific modules. * FlywheelIOSim.java Simulated flywheel module, for use with robot simulations, does not diff --git a/src/main/java/frc/robot/subsystems/vision/README b/src/main/java/frc/robot/subsystems/vision/README new file mode 100644 index 00000000..a8c8ad27 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/README @@ -0,0 +1,29 @@ +This directory contains an the basic vision template, as distributed with +AdvantageKit. The structure of the subsystem directory is: + + * Vision.java + The base subsystem definitions and outward-facing (public) hardware- + agnostic functions for interacting with the subsystem. The idea is + that any of the hardware-specific (i.e. camera system API calls) + functionality is insulated from the rest of the robot code within + the library-specific modules in this directory. + + * VisionIO.java + The base subsystem I/O (input/output) interface that contains the + structure and class variables needed by the library-specific modules. + + * VisionIOLimelight.java + The vision implementation for the Limelight camera system, which maps + Limelight-specific API calls back onto the generic vision API defined + above. + + * VisionIOPhotonVision.java + The vision implementation for the Photon Vision camera system, which + maps PhotonLib-specific API calls back onto the generic vision API + defined above. + + * VisionIOPhotonVisionSim.java + Simulated vision module, for use with robot simulations, does not + control actual hardware. + + \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 3038741e..34c676a2 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,8 +1,8 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 FRC 2486 +// Copyright (c) 2024-2025 FRC 2486 // http://github.com/Coconuts2486-FRC -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -17,7 +17,8 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.Constants.Cameras.*; +import static frc.robot.Constants.VisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -29,6 +30,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.AprilTagConstants; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; @@ -94,7 +96,7 @@ public void periodic() { // Add tag poses for (int tagId : inputs[cameraIndex].tagIds) { - var tagPose = aprilTagLayout.getTagPose(tagId); + var tagPose = AprilTagConstants.aprilTagLayout.getTagPose(tagId); if (tagPose.isPresent()) { tagPoses.add(tagPose.get()); } @@ -112,9 +114,9 @@ public void periodic() { // Must be within the field boundaries || observation.pose().getX() < 0.0 - || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getX() > AprilTagConstants.aprilTagLayout.getFieldLength() || observation.pose().getY() < 0.0 - || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + || observation.pose().getY() > AprilTagConstants.aprilTagLayout.getFieldWidth(); // Add pose to log robotPoses.add(observation.pose()); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java deleted file mode 100644 index ba5b1218..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright (c) 2024 Az-FIRST -// http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; - -public class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index f2b85b8f..6755c3a7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index ffa3a86a..14e06a87 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -62,7 +62,8 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { @Override public void updateInputs(VisionIOInputs inputs) { // Update connection status based on whether an update has been seen in the last 250ms - inputs.connected = (RobotController.getFPGATime() - latencySubscriber.getLastChange()) < 250; + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; // Update target observation inputs.latestTargetObservation = @@ -80,38 +81,38 @@ public void updateInputs(VisionIOInputs inputs) { List poseObservations = new LinkedList<>(); for (var rawSample : megatag1Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - for (int i = 10; i < rawSample.value.length; i += 7) { + for (int i = 11; i < rawSample.value.length; i += 7) { tagIds.add((int) rawSample.value[i]); } poseObservations.add( new PoseObservation( // Timestamp, based on server timestamp of publish and latency - rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3, + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, // 3D pose estimate parsePose(rawSample.value), // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag - rawSample.value.length >= 17 ? rawSample.value[16] : 0.0, + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, // Tag count - (int) rawSample.value[8], + (int) rawSample.value[7], // Average tag distance - rawSample.value[10], + rawSample.value[9], // Observation type PoseObservationType.MEGATAG_1)); } for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - for (int i = 10; i < rawSample.value.length; i += 7) { + for (int i = 11; i < rawSample.value.length; i += 7) { tagIds.add((int) rawSample.value[i]); } poseObservations.add( new PoseObservation( // Timestamp, based on server timestamp of publish and latency - rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3, + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, // 3D pose estimate parsePose(rawSample.value), @@ -120,10 +121,10 @@ public void updateInputs(VisionIOInputs inputs) { 0.0, // Tag count - (int) rawSample.value[8], + (int) rawSample.value[7], // Average tag distance - rawSample.value[10], + rawSample.value[9], // Observation type PoseObservationType.MEGATAG_2)); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 390f03a9..e61fc331 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,6 +15,8 @@ package frc.robot.subsystems.vision; +import static frc.robot.Constants.AprilTagConstants.*; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; @@ -59,7 +61,7 @@ public void updateInputs(VisionIOInputs inputs) { } // Add pose observation - if (result.multitagResult.isPresent()) { + if (result.multitagResult.isPresent()) { // Multitag result var multitagResult = result.multitagResult.get(); // Calculate robot pose @@ -85,6 +87,30 @@ public void updateInputs(VisionIOInputs inputs) { multitagResult.fiducialIDsUsed.size(), // Tag count totalTagDistance / result.targets.size(), // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + // Add tag ID + tagIds.add((short) target.fiducialId); + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 610c1f3f..e6574596 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -15,10 +15,9 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.Constants.AprilTagConstants; import java.util.function.Supplier; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; @@ -45,7 +44,7 @@ public VisionIOPhotonVisionSim( // Initialize vision sim if (visionSim == null) { visionSim = new VisionSystemSim("main"); - visionSim.addAprilTags(aprilTagLayout); + visionSim.addAprilTags(AprilTagConstants.aprilTagLayout); } // Add sim camera diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java index 5608c3b7..b29d4ee7 100644 --- a/src/main/java/frc/robot/util/Alert.java +++ b/src/main/java/frc/robot/util/Alert.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 FRC 6328 +// Copyright (c) 2024-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java index 3a59a7fd..3bc1578f 100644 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/GetJoystickValue.java b/src/main/java/frc/robot/util/GetJoystickValue.java new file mode 100644 index 00000000..d83b9018 --- /dev/null +++ b/src/main/java/frc/robot/util/GetJoystickValue.java @@ -0,0 +1,24 @@ +// Copyright (c) 2024-2025 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +/** + * Interface needed to abstraxct away which joystick is used for driving and which for steering with + * a swerve base. Teams may specify to use the left joystick for either driving or steering in the + * `Constants.java` file under OperatorConstants. + */ +@FunctionalInterface +public interface GetJoystickValue { + double value(); +} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 08b61727..e7828e50 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 Michael Jansen // http://gist.github.com/mjansen4857 diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 20c6b561..9db5d1cd 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java index a04b00e3..9301bab4 100644 --- a/src/main/java/frc/robot/util/OverrideSwitches.java +++ b/src/main/java/frc/robot/util/OverrideSwitches.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage @@ -18,7 +18,7 @@ // https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2024-build-thread/442736/72 // // The "Arcade Controller" used by 6328 (https://www.amazon.com/gp/product/B00UUROWWK) -// enumerates as a “Generic USB Controller”, mapped in this modules as a GenericHID. +// enumerates as a "Generic USB Controller", mapped in this modules as a GenericHID. // // If another type of controller is used, swap in the proper class. diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index cbe2d3ea..28951184 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/PowerMonitoring.java b/src/main/java/frc/robot/util/PowerMonitoring.java index 735054e2..52850d31 100644 --- a/src/main/java/frc/robot/util/PowerMonitoring.java +++ b/src/main/java/frc/robot/util/PowerMonitoring.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or @@ -13,12 +13,14 @@ package frc.robot.util; -import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.CANandPowerPorts; import frc.robot.Constants.PowerConstants; -import frc.robot.RobotContainer.Ports; import frc.robot.util.Alert.AlertType; +import org.littletonrobotics.conduit.ConduitApi; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggedPowerDistribution; /** * Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each @@ -30,30 +32,43 @@ public class PowerMonitoring extends VirtualSubsystem { private final RBSISubsystem[] subsystems; - /** Define the Power Distribution Hardware */ - private PowerDistribution m_powerModule = - new PowerDistribution( - Ports.POWER_CAN_DEVICE_ID.getDeviceNumber(), PowerConstants.kPowerModule); + // Get the AdvantageKit conduit for pulling PDM information + @SuppressWarnings("unused") + private LoggedPowerDistribution loggedPowerDistribution = LoggedPowerDistribution.getInstance(); - private int NUM_PDH_CHANNELS = m_powerModule.getNumChannels(); + private ConduitApi conduit = ConduitApi.getInstance(); + + // Define local variables + private int NUM_PDH_CHANNELS = conduit.getPDPChannelCount(); private double[] channelCurrents = new double[NUM_PDH_CHANNELS]; + private double totalAmpHours = 0.0; + private long lastTimestamp = RobotController.getFPGATime(); // In microseconds + private double deltaTime = 0.0; // In seconds + private double batteryVoltage; + private double totalCurrent; + private double driveCurrent; + private double steerCurrent; + private double subsystemCurrent; + private LoggedTunableNumber batteryCapacity; + private double batteryPercent; // DRIVE and STEER motor power ports private final int[] m_drivePowerPorts = { - Ports.FL_DRIVE.getPowerPort(), - Ports.FR_DRIVE.getPowerPort(), - Ports.BL_DRIVE.getPowerPort(), - Ports.BR_DRIVE.getPowerPort() + CANandPowerPorts.FL_DRIVE.getPowerPort(), + CANandPowerPorts.FR_DRIVE.getPowerPort(), + CANandPowerPorts.BL_DRIVE.getPowerPort(), + CANandPowerPorts.BR_DRIVE.getPowerPort() }; private final int[] m_steerPowerPorts = { - Ports.FL_ROTATION.getPowerPort(), - Ports.FR_ROTATION.getPowerPort(), - Ports.BL_ROTATION.getPowerPort(), - Ports.BR_ROTATION.getPowerPort() + CANandPowerPorts.FL_ROTATION.getPowerPort(), + CANandPowerPorts.FR_ROTATION.getPowerPort(), + CANandPowerPorts.BL_ROTATION.getPowerPort(), + CANandPowerPorts.BR_ROTATION.getPowerPort() }; // Class method definition, including inputs of optional subsystems - public PowerMonitoring(RBSISubsystem... subsystems) { + public PowerMonitoring(LoggedTunableNumber batteryCapacity, RBSISubsystem... subsystems) { + this.batteryCapacity = batteryCapacity; this.subsystems = subsystems; } @@ -61,20 +76,30 @@ public PowerMonitoring(RBSISubsystem... subsystems) { public void periodic() { // Check the total robot current and individual port currents against Constants - double totalCurrent = m_powerModule.getTotalCurrent(); + batteryVoltage = conduit.getPDPVoltage(); + totalCurrent = conduit.getPDPTotalCurrent(); if (totalCurrent > PowerConstants.kTotalMaxCurrent) { new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); } for (int i = 0; i < NUM_PDH_CHANNELS; i++) { - channelCurrents[i] = m_powerModule.getCurrent(i); + channelCurrents[i] = conduit.getPDPChannelCurrent(i); if (channelCurrents[i] > PowerConstants.kMotorPortMaxCurrent) { new Alert("Port " + i + " current draw exceeds limit!", AlertType.WARNING).set(true); } } + // Compute the total energy (charge) used in this loop + deltaTime = + (RobotController.getFPGATime() - lastTimestamp) / 1.0e6; // Time in seconds since last loop + lastTimestamp = RobotController.getFPGATime(); // Update timestamp + totalAmpHours += (totalCurrent * deltaTime / 3600.); + + // Compute estimated battery life left as a percent + batteryPercent = + (batteryCapacity.getAsDouble() - totalAmpHours) / batteryCapacity.getAsDouble() * 100; // Compute DRIVE and STEER summed currents - double driveCurrent = 0.0; - double steerCurrent = 0.0; + driveCurrent = 0.0; + steerCurrent = 0.0; for (int port : m_drivePowerPorts) { driveCurrent += channelCurrents[port]; } @@ -82,16 +107,22 @@ public void periodic() { steerCurrent += channelCurrents[port]; } // Log values to AdvantageKit and to SmartDashboard + Logger.recordOutput("PowerMonitor/Voltage", batteryVoltage); + Logger.recordOutput("PowerMonitor/EstimatedBatteryPercent", batteryPercent); + Logger.recordOutput("PowerMonitor/AmpHoursUsed", totalAmpHours); Logger.recordOutput("PowerMonitor/TotalCurrent", totalCurrent); Logger.recordOutput("PowerMonitor/DriveCurrent", driveCurrent); Logger.recordOutput("PowerMonitor/SteerCurrent", steerCurrent); + SmartDashboard.putNumber("BatteryVoltage", batteryVoltage); + SmartDashboard.putNumber("EstimatedBatteryPercent", batteryPercent); + SmartDashboard.putNumber("AmpHoursUsed", totalAmpHours); SmartDashboard.putNumber("TotalCurrent", totalCurrent); SmartDashboard.putNumber("DriveCurrent", driveCurrent); SmartDashboard.putNumber("SteerCurrent", steerCurrent); // Compute and log any passed-in subsystems for (RBSISubsystem subsystem : subsystems) { - double subsystemCurrent = 0.0; + subsystemCurrent = 0.0; for (int port : subsystem.getPowerPorts()) { subsystemCurrent += channelCurrents[port]; } @@ -99,7 +130,7 @@ public void periodic() { SmartDashboard.putNumber(subsystem.getName() + "Current", subsystemCurrent); } - // Do something about setting priorities if drawing too much current + // TODO: Do something about setting priorities if drawing too much current } } diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index 8725c45a..ddb31d86 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or @@ -51,4 +51,19 @@ public static enum VisionType { LIMELIGHT, // Limelight (https://docs.limelightvision.io/docs/docs-limelight/) NONE // No cameras } + + /** + * Enumerate CTRE Phoenix Pro Status + * https://v6.docs.ctr-electronics.com/en/latest/docs/licensing/licensing.html + */ + public static enum CTREPro { + LICENSED, // Have a valid 2025 CTRE Phoenix Pro License + UNLICENSED // Do not have a valid 2025 CTRE Phoenix Pro License + } + + /** Enumerate the supported motor idle modes */ + public static enum MotorIdleMode { + COAST, // Allow the motor to coast when idle + BRAKE // Hold motor position when idle + } } diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 7a5c8e46..829300ff 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java index 055491bc..4f1c2141 100644 --- a/src/main/java/frc/robot/util/RobotDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 254 // https://github.com/team254 diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java index 9c86fd2e..df627df4 100644 --- a/src/main/java/frc/robot/util/SparkUtil.java +++ b/src/main/java/frc/robot/util/SparkUtil.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright 2021-2024 FRC 6328 +// Copyright (c) 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 2943eb06..59117db7 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 9630b932..2b3302fe 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // // This program is free software; you can redistribute it and/or diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java index dcf25f9c..02459aa3 100644 --- a/src/test/CurrentLimitTests.java +++ b/src/test/CurrentLimitTests.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 Cross The Road Electronics +// Copyright (c) 2024-2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples // // This program is free software; you can redistribute it and/or diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java index 077eb269..bda6925f 100644 --- a/src/test/FusedCANcoderTests.java +++ b/src/test/FusedCANcoderTests.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 Cross The Road Electronics +// Copyright (c) 2024-2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples // // This program is free software; you can redistribute it and/or diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java index 27c696a2..4fb6b653 100644 --- a/src/test/LatencyCompensationTests.java +++ b/src/test/LatencyCompensationTests.java @@ -1,6 +1,6 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2024 Cross The Road Electronics +// Copyright (c) -2025 Cross The Road Electronics // https://github.com/CrossTheRoadElec/Phoenix6-Examples // // This program is free software; you can redistribute it and/or diff --git a/src/test/RobotContainerTest.java b/src/test/RobotContainerTest.java index a7912d60..ad125e03 100644 --- a/src/test/RobotContainerTest.java +++ b/src/test/RobotContainerTest.java @@ -1,4 +1,4 @@ -// Copyright (c) 2024 Az-FIRST +// Copyright (c) 2024-2025 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 6328 // http://github.com/Mechanical-Advantage diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 69538ea8..c7f5262f 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,23 +1,25 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.0.0-beta-1", + "version": "4.0.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", - "mavenUrls": [], + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.0.0-beta-1" + "version": "4.0.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.0.0-beta-1", + "version": "4.0.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/ChoreoLib2025Beta.json b/vendordeps/ChoreoLib2025.json similarity index 79% rename from vendordeps/ChoreoLib2025Beta.json rename to vendordeps/ChoreoLib2025.json index e3682576..faef0111 100644 --- a/vendordeps/ChoreoLib2025Beta.json +++ b/vendordeps/ChoreoLib2025.json @@ -1,19 +1,19 @@ { - "fileName": "ChoreoLib2025Beta.json", + "fileName": "ChoreoLib2025.json", "name": "ChoreoLib", - "version": "2025.0.0-beta-7", + "version": "2025.0.1", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ "https://lib.choreo.autos/dep", "https://repo1.maven.org/maven2" ], - "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025Beta.json", + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", "javaDependencies": [ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.0-beta-7" + "version": "2025.0.1" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.0-beta-7", + "version": "2025.0.1", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index 6e535663..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,39 +0,0 @@ -{ - "fileName": "NavX-2025.1.1-beta-1.json", - "name": "navx_frc", - "version": "2025.1.1-beta-1", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2025", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2025/" - ], - "jsonUrl": "https://dev.studica.com/releases/2025/NavX-2025.1.1-beta-1.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx_frc-java", - "version": "2025.1.1-beta-1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx_frc-cpp", - "version": "2025.1.1-beta-1", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} diff --git a/vendordeps/PathplannerLib-beta.json b/vendordeps/PathplannerLib.json similarity index 82% rename from vendordeps/PathplannerLib-beta.json rename to vendordeps/PathplannerLib.json index 3c999183..1371a406 100644 --- a/vendordeps/PathplannerLib-beta.json +++ b/vendordeps/PathplannerLib.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib-beta.json", + "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-5", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-5" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-5", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "osxuniversal", "linuxx86-64", + "osxuniversal", "linuxathena", "linuxarm32", "linuxarm64" diff --git a/vendordeps/Phoenix5-frc2025-beta-latest.json b/vendordeps/Phoenix5-frc2025-latest.json similarity index 84% rename from vendordeps/Phoenix5-frc2025-beta-latest.json rename to vendordeps/Phoenix5-frc2025-latest.json index ff142cf6..aa08d95b 100644 --- a/vendordeps/Phoenix5-frc2025-beta-latest.json +++ b/vendordeps/Phoenix5-frc2025-latest.json @@ -1,50 +1,50 @@ { - "fileName": "Phoenix5-frc2025-beta-latest.json", + "fileName": "Phoenix5-frc2025-latest.json", "name": "CTRE-Phoenix (v5)", - "version": "5.34.0-beta-4", + "version": "5.35.1", "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", "requires": [ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6-frc2025-beta-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" } ], "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" }, { "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - "offlineFileName": "Phoenix5-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.34.0-beta-4" + "version": "5.35.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.34.0-beta-4" + "version": "5.35.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.34.0-beta-4", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.34.0-beta-4", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.34.0-beta-4", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.34.0-beta-4", + "version": "5.35.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -106,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.34.0-beta-4", + "version": "5.35.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -122,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.34.0-beta-4", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.34.0-beta-4", + "version": "5.35.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -154,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.34.0-beta-4", + "version": "5.35.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6-frc2025-beta-latest.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 89% rename from vendordeps/Phoenix6-frc2025-beta-latest.json rename to vendordeps/Phoenix6-frc2025-latest.json index 140c7704..b1f42e3e 100644 --- a/vendordeps/Phoenix6-frc2025-beta-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,32 +1,32 @@ { - "fileName": "Phoenix6-frc2025-beta-latest.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-4", + "version": "25.1.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-frc2025-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-4" + "version": "25.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.0.0-beta-4", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +196,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +212,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +228,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +244,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +260,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +276,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +292,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +324,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -340,7 +340,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +356,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +372,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.0.0-beta-4", + "version": "25.1.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 440b1324..6e719675 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0-beta-3", + "version": "2025.0.0", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0-beta-3" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-3", + "version": "2025.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0-beta-3", + "version": "2025.0.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-3", + "version": "2025.0.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/ReduxLib_2025.json b/vendordeps/ReduxLib-2025.0.0.json similarity index 63% rename from vendordeps/ReduxLib_2025.json rename to vendordeps/ReduxLib-2025.0.0.json index b5e4a1f4..18ff5a11 100644 --- a/vendordeps/ReduxLib_2025.json +++ b/vendordeps/ReduxLib-2025.0.0.json @@ -1,8 +1,8 @@ { - "fileName": "ReduxLib_2025.json", + "fileName": "ReduxLib-2025.0.0.json", "name": "ReduxLib", - "version": "2025.0.0-beta0", - "frcYear": 2025, + "version": "2025.0.0", + "frcYear": "2025", "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ "https://maven.reduxrobotics.com/" @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2025.0.0-beta0" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2025.0.0-beta0", + "version": "2025.0.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,8 +36,8 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2025.0.0-beta0", - "libName": "ReduxLib-cpp", + "version": "2025.0.0", + "libName": "ReduxLib", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": true, @@ -50,6 +50,23 @@ "osxuniversal", "windowsx86-64" ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.0", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] } ] } diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.0.json new file mode 100644 index 00000000..8cba5305 --- /dev/null +++ b/vendordeps/Studica-2025.0.0.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.0.json", + "name": "Studica", + "version": "2025.0.0", + "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.0.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.0" + }, + { + "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.0" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.0" + } + ] +} diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json new file mode 100644 index 00000000..23948625 --- /dev/null +++ b/vendordeps/ThriftyLib.json @@ -0,0 +1,20 @@ +{ + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "https://docs.home.thethriftybot.com" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index 1099ee6c..c991b6af 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,25 +1,25 @@ { "fileName": "URCL.json", "name": "URCL", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "frcYear": "2025", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ - "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2025.0.0-beta-1" + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" ], - "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/main/URCL.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-java", - "version": "2025.0.0-beta-1" + "version": "2025.0.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-cpp", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "libName": "URCL", "headerClassifier": "headers", "sharedLibrary": false, @@ -49,7 +49,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0-beta-1", + "version": "2025.0.0", "libName": "URCLDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 2dbf1d32..39a6c80b 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.2.5", + "version": "0.3.1", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.2.5" + "version": "0.3.1" }, { "groupId": "org.dyn4j", diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 6a23ba8f..1163429e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-6", + "version": "v2025.0.0-beta-8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-6" + "version": "v2025.0.0-beta-8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-6" + "version": "v2025.0.0-beta-8" } ] } diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl-2025.1.3.jwt151.json similarity index 69% rename from vendordeps/yagsl.json rename to vendordeps/yagsl-2025.1.3.jwt151.json index 1b3bc77a..97dc57eb 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl-2025.1.3.jwt151.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl.json", + "fileName": "yagsl-2025.1.3.jwt151.json", "name": "YAGSL", - "version": "2025.1.0.0.10", + "version": "2025.1.3.jwt151", "frcYear": "2025", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,39 +12,45 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2025.1.0.0.10" + "version": "2025.1.3.jwt151" } ], "requires": [ { "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "errorMessage": "REVLib is required!", - "offlineFileName": "REVLib.json", + "offlineFileName": "REVLib-2025.0.0.json", "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" }, { "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "errorMessage": "ReduxLib is required!", - "offlineFileName": "ReduxLib_2025.json", + "offlineFileName": "ReduxLib-2025.0.0.json", "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" }, { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix6 is required!", - "offlineFileName": "Phoenix6-frc2025-beta-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-25.1.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" }, { "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "errorMessage": "Phoenix5 is required!", - "offlineFileName": "Phoenix5.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json" + "offlineFileName": "Phoenix5-5.35.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" }, { "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "errorMessage": "NavX is required!", - "offlineFileName": "Studica-2025.1.1-beta-3.json", - "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json" + "offlineFileName": "Studica-2025.0.0.json", + "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" + }, + { + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "errorMessage": "ThriftyLib is required!", + "offlineFileName": "ThriftyLib.json", + "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" }, { "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",