diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..02b78d3 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,18 @@ +name: Build + +on: + push: + pull_request: + +jobs: + build: + name: Build + runs-on: ubuntu-latest + container: wpilib/roborio-cross-ubuntu:2024-22.04 + steps: + - name: Checkout repository + uses: actions/checkout@v4 + - name: Grant execute permission + run: chmod +x gradlew + - name: Build robot code + run: ./gradlew build diff --git a/.gitignore b/.gitignore index 5528d4f..d012c1d 100644 --- a/.gitignore +++ b/.gitignore @@ -169,6 +169,8 @@ out/ .fleet # Simulation GUI and other tools window save file +networktables.json +simgui*.json *-window.json # Simulation data log directory @@ -176,3 +178,10 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Version file +src/main/java/frc/robot/BuildConstants.java \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json index e7ca395..567056a 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,6 +1,5 @@ { "recommendations": [ - "mechanical-advantage.event-deploy-wpilib", "richardwillis.vscode-spotless-gradle" ] } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 8150cff..132cbcd 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025beta", "teamNumber": 226 } diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json new file mode 100644 index 0000000..e3c2d54 --- /dev/null +++ b/AdvantageScope Swerve Calibration.json @@ -0,0 +1,463 @@ +{ + "hubs": [ + { + "x": 139, + "y": 95, + "width": 1100, + "height": 650, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/Drive", + "/RealOutputs", + "/RealOutputs/SwerveStates", + "/RealOutputs/SwerveChassisSpeeds", + "/RealOutputs/Odometry" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 9, + "title": "Drive Overview", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ffff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#00ffff" + } + }, + { + "type": "rotation", + "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "prdx7t2eedan6n46dxrjfu7eisf16f9o", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot" + } + }, + { + "type": "trajectory", + "logKey": "/RealOutputs/Odometry/Trajectory", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#ff8c00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#ff8c00" + } + } + ], + "game": "2024 Field", + "origin": "blue" + }, + "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 1.4695761589768238e-15, + 5.999999999999999, + -12 + ], + "cameraTarget": [ + 0, + 0.5, + 0 + ] + }, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "ol8lk80m83ma3aegae849d6k1d29sp7d", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "7jj1f83nsv59dd2ou11ls3ccz5tq1293", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "kj6tf6sxm5g04wnjrhed6mx2wpccmqtn", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "bctxg9pmwpo31m9bv9ofr92oqqtbpqzp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Max Speed Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "oy3vmjuhr7g36evikwnmjhfacl30mzn5", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Slip Current Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveCurrentAmps", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveVelocityRadPerSec", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "efudylbvebbv1ga2kosknov0uegmtj7h", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.0.0-beta-3" +} diff --git a/build.gradle b/build.gradle index 51a55b2..9b82865 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } @@ -29,12 +29,25 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // https://www.chiefdelphi.com/t/2024-wpilib-feedback/464322/141 + final MAX_JAVA_HEAP_SIZE_MB = 100; + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + + // Set the minimum heap size to the maximum heap size to avoid resizing + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") } // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' + // Change to true to delete files on roboRIO that no + // longer exist in deploy directory on roboRIO + deleteOldFiles = false } } } @@ -61,19 +74,15 @@ repositories { mavenLocal() } -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" classpath = sourceSets.main.runtimeClasspath } -compileJava.finalizedBy checkAkitInstall // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -95,7 +104,7 @@ dependencies { testRuntimeOnly 'org.junit.platform:junit-platform-launcher' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } test { @@ -143,6 +152,39 @@ gversion { indent = " " } +// Create commit with working changes on event branches +task(eventDeploy) { + doLast { + if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { + def branchPrefix = "event" + def branch = 'git branch --show-current'.execute().text.trim() + def commitMessage = "Update at '${new Date().toString()}'" + + if (branch.startsWith(branchPrefix)) { + exec { + workingDir(projectDir) + executable 'git' + args 'add', '-A' + } + exec { + workingDir(projectDir) + executable 'git' + args 'commit', '-m', commitMessage + ignoreExitValue = true + } + + println "Committed to branch: '$branch'" + println "Commit message: '$commitMessage'" + } else { + println "Not on an event branch, skipping commit" + } + } else { + println "Not running deploy task, skipping commit" + } + } +} +createVersionFile.dependsOn(eventDeploy) + // Spotless formatting project.compileJava.dependsOn(spotlessApply) spotless { diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 7f93135..a4b76b9 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 1058752..34bd9ce 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto index 77a8433..70b7ab2 100644 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -1,22 +1,9 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 180.0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Run Flywheel" - } - }, { "type": "path", "data": { @@ -26,6 +13,7 @@ ] } }, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 8f3609b..303dbb2 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -46,20 +46,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": null, - "previewStartingState": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..f981b63 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,21 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotWheelbase": 0.546, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60FOC", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2 +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 1d72851..268643d 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,8 +3,9 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025TemplateRobotCode"; + public static final String MAVEN_NAME = "AdvantageKit_TalonFXSwerveTemplate"; public static final String VERSION = "unspecified"; +<<<<<<< Updated upstream public static final int GIT_REVISION = 14; public static final String GIT_SHA = "71903e992f09d70d8dc29903114fa0ed607e07e7"; public static final String GIT_DATE = "2024-11-10 16:32:20 EST"; @@ -12,6 +13,15 @@ public final class BuildConstants { public static final String BUILD_DATE = "2024-11-10 17:28:21 EST"; public static final long BUILD_UNIX_TIME = 1731277701353L; public static final int DIRTY = 1; +======= + public static final int GIT_REVISION = -1; + public static final String GIT_SHA = "UNKNOWN"; + public static final String GIT_DATE = "UNKNOWN"; + public static final String GIT_BRANCH = "UNKNOWN"; + public static final String BUILD_DATE = "2024-12-06 16:30:32 EST"; + public static final long BUILD_UNIX_TIME = 1733520632774L; + public static final int DIRTY = 129; +>>>>>>> Stashed changes private BuildConstants() {} } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..4c29008 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,37 @@ +// 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; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * (log replay from a file). + */ +public final class Constants { + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 31dd8f8..58d7742 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -32,12 +33,7 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { + public Robot() { // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); @@ -57,7 +53,7 @@ public void robotInit() { } // Set up data receivers & replay source - switch (physicalConstants.currentMode) { + switch (Constants.currentMode) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") Logger.addDataReceiver(new WPILOGWriter()); @@ -78,9 +74,6 @@ public void robotInit() { break; } - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - // Start AdvantageKit logger Logger.start(); @@ -92,12 +85,18 @@ public void robotInit() { /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { + // Switch thread to high priority to improve loop timing + Threads.setCurrentThreadPriority(true, 99); + // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing // finished or interrupted commands, and running subsystem periodic() methods. // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + + // Return to normal thread priority + Threads.setCurrentThreadPriority(false, 10); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 971afff..c405902 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ package frc.robot; 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.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -24,18 +23,24 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +<<<<<<< Updated upstream +======= +import frc.robot.generated.TunerConstants; +>>>>>>> Stashed changes import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +<<<<<<< Updated upstream import frc.robot.subsystems.flywheel.Flywheel; import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOSim; import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; +======= +>>>>>>> Stashed changes import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -45,24 +50,27 @@ */ public class RobotContainer { // Subsystems +<<<<<<< Updated upstream public static Drive drive; private final Flywheel flywheel; +======= + private final Drive drive; +>>>>>>> Stashed changes // Controller private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - switch (physicalConstants.currentMode) { + switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations drive = new Drive( +<<<<<<< Updated upstream new GyroIOPigeon2(false), new ModuleIOTalonFX(0), new ModuleIOTalonFX(1), @@ -76,6 +84,13 @@ public RobotContainer() { // new ModuleIOTalonFX(2), // new ModuleIOTalonFX(3)); // flywheel = new Flywheel(new FlywheelIOTalonFX()); +======= + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight)); +>>>>>>> Stashed changes break; case SIM: @@ -83,11 +98,18 @@ public RobotContainer() { drive = new Drive( new GyroIO() {}, +<<<<<<< Updated upstream new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); flywheel = new Flywheel(new FlywheelIOSim()); +======= + new ModuleIOSim(TunerConstants.FrontLeft), + new ModuleIOSim(TunerConstants.FrontRight), + new ModuleIOSim(TunerConstants.BackLeft), + new ModuleIOSim(TunerConstants.BackRight)); +>>>>>>> Stashed changes break; default: @@ -99,19 +121,21 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); +<<<<<<< Updated upstream flywheel = new Flywheel(new FlywheelIO() {}); +======= +>>>>>>> Stashed changes break; } // Set up auto routines - NamedCommands.registerCommand( - "Run Flywheel", - Commands.startEnd( - () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel) - .withTimeout(5.0)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set up SysId routines + autoChooser.addOption( + "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); + autoChooser.addOption( + "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); autoChooser.addOption( "Drive SysId (Quasistatic Forward)", drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); @@ -122,16 +146,6 @@ public RobotContainer() { "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); autoChooser.addOption( "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Flywheel SysId (Quasistatic Forward)", - flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Flywheel SysId (Quasistatic Reverse)", - flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Flywheel SysId (Dynamic Forward)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // Configure the button bindings configureButtonBindings(); @@ -144,13 +158,28 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + // Default command, normal field-relative drive drive.setDefaultCommand( DriveCommands.joystickDrive( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); + + // Lock to 0° when A button is held + controller + .a() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> new Rotation2d())); + + // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + + // Reset gyro to 0° when B button is pressed controller .b() .onTrue( @@ -160,11 +189,14 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); +<<<<<<< Updated upstream controller .a() .whileTrue( Commands.startEnd( () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); +======= +>>>>>>> Stashed changes } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index b85724a..143d02a 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -14,23 +14,55 @@ package frc.robot.commands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +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.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.function.DoubleSupplier; +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; + 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 + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 private DriveCommands() {} + 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). */ @@ -41,39 +73,220 @@ public static Command joystickDrive( DoubleSupplier omegaSupplier) { return Commands.run( () -> { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); - Rotation2d linearDirection = - new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Apply rotation deadband double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; + // Square rotation value for more precise control omega = Math.copySign(omega * omega, omega); - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - // 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(), - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + speeds.toRobotRelativeSpeeds( + isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation()); + drive.runVelocity(speeds); }, drive); } -} -// Write extra vision code in vision, and drive commands at rotation controller (use object passing) + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + Supplier rotationSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Calculate angular speed + double omega = + angleController.calculate( + drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + 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) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(Drive drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(Drive drive) { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + drive)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + 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; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********"); + System.out.println( + "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java new file mode 100644 index 0000000..be9c9ab --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -0,0 +1,225 @@ +package frc.robot.generated; + +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 edu.wpi.first.units.measure.*; +// import frc.robot.subsystems.CommandSwerveDrivetrain; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(2.0) + .withKS(0.2) + .withKV(1.59) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // 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; + + // The stator current at which the wheels start to slip; + // 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. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.55); + + // 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 = 3.5; + + private static final double kDriveGearRatio = 7.363636364; + private static final double kSteerGearRatio = 12.8; + private static final Distance kWheelRadius = Inches.of(2.167); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final double kSteerInertia = 0.004; + private static final double kDriveInertia = 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); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .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); + + // Front Left + private static final int kFrontLeftDriveMotorId = 5; + private static final int kFrontLeftSteerMotorId = 4; + private static final int kFrontLeftEncoderId = 2; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.83544921875); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftCANcoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.5); + private static final Distance kFrontLeftYPos = Inches.of(10.5); + + // Front Right + private static final int kFrontRightDriveMotorId = 7; + private static final int kFrontRightSteerMotorId = 6; + private static final int kFrontRightEncoderId = 3; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.15234375); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightCANcoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.5); + private static final Distance kFrontRightYPos = Inches.of(-10.5); + + // Back Left + private static final int kBackLeftDriveMotorId = 1; + private static final int kBackLeftSteerMotorId = 0; + private static final int kBackLeftEncoderId = 0; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.4794921875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftCANcoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.5); + private static final Distance kBackLeftYPos = Inches.of(10.5); + + // Back Right + private static final int kBackRightDriveMotorId = 3; + private static final int kBackRightSteerMotorId = 2; + private static final int kBackRightEncoderId = 1; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.84130859375); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightCANcoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.5); + private static final Distance kBackRightYPos = Inches.of(-10.5); + + 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 + // ); + // } +} diff --git a/src/main/java/frc/robot/setPointsConstants.java b/src/main/java/frc/robot/setPointsConstants.java deleted file mode 100644 index 9af7e46..0000000 --- a/src/main/java/frc/robot/setPointsConstants.java +++ /dev/null @@ -1,3 +0,0 @@ -package frc.robot; - -public class setPointsConstants {} diff --git a/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java b/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java index 47dea78..347bdf3 100644 --- a/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/arms/PivotIOSim.java @@ -7,12 +7,20 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.physicalConstants; /** Add your docs here. */ public class PivotIOSim implements PivotIO { +<<<<<<< Updated upstream +======= + private final DCMotor pivotGearbox = DCMotor.getKrakenX60Foc(2); + + public RobotContainer robotContainer = new RobotContainer(); + +>>>>>>> Stashed changes // SIM VARIABLES private int gearBoxMotorCount = 2; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3f153eb..ae030f7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,13 +15,18 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -31,13 +36,19 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.physicalConstants; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.generated.TunerConstants; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -45,27 +56,43 @@ import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - private static final double MAX_LINEAR_SPEED = - physicalConstants.SwerveConstants.MAX_LINEAR_SPEED * 0.85; - private static final double TRACK_WIDTH_X = - physicalConstants.SwerveConstants.TRACK_WIDTH_X_METERS; - private static final double TRACK_WIDTH_Y = - physicalConstants.SwerveConstants.TRACK_WIDTH_Y_METERS; - private static final double DRIVE_BASE_RADIUS = - physicalConstants.SwerveConstants.DRIVE_BASE_RADIUS; - private static final double MAX_ANGULAR_SPEED = - physicalConstants.SwerveConstants.MAX_ANGULAR_SPEED; - private static double multiplier = 1.0; - private static boolean toggle = false; + // TunerConstants doesn't include these constants, so they are declared locally + static final double ODOMETRY_FREQUENCY = + new CANBus(TunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; + public static final double DRIVE_BASE_RADIUS = + Math.max( + Math.max( + Math.hypot(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontRight.LocationY), + Math.hypot(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); + + // 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, + new ModuleConfig( + TunerConstants.FrontLeft.WheelRadius, + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1) + .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), + TunerConstants.FrontLeft.SlipCurrent, + 1), + getModuleTranslations()); + static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; - public double yawVelocityRadPerSec = gyroInputs.yawVelocityRadPerSec; - - private final PIDController rotationController; - static final Lock odometryLock = new ReentrantLock(); + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -76,26 +103,8 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - public SwerveDrivePoseEstimator poseEstimator = + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - TimestampedT2d lastNoteLocT2d = new TimestampedT2d(new Translation2d(0, 0), -1.); - - public class TimestampedPose2d { - Pose2d pose; - double time; - } - - public class TimestampedT2d { - Translation2d translation; - double time; - - public TimestampedT2d(Translation2d translation, double time) { - this.translation = translation; - this.time = time; - } - } - - CircularBuffer robotPoseBuffer; public Drive( GyroIO gyroIO, @@ -104,28 +113,27 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO) { this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); + modules[0] = new Module(flModuleIO, 0, TunerConstants.FrontLeft); + modules[1] = new Module(frModuleIO, 1, TunerConstants.FrontRight); + modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft); + modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight); - // Start threads (no-op for each if no signals have been created) + // Usage reporting for swerve template + HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); + + // Start odometry thread PhoenixOdometryThread.getInstance().start(); - AutoBuilder.configureHolonomic( + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( this::getPose, this::setPose, - () -> kinematics.toChassisSpeeds(getModuleStates()), + this::getChassisSpeeds, this::runVelocity, - new HolonomicPathFollowerConfig( - new PIDConstants(5), - new PIDConstants(1.5), - physicalConstants.SwerveConstants.MAX_LINEAR_SPEED, - DRIVE_BASE_RADIUS, - new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + PP_CONFIG, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( @@ -147,32 +155,18 @@ public Drive( null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(voltage.in(Volts)); - } - }, - null, - this)); - - rotationController = new PIDController(0.1, 0, 0); - - rotationController.setTolerance(5); - rotationController.enableContinuousInput(-180, 180); - robotPoseBuffer = new CircularBuffer<>(11); + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); } + @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); - } - odometryLock.unlock(); Logger.processInputs("Drive/Gyro", gyroInputs); for (var module : modules) { module.periodic(); } + odometryLock.unlock(); // Stop moving when disabled if (DriverStation.isDisabled()) { @@ -180,6 +174,7 @@ public void periodic() { module.stop(); } } + // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); @@ -217,6 +212,9 @@ public void periodic() { // Apply update poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); } /** @@ -226,20 +224,28 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + speeds.discretize(0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + modules[i].runSetpoint(setpointStates[i]); } - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } } /** Stops the drive. */ @@ -262,12 +268,14 @@ public void stopWithX() { /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); } /** Returns a command to run a dynamic test in the specified direction. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @@ -289,6 +297,30 @@ private SwerveModulePosition[] getModulePositions() { return states; } + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { @@ -305,33 +337,32 @@ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } - /** - * Adds a vision measurement to the pose estimator. - * - * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. - */ - public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return MAX_LINEAR_SPEED; + return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); } /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return MAX_ANGULAR_SPEED; + return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; } /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { - new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) + new Translation2d(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), + new Translation2d(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), + new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 18ce6fd..25c3338 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -21,9 +21,9 @@ public interface GyroIO { public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; } public default void updateInputs(GyroIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java new file mode 100644 index 0000000..473cd3d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -0,0 +1,48 @@ +// 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.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.SPI; +import java.util.Queue; + +/** IO implementation for NavX. */ +public class GyroIONavX implements GyroIO { + private final AHRS navX = new AHRS(SPI.Port.kMXP, (byte) Drive.ODOMETRY_FREQUENCY); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + + public GyroIONavX() { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(navX::getYaw); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = navX.isConnected(); + inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getRawGyroZ()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(-value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index e520ed2..f849a25 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -20,24 +20,30 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.generated.TunerConstants; import java.util.Queue; -/** IO implementation for Pigeon2 */ +/** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); - private final StatusSignal yaw = pigeon.getYaw(); + private final Pigeon2 pigeon = + new Pigeon2( + TunerConstants.DrivetrainConstants.Pigeon2Id, + TunerConstants.DrivetrainConstants.CANBusName); + private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - public GyroIOPigeon2(boolean phoenixDrive) { + public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(100.0); + yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); + yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index b4db3e8..c43184d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -13,168 +13,98 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import frc.robot.physicalConstants; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import org.littletonrobotics.junction.Logger; public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - static final double ODOMETRY_FREQUENCY = 250.0; - private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; + private final SwerveModuleConstants constants; - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - public Module(ModuleIO io, int index) { + public Module(ModuleIO io, int index, SwerveModuleConstants constants) { this.io = io; this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (physicalConstants.currentMode) { - case REAL: - case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - turnFeedback = new PIDController(10.0, 0.0, 0.0); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - turnFeedback = new PIDController(0.0, 0.0, 0.0); - break; - } - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } - - /** - * Update inputs without running the rest of the periodic logic. This is useful since these - * updates need to be properly thread-locked. - */ - public void updateInputs() { - io.updateInputs(inputs); + this.constants = constants; + driveDisconnectedAlert = + new Alert( + "Disconnected drive motor on module " + Integer.toString(index) + ".", + AlertType.kError); + turnDisconnectedAlert = + new Alert( + "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + turnEncoderDisconnectedAlert = + new Alert( + "Disconnected turn encoder on module " + Integer.toString(index) + ".", + AlertType.kError); } public void periodic() { + io.updateInputs(inputs); Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - // Run closed loop turn control - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } - } - // Calculate positions for odometry int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together odometryPositions = new SwerveModulePosition[sampleCount]; for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; - Rotation2d angle = - inputs.odometryTurnPositions[i].plus( - turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); + double positionMeters = inputs.odometryDrivePositionsRad[i] * constants.WheelRadius; + Rotation2d angle = inputs.odometryTurnPositions[i]; odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } - } - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + } - // Update setpoints, controllers run in "periodic" - angleSetpoint = optimizedState.angle; - speedSetpoint = optimizedState.speedMetersPerSecond; + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + public void runSetpoint(SwerveModuleState state) { + // Optimize velocity setpoint + state.optimize(getAngle()); + state.cosineScale(inputs.turnPosition); - return optimizedState; + // Apply setpoints + io.setDriveVelocity(state.speedMetersPerSecond / constants.WheelRadius); + io.setTurnPosition(state.angle); } - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + io.setDriveOpenLoop(output); + io.setTurnPosition(new Rotation2d()); } /** Disables all outputs to motors. */ public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); + io.setDriveOpenLoop(0.0); + io.setTurnOpenLoop(0.0); } /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); - } + return inputs.turnPosition; } /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return inputs.drivePositionRad * WHEEL_RADIUS; + return inputs.drivePositionRad * constants.WheelRadius; } /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + return inputs.driveVelocityRadPerSec * constants.WheelRadius; } /** Returns the module position (turn angle and drive position). */ @@ -197,8 +127,13 @@ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(inputs.driveVelocityRadPerSec); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 200afa3..c6db5a0 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -19,16 +19,19 @@ public interface ModuleIO { @AutoLog public static class ModuleIOInputs { + public boolean driveConnected = false; public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; + public double driveCurrentAmps = 0.0; + public boolean turnConnected = false; + public boolean turnEncoderConnected = false; public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; + public double turnCurrentAmps = 0.0; public double[] odometryTimestamps = new double[] {}; public double[] odometryDrivePositionsRad = new double[] {}; @@ -38,15 +41,15 @@ public static class ModuleIOInputs { /** Updates the set of loggable inputs. */ public default void updateInputs(ModuleIOInputs inputs) {} - /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} - /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} - /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} - /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 3e79e50..7702e48 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -13,20 +13,22 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; /** - * Physics sim implementation of module IO. - * - *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized - * to a random value. The flywheel sims are not physically accurate, but provide a decent - * approximation for the behavior of the module. + * Physics sim implementation of module IO. The sim models are configured using a set of module + * constants from Phoenix. Simulation is always based on voltage control. */ public class ModuleIOSim implements ModuleIO { +<<<<<<< Updated upstream private static final double LOOP_PERIOD_SECS = 0.02; private int gearBoxMotorCountDrive = 1; @@ -40,37 +42,98 @@ public class ModuleIOSim implements ModuleIO { new DCMotorSim(DCMotor.getNEO(gearBoxMotorCountDrive), gearingDrive, momentOfInertiaDrive); private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(gearBoxMotorCountTurn), gearingTurn, momentOfInertiaTurn); +======= + // TunerConstants doesn't support separate sim constants, so they are declared locally + private static final double DRIVE_KP = 0.05; + private static final double DRIVE_KD = 0.0; + private static final double DRIVE_KS = 0.0; + private static final double DRIVE_KV_ROT = + 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + private static final double TURN_KP = 8.0; + private static final double TURN_KD = 0.0; + private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); + private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); - private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; +>>>>>>> Stashed changes + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController(DRIVE_KP, 0, DRIVE_KD); + private PIDController turnController = new PIDController(TURN_KP, 0, TURN_KD); + private double driveFFVolts = 0.0; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; +<<<<<<< Updated upstream private double clampedValueLowVolts = -12.0; private double clampedValueHighVolts = 12.0; +======= + public ModuleIOSim(SwerveModuleConstants constants) { + // Create drive and turn sim models + driveSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio), + DRIVE_GEARBOX); + turnSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), + TURN_GEARBOX); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } +>>>>>>> Stashed changes @Override public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(LOOP_PERIOD_SECS); - turnSim.update(LOOP_PERIOD_SECS); + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = + driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); + } else { + turnController.reset(); + } + + // Update simulation state + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(0.02); + turnSim.update(0.02); + // Update drive inputs + inputs.driveConnected = true; inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); - inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()); inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override +<<<<<<< Updated upstream public void setDriveVoltage(double volts) { driveAppliedVolts = MathUtil.clamp(volts, clampedValueLowVolts, clampedValueHighVolts); driveSim.setInputVoltage(driveAppliedVolts); @@ -80,5 +143,29 @@ public void setDriveVoltage(double volts) { public void setTurnVoltage(double volts) { turnAppliedVolts = MathUtil.clamp(volts, clampedValueLowVolts, clampedValueHighVolts); turnSim.setInputVoltage(turnAppliedVolts); +======= + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); +>>>>>>> Stashed changes } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b41202e..e24dc54 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -13,122 +13,162 @@ package frc.robot.subsystems.drive; +import static frc.robot.util.PhoenixUtil.*; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.generated.TunerConstants; import java.util.Queue; /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and - * CANcoder - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax") + * CANcoder. Configured using a set of module constants from Phoenix. * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. */ public class ModuleIOTalonFX implements ModuleIO { + private final SwerveModuleConstants constants; + + // Hardware objects private final TalonFX driveTalon; private final TalonFX turnTalon; private final CANcoder cancoder; + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread private final Queue timestampQueue; - private final StatusSignal drivePosition; + // Inputs from drive motor + private final StatusSignal drivePosition; private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOTalonFX(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(0); - turnTalon = new TalonFX(1); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveTalon = new TalonFX(6); - turnTalon = new TalonFX(7); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); + public ModuleIOTalonFX(SwerveModuleConstants constants) { + this.constants = constants; + driveTalon = new TalonFX(constants.DriveMotorId, TunerConstants.DrivetrainConstants.CANBusName); + turnTalon = new TalonFX(constants.SteerMotorId, TunerConstants.DrivetrainConstants.CANBusName); + cancoder = new CANcoder(constants.CANcoderId, TunerConstants.DrivetrainConstants.CANBusName); + + // 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.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.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnTalon.getConfigurator().apply(turnConfig); - setTurnBrakeMode(true); + 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.MotionMagic.MotionMagicAcceleration = + turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + 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)); - cancoder.getConfigurator().apply(new CANcoderConfiguration()); + // Configure CANCoder + CANcoderConfiguration cancoderConfig = constants.CANcoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.CANcoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = + constants.CANcoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cancoder.getConfigurator().apply(cancoderConfig); + // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + // Create drive status signals drivePosition = driveTalon.getPosition(); drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition()); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); + driveCurrent = driveTalon.getStatorCurrent(); + // Create turn status signals turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon.getPosition()); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getSupplyCurrent(); + turnCurrent = turnTalon.getStatorCurrent(); + // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( - Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, @@ -138,49 +178,44 @@ public ModuleIOTalonFX(int index) { turnVelocity, turnAppliedVolts, turnCurrent); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); } @Override public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + // Refresh all signals + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + // Update odometry inputs inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryDrivePositionsRad = drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) .toArray(); inputs.odometryTurnPositions = turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .map((Double value) -> Rotation2d.fromRotations(value)) .toArray(Rotation2d[]::new); timestampQueue.clear(); drivePositionQueue.clear(); @@ -188,31 +223,40 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); + public void setDriveOpenLoop(double output) { + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); } @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts)); + public void setTurnOpenLoop(double output) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); } @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + }); } @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition( + rotation.getRotations()); + }); } } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index b7531fa..4653b2f 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -16,14 +16,16 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.ParentDevice; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.generated.TunerConstants; import java.util.ArrayList; import java.util.List; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import org.littletonrobotics.junction.Logger; +import java.util.function.DoubleSupplier; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -36,11 +38,14 @@ public class PhoenixOdometryThread extends Thread { private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + private final List genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private boolean isCANFD = false; + private static boolean isCANFD = + new CANBus(TunerConstants.DrivetrainConstants.CANBusName).isNetworkFD(); private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { @@ -62,17 +67,17 @@ public void start() { } } - public Queue registerSignal(ParentDevice device, StatusSignal signal) { + /** Registers a Phoenix signal to be read from the thread. */ + public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); Drive.odometryLock.lock(); try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); + BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; + System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); + newSignals[phoenixSignals.length] = signal; + phoenixSignals = newSignals; + phoenixQueues.add(queue); } finally { signalsLock.unlock(); Drive.odometryLock.unlock(); @@ -80,6 +85,22 @@ public Queue registerSignal(ParentDevice device, StatusSignal si return queue; } + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); @@ -97,15 +118,14 @@ public void run() { // Wait for updates from all signals signalsLock.lock(); try { - if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY, signals); + if (isCANFD && phoenixSignals.length > 0) { + BaseStatusSignal.waitForAll(2.0 / Drive.ODOMETRY_FREQUENCY, 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 / Module.ODOMETRY_FREQUENCY)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + // "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)); + if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); } } catch (InterruptedException e) { e.printStackTrace(); @@ -116,16 +136,24 @@ public void run() { // Save new data to queues Drive.odometryLock.lock(); try { - double timestamp = Logger.getRealTimestamp() / 1e6; + // Sample timestamp is current FPGA time minus average CAN latency + // Default timestamps from Phoenix are NOT compatible with + // FPGA timestamps, this solution is imperfect but close + double timestamp = RobotController.getFPGATime() / 1e6; double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { + for (BaseStatusSignal signal : phoenixSignals) { totalLatency += signal.getTimestamp().getLatency(); } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; + if (phoenixSignals.length > 0) { + timestamp -= totalLatency / phoenixSignals.length; + } + + // Add new samples to queues + for (int i = 0; i < phoenixSignals.length; i++) { + phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble()); } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); } for (int i = 0; i < timestampQueues.size(); i++) { timestampQueues.get(i).offer(timestamp); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index bcaeba4..94b19f2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -4,9 +4,11 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.physicalConstants; -import frc.robot.util.LoggedTunableNumber; + +import static edu.wpi.first.units.Units.MetersPerSecond; + import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class Elevator extends SubsystemBase { @@ -14,19 +16,19 @@ public class Elevator extends SubsystemBase { private final ElevatorIOInputsAutoLogged eInputs = new ElevatorIOInputsAutoLogged(); - private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); - private static final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/kI"); + private static final LoggedNetworkNumber kP = new LoggedNetworkNumber("Elevator/kP"); + private static final LoggedNetworkNumber kI = new LoggedNetworkNumber("Elevator/kI"); - private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); - private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); - private static final LoggedTunableNumber kV = new LoggedTunableNumber("Elevator/kV"); - private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/kA"); + private static final LoggedNetworkNumber kS = new LoggedNetworkNumber("Elevator/kS"); + private static final LoggedNetworkNumber kG = new LoggedNetworkNumber("Elevator/kG"); + private static final LoggedNetworkNumber kV = new LoggedNetworkNumber("Elevator/kV"); + private static final LoggedNetworkNumber kA = new LoggedNetworkNumber("Elevator/kA"); // amp bar gains - private static final LoggedTunableNumber barkP = new LoggedTunableNumber("Bar/kP"); - private static final LoggedTunableNumber barkV = new LoggedTunableNumber("Bar/kV"); - private static final LoggedTunableNumber barkG = new LoggedTunableNumber("Bar/kG"); + private static final LoggedNetworkNumber barkP = new LoggedNetworkNumber("Bar/kP"); + private static final LoggedNetworkNumber barkV = new LoggedNetworkNumber("Bar/kV"); + private static final LoggedNetworkNumber barkG = new LoggedNetworkNumber("Bar/kG"); private TrapezoidProfile extenderProfile; private TrapezoidProfile.Constraints extenderConstraints = @@ -52,56 +54,57 @@ public Elevator(ElevatorIO elevator) { switch (physicalConstants.getMode()) { case REAL: - kS.initDefault(0); - kG.initDefault(0.25); - kV.initDefault(0); - kA.initDefault(0); - - kP.initDefault(0.44); - kI.initDefault(0); - - barkP.initDefault(0.25); - barkV.initDefault(0.2); - barkG.initDefault(0); + + kS.setDefault(0); + kG.setDefault(0.25); + kV.setDefault(0); + kA.setDefault(0); + + kP.setDefault(0.44); + kI.setDefault(0); + + barkP.setDefault(0.25); + barkV.setDefault(0.2); + barkG.setDefault(0); break; case REPLAY: - kS.initDefault(0); - kG.initDefault(0.13); - kV.initDefault(0); - kA.initDefault(0); + kS.setDefault(0); + kG.setDefault(0.13); + kV.setDefault(0); + kA.setDefault(0); - kP.initDefault(15); - kI.initDefault(0); + kP.setDefault(15); + kI.setDefault(0); - barkP.initDefault(0); - barkV.initDefault(0); - barkG.initDefault(0); + barkP.setDefault(0); + barkV.setDefault(0); + barkG.setDefault(0); break; case SIM: - kS.initDefault(0); - kG.initDefault(0.04); - kV.initDefault(0); - kA.initDefault(0); + kS.setDefault(0); + kG.setDefault(0.04); + kV.setDefault(0); + kA.setDefault(0); - kP.initDefault(1); - kI.initDefault(0); + kP.setDefault(1); + kI.setDefault(0); - barkP.initDefault(0); - barkV.initDefault(0); - barkG.initDefault(0); + barkP.setDefault(0); + barkV.setDefault(0); + barkG.setDefault(0); break; default: - kS.initDefault(0); - kG.initDefault(0.13); - kV.initDefault(0); - kA.initDefault(0); + kS.setDefault(0); + kG.setDefault(0.13); + kV.setDefault(0); + kA.setDefault(0); - kP.initDefault(15); - kI.initDefault(0); + kP.setDefault(15); + kI.setDefault(0); - barkP.initDefault(0); - barkV.initDefault(0); - barkG.initDefault(0); + barkP.setDefault(0); + barkV.setDefault(0); + barkG.setDefault(0); break; } @@ -149,7 +152,7 @@ public void setExtenderGoal(double setpoint) { } public void setPositionExtend(double position, double velocity) { - elevator.setPositionSetpoint(position, elevatorFFModel.calculate(velocity)); + elevator.setPositionSetpoint(position, elevatorFFModel.calculate(MetersPerSecond.of(velocity))); } public void elevatorStop() { @@ -190,11 +193,13 @@ public void periodic() { Logger.processInputs("Elevator", eInputs); Logger.recordOutput("amp bar currentPos", barCurrent.position); - if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode())) { - elevator.configurePID(kP.get(), kI.get(), 0); - } - if (barkP.hasChanged(hashCode()) - || barkV.hasChanged(hashCode()) - || barkG.hasChanged(hashCode())) {} - } -} + // if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode())) { + // elevator.configurePID(kP.get(), kI.get(), 0); + // } + // if (barkP.hasChanged(hashCode()) + // || barkV.hasChanged(hashCode()) + // || barkG.hasChanged(hashCode())) {} + // } + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 25757f1..ec88363 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -2,6 +2,8 @@ import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.units.measure.Voltage; + public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { @@ -16,7 +18,7 @@ public default void updateInputs(ElevatorIOInputs inputs) {} public default void runCharacterization(double volts) {} - public default void setPositionSetpoint(double position, double ffVolts) {} + public default void setPositionSetpoint(double position, Voltage ffVolts) {} public default void stop() {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 688a012..fc21c9f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -4,15 +4,29 @@ package frc.robot.subsystems.elevator; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.CurrentUnit; + import edu.wpi.first.wpilibj.simulation.ElevatorSim; import frc.robot.physicalConstants; /** Add your docs here. */ public class ElevatorIOSim implements ElevatorIO { +<<<<<<< Updated upstream // SIM VARIABLES (CHANGE) private int gearBoxMotorCount = 2; private int gearing = 1; @@ -34,13 +48,17 @@ public class ElevatorIOSim implements ElevatorIO { maxHeightMeters, simulateGravity, initialPositionMeters); +======= + private final DCMotor simGearbox = DCMotor.getKrakenX60Foc(2); + private ElevatorSim sim = new ElevatorSim(simGearbox, 1, 1, 0.01, 0.0, 3, true, 0.0); +>>>>>>> Stashed changes private PIDController pid = new PIDController(0, 0, 0); - private double positionInches = 0.0; - private double velocityInchPerSec = 0.0; - private double appliedVolts = 0.0; - private double currentAmps = 0.0; - private double positionSetpointInches = 0.0; + private Distance positionInches = Inches.of(0); + private LinearVelocity velocityInchPerSec = InchesPerSecond.of(0); + private Voltage appliedVolts = Volts.of(0.0); + private Current currentAmps = Amps.of(0.0); + private Distance positionSetpointInches = Inches.of(0); private double clampedValueLowVolts = -12.0; private double clampedValueHighVolts = 12.0; @@ -49,25 +67,37 @@ public class ElevatorIOSim implements ElevatorIO { @Override public void updateInputs(ElevatorIOInputs inputs) { - positionSetpointInches = pid.getSetpoint(); + positionSetpointInches = Inches.of(pid.getSetpoint()); +<<<<<<< Updated upstream appliedVolts += MathUtil.clamp( pid.calculate(sim.getPositionMeters() * metersToInches, positionSetpointInches), clampedValueLowVolts, clampedValueHighVolts); +======= + appliedVolts.plus( + Volts.of(MathUtil.clamp( + pid.calculate(Meters.of(sim.getPositionMeters()).in(Inches), positionSetpointInches.in(Inches)), -12.0, 12))); +>>>>>>> Stashed changes - sim.setInputVoltage(appliedVolts); + sim.setInputVoltage(appliedVolts.in(Volts)); +<<<<<<< Updated upstream positionInches = sim.getPositionMeters() * metersToInches; velocityInchPerSec = sim.getVelocityMetersPerSecond() * metersToInches; currentAmps = sim.getCurrentDrawAmps(); +======= + positionInches = Inches.of(Meters.of(sim.getPositionMeters()).in(Inches)); + velocityInchPerSec = InchesPerSecond.of(MetersPerSecond.of(sim.getVelocityMetersPerSecond()).in(MetersPerSecond)); + currentAmps = Amps.of(sim.getCurrentDrawAmps()); +>>>>>>> Stashed changes inputs.positionSetpoint = positionSetpointInches; - inputs.appliedVolts = appliedVolts; + inputs.appliedVolts = appliedVolts.in(Volts); inputs.elevatorPosition = positionInches; inputs.elevatorVelocity = velocityInchPerSec; - inputs.currentAmps = currentAmps; + inputs.currentAmps = currentAmps.in(Amps); sim.update(physicalConstants.LOOP_PERIOD_SECS); } @@ -78,15 +108,20 @@ public void runCharacterization(double volts) { } @Override - public void setPositionSetpoint(double positionInches, double ffVolts) { + public void setPositionSetpoint(double positionInches, Voltage ffVolts) { appliedVolts = ffVolts; pid.setSetpoint(positionInches); } @Override public void stop() { +<<<<<<< Updated upstream appliedVolts = 0; pid.setSetpoint(sim.getPositionMeters() * metersToInches); +======= + appliedVolts = Volts.of(0); + pid.setSetpoint(sim.getPositionMeters() * 39.37); +>>>>>>> Stashed changes } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index da49ce2..6eedbca 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -9,6 +9,8 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Voltage; import frc.robot.physicalConstants; import frc.robot.util.Conversions; @@ -72,7 +74,7 @@ public void runCharacterization(double volts) { } @Override - public void setPositionSetpoint(double position, double ffVolts) { + public void setPositionSetpoint(double position, Voltage ffVolts) { this.positionSetpoint = position; leader.setControl( new PositionVoltage( diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index a0e532c..0000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,165 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer; -import frc.robot.physicalConstants; -import frc.robot.util.LimelightHelpers; -import frc.robot.util.LimelightHelpers.PoseEstimate; -import frc.robot.util.LimelightHelpers.RawFiducial; -import org.littletonrobotics.junction.Logger; - -public class Vision extends SubsystemBase { - private static double multiplier = 1.0; - private static boolean toggle = false; - - private boolean overridePathplanner = false; - - // private NetworkTable limelightintake = - // NetworkTableInstance.getDefault().getTable(physicalConstants.LL_INTAKE); - - private final VisionIO visionIO; - private final VisionIOInputsAutoLogged visionInputs = new VisionIOInputsAutoLogged(); - - public Vision(VisionIO visionIO) { - this.visionIO = visionIO; - } - - @Override - public void periodic() { - visionIO.updateInputs(visionInputs); - LimelightHelpers.SetRobotOrientation( - physicalConstants.LL_ALIGN, - RobotContainer.drive.poseEstimator.getEstimatedPosition().getRotation().getDegrees(), - 0, - 0, - 0, - 0, - 0); - - if (DriverStation.getAlliance().isPresent() && visionInputs.aTV) { - Logger.recordOutput( - "tags > 1 or disabled ", visionInputs.tagCount > 1 || DriverStation.isDisabled()); - - if (visionInputs.tagCount > 1 || DriverStation.isDisabled()) { - visionLogic(); - } else { - // mt2TagFiltering(); - visionLogic(); - } - } - } - - public void mt2TagFiltering() { - boolean doRejectUpdate = false; - - LimelightHelpers.PoseEstimate mt2 = - new PoseEstimate( - visionInputs.mt2VisionPose, - visionInputs.timestampSeconds, - visionInputs.latency, - visionInputs.tagCount, - visionInputs.tagSpan, - visionInputs.avgTagDist, - visionInputs.avgTagArea, - new RawFiducial[] {}); - if (Math.abs(RobotContainer.drive.yawVelocityRadPerSec) > Math.toRadians(360)) { - doRejectUpdate = true; - } - - if (mt2.tagCount == 0) { - doRejectUpdate = true; - } - - if (mt2.pose.getTranslation().getDistance(new Translation2d(7.9, 4.1)) < 0.4) { - doRejectUpdate = true; - } - - if (!doRejectUpdate) { - RobotContainer.drive.poseEstimator.setVisionMeasurementStdDevs( - VecBuilder.fill(0.7, 0.7, Units.degreesToRadians(9999999))); - RobotContainer.drive.poseEstimator.addVisionMeasurement( - mt2.pose, mt2.timestampSeconds - (mt2.latency / 1000.)); - } - - Logger.recordOutput("Vision Measurement", mt2.pose); - Logger.recordOutput("Rejecting Tags", doRejectUpdate); - } - - public void visionLogic() { - LimelightHelpers.PoseEstimate limelightMeasurement = - new PoseEstimate( - visionInputs.mt1VisionPose, - visionInputs.timestampSeconds, - visionInputs.latency, - visionInputs.tagCount, - visionInputs.tagSpan, - visionInputs.avgTagDist, - visionInputs.avgTagArea, - new RawFiducial[] {}); - - double xMeterStds; - double yMeterStds; - double headingDegStds; - - double poseDifference = getVisionPoseDifference(limelightMeasurement.pose); - - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - - Logger.recordOutput("avg area", limelightMeasurement.avgTagArea); - - if (limelightMeasurement.tagCount >= 2 && limelightMeasurement.avgTagArea > 0.04) { - xMeterStds = 0.7; - yMeterStds = 0.7; - headingDegStds = 8; - } else if (limelightMeasurement.tagCount == 1 - && poseDifference < 0.5) { // && poseDifference < 0.5 - xMeterStds = 5; - yMeterStds = 5; - headingDegStds = 30; - } else if (limelightMeasurement.tagCount == 1 && poseDifference < 3) { // && poseDifference < 3 - xMeterStds = 11.43; - yMeterStds = 11.43; - headingDegStds = 9999; - } else return; - - Logger.recordOutput("number of tags", limelightMeasurement.tagCount); - - RobotContainer.drive.poseEstimator.setVisionMeasurementStdDevs( - VecBuilder.fill(xMeterStds, yMeterStds, Units.degreesToRadians(headingDegStds))); - - Pose2d pose = limelightMeasurement.pose; - - if (isFlipped) { - pose.getRotation().plus(new Rotation2d(Math.PI)); - } - - Logger.recordOutput("Vision Measurement", limelightMeasurement.pose); - } - - public double getVisionPoseDifference(Pose2d visionPose) { - return RobotContainer.drive.getPose().getTranslation().getDistance(visionPose.getTranslation()); - } - - public boolean acceptableMeasurements(Pose2d visionMeasurement) { - return Math.abs(visionMeasurement.getX() - RobotContainer.drive.getPose().getX()) < 1 - && Math.abs(visionMeasurement.getY() - RobotContainer.drive.getPose().getY()) < 1; - } - - public boolean canCorrect(Pose2d visionMeasurement, double timeSinceLastCorrection) { - if (timeSinceLastCorrection < 5) { - if (acceptableMeasurements(visionMeasurement)) return true; - } else { - return true; - } - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java deleted file mode 100644 index 714dfd1..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import org.littletonrobotics.junction.AutoLog; - -public interface VisionIO { - - @AutoLog - public static class VisionIOInputs { - public Pose2d mt2VisionPose = new Pose2d(); - public Pose2d mt1VisionPose = new Pose2d(); - public double timestampSeconds; - public int tagCount; - public double latency; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - - public double iTX; - public double iTY; - public double iTA; - public double iHB; - public boolean iTV; - - public double iTHOR; - public double iTVERT; - - public double iPIPELINELATENCY; - public double iCAPTURELATENCY; - - public double aTX; - public double aTY; - public double aTA; - public double aHB; - public boolean aTV; - - public double aTHOR; - public double aTVERT; - - public double aPIPELINELATENCY; - public double aCAPTURELATENCY; - } - - public default void updateInputs(VisionIOInputs inputs) {} -} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java deleted file mode 100644 index 312e732..0000000 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc.robot.util; - -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -/** Utility functions for flipping from the blue to red alliance. */ -public class AllianceFlipUtil { - /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ - public static double apply(double xCoordinate) { - if (shouldFlip()) { - return FieldConstants.fieldLength - xCoordinate; - } else { - return xCoordinate; - } - } - - /** Flips a translation to the correct side of the field based on the current alliance color. */ - public static Translation2d apply(Translation2d translation) { - if (shouldFlip()) { - return new Translation2d(apply(translation.getX()), translation.getY()); - } else { - return translation; - } - } - - /** Flips a rotation based on the current alliance color. */ - public static Rotation2d apply(Rotation2d rotation) { - if (shouldFlip()) { - return new Rotation2d(-rotation.getCos(), rotation.getSin()); - } else { - return rotation; - } - } - - /** Flips a pose to the correct side of the field based on the current alliance color. */ - public static Pose2d apply(Pose2d pose) { - if (shouldFlip()) { - return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); - } else { - return pose; - } - } - - public static Translation3d apply(Translation3d translation3d) { - if (shouldFlip()) { - return new Translation3d( - apply(translation3d.getX()), translation3d.getY(), translation3d.getZ()); - } else { - return translation3d; - } - } - - public static boolean shouldFlip() { - return DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - } -} diff --git a/src/main/java/frc/robot/util/Conversions.java b/src/main/java/frc/robot/util/Conversions.java deleted file mode 100644 index c128078..0000000 --- a/src/main/java/frc/robot/util/Conversions.java +++ /dev/null @@ -1,179 +0,0 @@ -package frc.robot.util; - -public class Conversions { - - /** - * @param positionCounts CANCoder Position Counts - * @param gearRatio Gear Ratio between CANCoder and Mechanism - * @return Degrees of Rotation of Mechanism - */ - public static double CANcoderToDegrees(double positionCounts, double gearRatio) { - return positionCounts * (360.0 / (gearRatio * 4096.0)); - } - /** - * @param radians - * @return degrees - */ - public static double radiansToDegrees(double radians) { - return radians * 180.0 / Math.PI; - } - /** - * @param degrees - * @return radians - */ - public static double degreesToRadians(double degrees) { - return degrees * Math.PI / 180.0; - } - /** - * @param degrees Degrees of rotation of Mechanism - * @param gearRatio Gear Ratio between CANCoder and Mechanism - * @return CANCoder Position Counts - */ - public static double degreesToCANcoder(double degrees, double gearRatio) { - return degrees / (360.0 / (gearRatio * 4096.0)); - } - - /** - * @param counts Falcon Position Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Degrees of Rotation of Mechanism - */ - public static double falconToDegrees(double positionCounts, double gearRatio) { - // return positionCounts * (360.0 / (gearRatio * 2048.0)); - return positionCounts * (360.0 / (gearRatio)); - } - - /** - * @param degrees Degrees of rotation of Mechanism - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Falcon Position Counts - */ - public static double degreesToFalcon(double degrees, double gearRatio) { - // return degrees / (360.0 / (gearRatio * 2048.0)); - return degrees / (360.0 / (gearRatio)); - } - - /** - * @param velocityCounts Falcon Velocity Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return RPM of Mechanism - */ - public static double falconToRPM(double velocityCounts, double gearRatio) { - double motorRPM = velocityCounts * (600.0 / 2048.0); - double mechRPM = motorRPM / gearRatio; - return mechRPM; - } - - /** - * @param RPM RPM of mechanism - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return RPM of Mechanism - */ - public static double RPMToFalcon(double RPM, double gearRatio) { - double motorRPM = RPM * gearRatio; - double sensorCounts = motorRPM * (2048.0 / 600.0); - return sensorCounts; - } - - /** - * @param velocitycounts Falcon Velocity Counts - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS) - * @return Falcon Velocity Counts - */ - public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) { - double wheelRPM = falconToRPM(velocitycounts, gearRatio); - double wheelMPS = (wheelRPM * circumference) / 60; - return wheelMPS; - } - - /** - * @param velocity Velocity MPS - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS) - * @return Falcon Velocity Counts - */ - public static double MPSToFalcon(double velocity, double circumference, double gearRatio) { - double wheelRPM = ((velocity * 60) / circumference); - double wheelVelocity = RPMToFalcon(wheelRPM, gearRatio); - return wheelVelocity; - } - - /** - * @param positionCounts Falcon Position Counts - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Wheel - * @return Meters - */ - public static double falconToMeters( - double positionCounts, double circumference, double gearRatio) { - return positionCounts * (circumference / (gearRatio * 2048.0)); - } - - /** - * @param meters Meters - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Wheel - * @return Falcon Position Counts - */ - public static double metersToFalcon(double meters, double circumference, double gearRatio) { - return meters / (circumference / (gearRatio * 2048.0)); - } - /** - * @param meters Meters - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Wheel - * @return Motor Rotations - */ - public static double MetersToMotorRot(double meters, double circumference, double gearRatio) { - return meters / (circumference / (gearRatio)); - } - /** - * @param inches Inches - * @param circumference Circumference of Wheel (Inches) - * @param gearRatio Gear Ratio between Falcon and Wheel - * @return Motor Rotations - */ - public static double inchesToMotorRot(double inches, double circumference, double gearRatio) { - return inches / (circumference / (gearRatio)); - } - - /** - * @param motorPositionRot Motor Position Inches - * @param circumference Circumference of Wheel (Inches) - * @param gearRatio Gear Ratio between Motor and Mechanism - * @return Position of Motor in Inches - */ - public static double motorRotToInches( - double motorPositionRot, double circumference, double gearRatio) { - return motorPositionRot * (circumference / (gearRatio)); - } - /** - * @param velocityInches Mechanism Velocity Inches - * @param circumference Circumference of Wheel (Inches) - * @param gearRatio Gear Ratio between Motor and Mechanism - * @return RPM of Motor - */ - public static double IPSToMotorRPM(double velocityIPS, double circumference, double gearRatio) { - return velocityIPS / (circumference / (gearRatio)) * 60.0; - } - /** - * @param velocityRPM Motor Velocity RPM - * @param circumference Circumference of Wheel (Inches) - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Mechanism Velocity in Inches Per Second - */ - public static double motorRPMToIPS(double velocityRPM, double circumference, double gearRatio) { - return velocityRPM * (circumference / (gearRatio)) / 60.0; - } - /** - * @param accelerationInchPerSecSec Mechanism Acceleration Inch Per Sec^2 - * @param circumference Circumference of Wheel (Inches) - * @param gearRatio Gear Ratio between Motor and Mechanism - * @return Motor Accelearation RPM Per Sec - */ - public static double IPSSToMotorRPMS( - double accelerationInchPerSecSec, double circumference, double gearRatio) { - return accelerationInchPerSecSec / (circumference / (gearRatio)) * 60.0; - } -} diff --git a/src/main/java/frc/robot/util/FeedForwardConstants.java b/src/main/java/frc/robot/util/FeedForwardConstants.java deleted file mode 100644 index 5066611..0000000 --- a/src/main/java/frc/robot/util/FeedForwardConstants.java +++ /dev/null @@ -1,3 +0,0 @@ -package frc.robot.util; - -public class FeedForwardConstants {} diff --git a/src/main/java/frc/robot/util/FieldConstants.java b/src/main/java/frc/robot/util/FieldConstants.java deleted file mode 100644 index 02cc40c..0000000 --- a/src/main/java/frc/robot/util/FieldConstants.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.util; - -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -import edu.wpi.first.math.util.Units; - -/** - * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets - * of corners start in the lower left moving clockwise. All units in Meters
- *
- * - *

All translations and poses are stored with the origin at the rightmost point on the BLUE - * ALLIANCE wall.
- *
- * Length refers to the x direction (as described by wpilib)
- * Width refers to the y direction (as described by wpilib) - */ -public class FieldConstants { - public static final double fieldLength = Units.inchesToMeters(651.223); - public static final double fieldWidth = Units.inchesToMeters(323.277); -} diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java deleted file mode 100644 index afa30a3..0000000 --- a/src/main/java/frc/robot/util/LimelightHelpers.java +++ /dev/null @@ -1,990 +0,0 @@ -// LimelightHelpers v1.5.0 (March 27, 2024) - -package frc.robot.util; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.concurrent.CompletableFuture; - -public class LimelightHelpers { - - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Barcode {} - - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() {} - } - - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Detector() {} - } - - public static class Results { - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public Results() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public String error; - - public LimelightResults() { - targetingResults = new Results(); - error = ""; - } - } - - public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; - - public RawFiducial( - int id, - double txnc, - double tync, - double ta, - double distToCamera, - double distToRobot, - double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - public RawFiducial[] rawFiducials; - - public PoseEstimate( - Pose2d pose, - double timestampSeconds, - double latency, - int tagCount, - double tagSpan, - double avgTagDist, - double avgTagArea, - RawFiducial[] rawFiducials) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - } - } - - private static ObjectMapper mapper; - - /** Print JSON Parse time to the console in milliseconds */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } - - private static Pose3d toPose3D(double[] inData) { - if (inData.length < 6) { - // System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d( - Units.degreesToRadians(inData[3]), - Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - private static Pose2d toPose2D(double[] inData) { - if (inData.length < 6) { - // System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - private static double extractBotPoseEntry(double[] inData, int position) { - if (inData.length < position + 1) { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); - var pose = toPose2D(poseArray); - double latency = extractBotPoseEntry(poseArray, 6); - int tagCount = (int) extractBotPoseEntry(poseArray, 7); - double tagSpan = extractBotPoseEntry(poseArray, 8); - double tagDist = extractBotPoseEntry(poseArray, 9); - double tagArea = extractBotPoseEntry(poseArray, 10); - // getlastchange() in microseconds, ll latency in milliseconds - var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for (int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate( - pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); - } - - private static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the BLUE alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the BLUE alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the RED alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the RED alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - } - - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** The LEDs will be controlled by Limelight pipeline settings, and not by robot code. */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - - /** - * Sets the crop window. The crop window in the UI must be completely open for dynamic cropping to - * work. - */ - public static void setCropWindow( - String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - public static void SetRobotOrientation( - String limelightName, - double yaw, - double yawRate, - double pitch, - double pitchRate, - double roll, - double rollRate) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - } - - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - public static void setCameraPose_RobotSpace( - String limelightName, - double forward, - double side, - double up, - double roll, - double pitch, - double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** Asynchronously take snapshot. */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync( - () -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** Parses Limelight's JSON results dump into a LimelightResults Object */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = - new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java deleted file mode 100644 index 5d584bf..0000000 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.robot.util; - -import frc.robot.physicalConstants; -import java.util.HashMap; -import java.util.Map; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; - -/** - * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or - * value not in dashboard. - */ -public class LoggedTunableNumber { - private static final String tableKey = "Tunables"; - - private final String key; - private boolean hasDefault = false; - private double defaultValue; - private LoggedDashboardNumber dashboardNumber; - private Map lastHasChangedValues = new HashMap<>(); - - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - */ - public LoggedTunableNumber(String dashboardKey) { - this.key = tableKey + "/" + dashboardKey; - // this.key = dashboardKey; - } - - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue) { - this(dashboardKey); - initDefault(defaultValue); - } - - /** - * Set the default value of the number. The default value can only be set once. - * - * @param defaultValue The default value - */ - public void initDefault(double defaultValue) { - if (!hasDefault) { - hasDefault = true; - this.defaultValue = defaultValue; - if (physicalConstants.tuningMode) { - dashboardNumber = new LoggedDashboardNumber(key, defaultValue); - } - } - } - - /** - * Get the current value, from dashboard if available and in tuning mode. - * - * @return The current value - */ - public double get() { - if (!hasDefault) { - return 0.0; - } else { - return physicalConstants.tuningMode ? dashboardNumber.get() : defaultValue; - } - } - - /** - * Checks whether the number has changed since our last check - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple - * objects. Recommended approach is to pass the result of "hashCode()" - * @return True if the number has changed since the last time this method was called, false - * otherwise. - */ - public boolean hasChanged(int id) { - double currentValue = get(); - Double lastValue = lastHasChangedValues.get(id); - if (lastValue == null || currentValue != lastValue) { - lastHasChangedValues.put(id, currentValue); - return true; - } - - return false; - } -} diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java new file mode 100644 index 0000000..869bf4e --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -0,0 +1,27 @@ +// 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.util; + +import com.ctre.phoenix6.StatusCode; +import java.util.function.Supplier; + +public class PhoenixUtil { + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 1f57c3b..69538ea 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,33 +1,23 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.1", + "version": "4.0.0-beta-1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", "javaDependencies": [ { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "3.2.1" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "3.2.1" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "3.2.1" + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.0.0-beta-1" } ], "jniDependencies": [ { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "3.2.1", + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.0.0-beta-1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..6e53566 --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,39 @@ +{ + "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.json b/vendordeps/PathplannerLib-beta.json similarity index 80% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-beta.json index f528a51..3c99918 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-beta.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.0.0-beta-5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.0.0-beta-5" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.0.0-beta-5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64", "osxuniversal", + "linuxx86-64", "linuxathena", "linuxarm32", "linuxarm64" diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json deleted file mode 100644 index adf6e02..0000000 --- a/vendordeps/Phoenix5.json +++ /dev/null @@ -1,151 +0,0 @@ -{ - "fileName": "Phoenix5.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-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.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.33.1" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.33.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-frc2025-beta-latest.json similarity index 79% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-frc2025-beta-latest.json index 1f0cf3a..cd9d153 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-frc2025-beta-latest.json @@ -1,76 +1,94 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-frc2025-beta-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, + "version": "25.0.0-beta-3", + "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.3.0" + "version": "25.0.0-beta-3" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.0.0-beta-3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", + "artifactId": "api-cpp-sim", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", + "artifactId": "tools-sim", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +138,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +152,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +182,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +190,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +198,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +206,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +214,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +222,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +238,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +246,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,21 +254,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -248,7 +262,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,6 +270,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -263,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -271,6 +286,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -278,7 +294,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -286,6 +302,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -293,7 +310,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -301,6 +318,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +334,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +342,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +350,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json deleted file mode 100644 index 9eda5ac..0000000 --- a/vendordeps/REVLib.json +++ /dev/null @@ -1,74 +0,0 @@ -{ - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2024.2.4", - "frcYear": "2024", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2024.2.4" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2024.2.4", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2024.2.4", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2024.2.4", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf389..3718e0a 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [