From dd9bcb0ba5e51154918a2c4369200f09743d6281 Mon Sep 17 00:00:00 2001 From: Wu-Fan-529 <3681247216@qq.com> Date: Sun, 24 May 2026 12:04:46 +0800 Subject: [PATCH 1/2] Migrate from Standard swerve --- .vscode/extensions.json | 3 +- .vscode/settings.json | 33 +- .wpilib/wpilib_preferences.json | 2 +- WPILib-License.md | 2 +- build.gradle | 11 +- settings.gradle | 6 +- src/main/deploy/choreo/NewPath.traj | 28 ++ src/main/deploy/choreo/_all_paths.chor | 84 ++++ src/main/deploy/elastic-layout.json | 228 +++++++++ src/main/java/frc/robot/Robot.java | 46 +- src/main/java/frc/robot/RobotContainer.java | 79 +-- .../robot/commands/AddVisionMeasurement.java | 29 +- .../java/frc/robot/commands/DriveManual.java | 84 +++- .../java/frc/robot/commands/PoseDrive.java | 34 +- .../java/frc/robot/commands/ResetPose.java | 42 ++ .../java/frc/robot/commands/states/None.java | 11 +- .../java/frc/robot/constants/ChoreoTraj.java | 59 +++ .../java/frc/robot/constants/ChoreoVars.java | 12 + .../frc/robot/constants/ConstDrivetrain.java | 69 +-- .../java/frc/robot/constants/ConstField.java | 50 +- .../java/frc/robot/constants/ConstMotion.java | 3 +- .../frc/robot/constants/ConstPoseDrive.java | 2 +- .../java/frc/robot/constants/ConstSystem.java | 3 +- .../java/frc/robot/constants/ConstVision.java | 34 +- .../robot/subsystems/DriverStateMachine.java | 15 +- .../java/frc/robot/subsystems/Drivetrain.java | 120 ++++- .../java/frc/robot/subsystems/RobotPoses.java | 20 +- .../java/frc/robot/subsystems/Rotors.java | 3 +- .../frc/robot/subsystems/StateMachine.java | 10 +- .../java/frc/robot/subsystems/Telemetry.java | 35 ++ .../java/frc/robot/subsystems/Vision.java | 108 ++++- vendordeps/ChoreoLib2026.json | 44 ++ vendordeps/PathplannerLib-2026.1.2.json | 38 ++ vendordeps/Phoenix6-26.2.0.json | 449 ++++++++++++++++++ vendordeps/SuperCORE.json | 6 +- vendordeps/WPILibNewCommands.json | 3 +- 36 files changed, 1558 insertions(+), 247 deletions(-) create mode 100644 src/main/deploy/choreo/NewPath.traj create mode 100644 src/main/deploy/choreo/_all_paths.chor create mode 100644 src/main/deploy/elastic-layout.json create mode 100644 src/main/java/frc/robot/commands/ResetPose.java create mode 100644 src/main/java/frc/robot/constants/ChoreoTraj.java create mode 100644 src/main/java/frc/robot/constants/ChoreoVars.java create mode 100644 src/main/java/frc/robot/subsystems/Telemetry.java create mode 100644 vendordeps/ChoreoLib2026.json create mode 100644 vendordeps/PathplannerLib-2026.1.2.json create mode 100644 vendordeps/Phoenix6-26.2.0.json diff --git a/.vscode/extensions.json b/.vscode/extensions.json index e6f5c26..5b6d19a 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -3,6 +3,7 @@ "bierner.markdown-mermaid", "tldraw-org.tldraw-vscode", "pkief.material-icon-theme", + "github.vscode-pull-request-github", "richardkotze.git-mob" ] -} +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index df59304..73b73d2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,4 @@ { - "extensions.ignoreRecommendations": false, "java.configuration.updateBuildConfiguration": "automatic", "java.server.launchMode": "Standard", "files.exclude": { @@ -29,10 +28,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "editor.formatOnSave": true, - "editor.tabSize": 2, - "editor.insertSpaces": true, - "editor.detectIndentation": false, "java.import.gradle.annotationProcessing.enabled": false, "java.completion.favoriteStaticMembers": [ "org.junit.Assert.*", @@ -64,7 +59,33 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], + "java.dependency.enableDependencyCheckup": false, + // CUSTOM - KEEP WHEN UPDATING + "extensions.ignoreRecommendations": false, + "editor.formatOnSave": true, + "editor.tabSize": 2, + "editor.insertSpaces": true, + "editor.detectIndentation": false, "git.pullBeforeCheckout": true, "java.saveActions.organizeImports": true, - "workbench.iconTheme": "material-icon-theme" + "workbench.iconTheme": "material-icon-theme", + "githubIssues.queries": [ + { + "label": "Issues", + "query": "is:open repo:${owner}/${repository}", + "groupBy": [ + "milestone" + ] + } + ], + "githubPullRequests.queries": [ + { + "label": "Ready PRs", + "query": "repo:${owner}/${repository} is:open draft:false" + }, + { + "label": "Draft PRs", + "query": "repo:${owner}/${repository} is:open draft:true" + } + ], } \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index a9c9e7a..f9f97e4 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025", + "projectYear": "2026", "teamNumber": 3255 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index e7cd597..9ecb965 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors +Copyright (c) 2009-2026 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index af308b3..7dc5472 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { @@ -43,11 +43,12 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava -// Set to true to use debug for JNI. +// Set to true to use debug for all targets including JNI, which will drastically impact +// performance. wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = true +def includeDesktopSupport = false // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -88,7 +89,9 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - from sourceSets.main.allSource + from('src') { into 'backup/src' } + from('vendordeps') { into 'backup/vendordeps' } + from('build.gradle') { into 'backup' } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/settings.gradle b/settings.gradle index 7cab49b..8fe3925 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -20,8 +20,8 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name 'frcHome' - url frcHomeMaven + name = 'frcHome' + url = frcHomeMaven } } } diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj new file mode 100644 index 0000000..c032e9d --- /dev/null +++ b/src/main/deploy/choreo/NewPath.traj @@ -0,0 +1,28 @@ +{ + "name":"NewPath", + "version":3, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":null, + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/_all_paths.chor b/src/main/deploy/choreo/_all_paths.chor new file mode 100644 index 0000000..7bc9d58 --- /dev/null +++ b/src/main/deploy/choreo/_all_paths.chor @@ -0,0 +1,84 @@ +{ + "name":"_all_paths", + "version":2, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[], + "codegen":{ + "root":"..\\..\\java\\frc\\robot\\constants", + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 0000000..f14ae16 --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,228 @@ +{ + "version": 1.0, + "grid_size": 34, + "tabs": [ + { + "name": "Disabled", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "FMSInfo", + "x": 1122.0, + "y": 0.0, + "width": 408.0, + "height": 272.0, + "type": "FMSInfo", + "properties": { + "topic": "/FMSInfo", + "period": 0.06 + } + }, + { + "title": "Field", + "x": 374.0, + "y": 272.0, + "width": 748.0, + "height": 272.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Right Limelight", + "x": 1122.0, + "y": 272.0, + "width": 408.0, + "height": 272.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-right", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Left Limelight", + "x": 0.0, + "y": 272.0, + "width": 374.0, + "height": 272.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-left", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Auto Chooser", + "x": 0.0, + "y": 0.0, + "width": 374.0, + "height": 272.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Active Auto", + "x": 374.0, + "y": 0.0, + "width": 748.0, + "height": 272.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/Auto Chooser/active", + "period": 0.06, + "data_type": "string" + } + } + ] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 748.0, + "y": 0.0, + "width": 782.0, + "height": 340.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Robot State", + "x": 510.0, + "y": 0.0, + "width": 238.0, + "height": 340.0, + "type": "Large Text Display", + "properties": { + "topic": "Robot/m_robotContainer/robotStateToString", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Driving State", + "x": 0.0, + "y": 170.0, + "width": 510.0, + "height": 170.0, + "type": "Large Text Display", + "properties": { + "topic": "Robot/m_robotContainer/driverStateToString", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Match Time", + "x": 0.0, + "y": 0.0, + "width": 510.0, + "height": 170.0, + "type": "Match Time", + "properties": { + "topic": "Robot/m_robotContainer/loggedTelemetryInstance/getMatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + } + ] + } + }, + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Robot State", + "x": 0.0, + "y": 0.0, + "width": 374.0, + "height": 578.0, + "type": "Large Text Display", + "properties": { + "topic": "Robot/m_robotContainer/robotStateToString", + "period": 0.6, + "data_type": "string" + } + }, + { + "title": "Match Time", + "x": 374.0, + "y": 0.0, + "width": 782.0, + "height": 170.0, + "type": "Match Time", + "properties": { + "topic": "Robot/m_robotContainer/loggedTelemetryInstance/getMatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Field", + "x": 374.0, + "y": 170.0, + "width": 782.0, + "height": 408.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5b8e811..dd45420 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,29 +6,32 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.net.WebServer; -import edu.wpi.first.units.measure.MutCurrent; -import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.constants.ConstField; import frc.robot.constants.ConstSystem; -import edu.wpi.first.cameraserver.CameraServer; +import frc.robot.constants.ConstVision; +import com.frcteam3255.utils.LimelightHelpers; @Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private final StringTopic selectedTabTopic = NetworkTableInstance.getDefault() + .getStringTopic("/Elastic/SelectedTab"); + private final StringPublisher selectedTabPublisher = selectedTabTopic + .publish(PubSubOption.keepDuplicates(true)); @Override public void robotInit() { @@ -49,6 +52,10 @@ public void robotInit() { DriverStation.silenceJoystickConnectionWarning(ConstSystem.constControllers.SILENCE_JOYSTICK_WARNINGS); } + public void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); @@ -56,24 +63,46 @@ public void robotPeriodic() { @Override public void disabledInit() { + selectTab("Disabled"); + LimelightHelpers.SetIMUAssistAlpha(ConstVision.LIMELIGHT_RIGHT_NAME, ConstVision.IMU_ASSIST_ALPHA_VALUE); + LimelightHelpers.SetIMUAssistAlpha(ConstVision.LIMELIGHT_LEFT_NAME, ConstVision.IMU_ASSIST_ALPHA_VALUE); + LimelightHelpers.SetIMUAssistAlpha(ConstVision.LIMELIGHT_BACK_NAME, ConstVision.IMU_ASSIST_ALPHA_VALUE); + LimelightHelpers.SetThrottle(ConstVision.LIMELIGHT_RIGHT_NAME, ConstVision.DisabledThrottle); + LimelightHelpers.SetThrottle(ConstVision.LIMELIGHT_LEFT_NAME, ConstVision.DisabledThrottle); + LimelightHelpers.SetThrottle(ConstVision.LIMELIGHT_BACK_NAME, ConstVision.DisabledThrottle); + m_robotContainer.visionInstance.setIMUAssistMode(false); } @Override public void disabledPeriodic() { ConstField.ALLIANCE = DriverStation.getAlliance(); SmartDashboard.putString("ALLIANCE", ConstField.ALLIANCE.toString()); + double yaw = m_robotContainer.drivetrainInstance.getPose().getRotation().getDegrees(); + LimelightHelpers.SetRobotOrientation(ConstVision.LIMELIGHT_RIGHT_NAME, + yaw, 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(ConstVision.LIMELIGHT_LEFT_NAME, + yaw, 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(ConstVision.LIMELIGHT_BACK_NAME, + yaw, 0, 0, 0, 0, 0); + } @Override public void disabledExit() { + LimelightHelpers.SetThrottle(ConstVision.LIMELIGHT_RIGHT_NAME, ConstVision.TeleopThrottle); + LimelightHelpers.SetThrottle(ConstVision.LIMELIGHT_LEFT_NAME, ConstVision.TeleopThrottle); + LimelightHelpers.SetThrottle(ConstVision.LIMELIGHT_BACK_NAME, ConstVision.TeleopThrottle); + m_robotContainer.visionInstance.setIMUAssistMode(true); + m_robotContainer.addVisionMeasurement().schedule(); } @Override public void autonomousInit() { + selectTab("Autonomous"); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + CommandScheduler.getInstance().schedule(m_autonomousCommand); } } @@ -87,6 +116,7 @@ public void autonomousExit() { @Override public void teleopInit() { + selectTab("Teleoperated"); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ab5e1c..e0a0dd9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.DeferredCommand; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -33,6 +34,7 @@ import frc.robot.subsystems.Rotors; import frc.robot.subsystems.StateMachine; import frc.robot.subsystems.StateMachine.RobotState; +import frc.robot.subsystems.Telemetry; import frc.robot.subsystems.Vision; @Logged @@ -47,58 +49,58 @@ public class RobotContainer { public static final Rotors rotorsInstance = new Rotors(); public static final Motion motionInstance = new Motion(); private final Rotors loggedRotorsInstance = rotorsInstance; + public static final Motion motionInstance = new Motion(); private final Motion loggedMotionInstance = motionInstance; - public static final Drivetrain subDrivetrain = new Drivetrain(); - private final Drivetrain loggedSubDrivetrain = subDrivetrain; - public static final DriverStateMachine subDriverStateMachine = new DriverStateMachine(subDrivetrain); - private final DriverStateMachine loggedSubDriverStateMachine = subDriverStateMachine; - public static final StateMachine subStateMachine = new StateMachine(subDrivetrain); - private final StateMachine loggedSubStateMachine = subStateMachine; - public static final RobotPoses robotPose = new RobotPoses(subDrivetrain); + public static final Drivetrain drivetrainInstance = new Drivetrain(); + private final Drivetrain loggedDrivetrainInstance = drivetrainInstance; + public static final DriverStateMachine driverStateMachineInstance = new DriverStateMachine(); + private final DriverStateMachine loggedDriverStateMachineInstance = driverStateMachineInstance; + public static final StateMachine stateMachineInstance = new StateMachine(); + private final StateMachine loggedStateMachineInstance = stateMachineInstance; + public static final RobotPoses robotPose = new RobotPoses(); private final RobotPoses loggedRobotPose = robotPose; - public static final Vision subVision = new Vision(); - private final Vision loggedSubVision = subVision; public static final Intaking intakingInstance = new Intaking(); public static final RetractIntake RetractingInstance = new RetractIntake(); + public static final Vision visionInstance = new Vision(); + private final Vision loggedVisionInstance = visionInstance; + public static final Telemetry telemetryInstance = new Telemetry(); + private final Telemetry loggedTelemetryInstance = telemetryInstance; + Command TRY_NONE = Commands.deferredProxy( - () -> subStateMachine.tryState(RobotState.NONE)); + () -> stateMachineInstance.tryState(RobotState.NONE)); Command MANUAL = new DeferredCommand( - subDriverStateMachine.tryState( + driverStateMachineInstance.tryState( DriverStateMachine.DriverState.MANUAL, conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX, conDriver.btn_RightBumper), - Set.of(subDriverStateMachine)); + Set.of(driverStateMachineInstance)); Command EXAMPLE_POSE_DRIVE = new DeferredCommand( - subDriverStateMachine.tryState( + driverStateMachineInstance.tryState( DriverStateMachine.DriverState.EXAMPLE_POSE_DRIVE, conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX, conDriver.btn_RightBumper), - Set.of(subDriverStateMachine)); + Set.of(driverStateMachineInstance)); public RobotContainer() { conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND); - subDriverStateMachine + driverStateMachineInstance .setDefaultCommand(MANUAL); configDriverBindings(); configOperatorBindings(); configAutonomous(); - - // subDrivetrain.resetModulesToAbsolute(); + RobotController.setBrownoutVoltage(5.5); + // drivetrainInstance.resetModulesToAbsolute(); } private void configDriverBindings() { - // conDriver.btn_B.onTrue(Commands.runOnce(() -> - // subDrivetrain.resetModulesToAbsolute())); - conDriver.btn_Back - .onTrue(Commands.runOnce(() -> subDrivetrain.resetPose(new Pose2d(0, 0, new Rotation2d())))); conDriver.btn_RightTrigger .whileTrue(intakingInstance); conDriver.btn_LeftTrigger @@ -106,7 +108,7 @@ private void configDriverBindings() { // Example Pose Drive conDriver.btn_X .whileTrue(EXAMPLE_POSE_DRIVE) - .onFalse(Commands.runOnce(() -> subDriverStateMachine.setDriverState(DriverState.MANUAL))); + .onFalse(Commands.runOnce(() -> driverStateMachineInstance.setDriverState(DriverState.MANUAL))); } private void configOperatorBindings() { @@ -115,11 +117,11 @@ private void configOperatorBindings() { public void configAutonomous() { autoFactory = new AutoFactory( - subDrivetrain::getPose, // A function that returns the current robot pose - subDrivetrain::resetPose, // A function that resets the current robot pose to the provided Pose2d - subDrivetrain::followTrajectory, // The drive subsystem trajectory follower + drivetrainInstance::getPose, // A function that returns the current robot pose + drivetrainInstance::resetPose, // A function that resets the current robot pose to the provided Pose2d + drivetrainInstance::followTrajectory, // The drive subsystem trajectory follower true, // If alliance flipping should be enabled - subDriverStateMachine // The drive subsystem + driverStateMachineInstance // The drive subsystem ); // make our entries name @@ -131,10 +133,9 @@ public void configAutonomous() { autoChooser.onChange(selectedAuto -> { String startingPose = autoStartingPoses.get(selectedAuto); // if there is a stating pose, reset to it - if (startingPose != null) { - autoFactory.resetOdometry(startingPose) - .ignoringDisable(true) // Run even when disabled - .schedule(); + if (startingPose != null) { // Run even when disabled + CommandScheduler.getInstance().schedule(autoFactory.resetOdometry(startingPose) + .ignoringDisable(true)); } }); @@ -149,7 +150,7 @@ public void configAutonomous() { public Command runPath(String pathName) { return autoFactory.trajectoryCmd(pathName).asProxy() - .alongWith(Commands.runOnce(() -> subDriverStateMachine.setDriverState(DriverState.CHOREO))); + .alongWith(Commands.runOnce(() -> driverStateMachineInstance.setDriverState(DriverState.CHOREO))); } public Command getAutonomousCommand() { @@ -157,11 +158,23 @@ public Command getAutonomousCommand() { } public RobotState getRobotState() { - return subStateMachine.getRobotState(); + return stateMachineInstance.getRobotState(); + } + + public String robotStateToString() { + return stateMachineInstance.getRobotState().toString(); + } + + public String driverStateToString() { + return driverStateMachineInstance.getDriverState().toString(); } public Command addVisionMeasurement() { - return new AddVisionMeasurement(subDrivetrain, subVision) + return new AddVisionMeasurement() .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).ignoringDisable(true); } + + public static boolean isPracticeBot() { + return RobotController.getSerialNumber().equals(ConstSystem.PRACTICE_BOT_RIO); + } } diff --git a/src/main/java/frc/robot/commands/AddVisionMeasurement.java b/src/main/java/frc/robot/commands/AddVisionMeasurement.java index fed3919..f403ae9 100644 --- a/src/main/java/frc/robot/commands/AddVisionMeasurement.java +++ b/src/main/java/frc/robot/commands/AddVisionMeasurement.java @@ -9,23 +9,17 @@ import com.frcteam3255.utils.LimelightHelpers; import com.frcteam3255.utils.LimelightHelpers.PoseEstimate; +import frc.robot.RobotContainer; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.ConstVision; -import frc.robot.subsystems.Drivetrain; -import frc.robot.subsystems.Vision; public class AddVisionMeasurement extends Command { - Drivetrain subDrivetrain; - Vision subVision; Optional estimatedPose; - public AddVisionMeasurement(Drivetrain subDrivetrain, Vision subVision) { - this.subDrivetrain = subDrivetrain; - this.subVision = subVision; - - addRequirements(subVision); + public AddVisionMeasurement() { + addRequirements(RobotContainer.visionInstance); } @Override @@ -35,17 +29,18 @@ public void initialize() { @Override public void execute() { // Tells the limelight where we are on the field + double yaw = RobotContainer.drivetrainInstance.getPose().getRotation().getDegrees(); LimelightHelpers.SetRobotOrientation(ConstVision.LIMELIGHT_RIGHT_NAME, - subDrivetrain.getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); + yaw, 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation(ConstVision.LIMELIGHT_LEFT_NAME, - subDrivetrain.getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); + yaw, 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation(ConstVision.LIMELIGHT_BACK_NAME, - subDrivetrain.getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); - AngularVelocity gyroRate = subDrivetrain.getGyroRate(); - - estimatedPose = subVision.determinePoseEstimate(gyroRate); - if (estimatedPose.isPresent()) { - subDrivetrain.addVisionMeasurement(estimatedPose.get().pose, estimatedPose.get().timestampSeconds); + yaw, 0, 0, 0, 0, 0); + AngularVelocity gyroRate = RobotContainer.drivetrainInstance.getGyroRate(); + estimatedPose = RobotContainer.visionInstance.determinePoseEstimate(gyroRate); + if (estimatedPose.isPresent() && RobotContainer.visionInstance.isVisionEnabled()) { + RobotContainer.drivetrainInstance.addVisionMeasurement(estimatedPose.get().pose, + estimatedPose.get().timestampSeconds); } } diff --git a/src/main/java/frc/robot/commands/DriveManual.java b/src/main/java/frc/robot/commands/DriveManual.java index 57d25d2..6d0249b 100644 --- a/src/main/java/frc/robot/commands/DriveManual.java +++ b/src/main/java/frc/robot/commands/DriveManual.java @@ -8,31 +8,31 @@ import java.util.function.DoubleSupplier; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.constants.ConstDrivetrain; import frc.robot.constants.ConstField; -import frc.robot.subsystems.DriverStateMachine; +import frc.robot.subsystems.DriverStateMachine.DriverState; import frc.robot.subsystems.Drivetrain; public class DriveManual extends Command { - Drivetrain subDrivetrain; - DoubleSupplier xAxis, yAxis, rotationAxis; + DoubleSupplier xAxis, yAxis, rotationXAxis; boolean isOpenLoop; - DriverStateMachine subDriverStateMachine; BooleanSupplier slowMode; + Timer delayTimer = new Timer(); - public DriveManual(Drivetrain subDrivetrain, DoubleSupplier xAxis, DoubleSupplier yAxis, - DoubleSupplier rotationAxis, DriverStateMachine subDriverStateMachine, BooleanSupplier slowMode) { - this.subDrivetrain = subDrivetrain; - this.subDriverStateMachine = subDriverStateMachine; + public DriveManual(DoubleSupplier xAxis, DoubleSupplier yAxis, + DoubleSupplier rotationAxis, BooleanSupplier slowMode) { this.xAxis = xAxis; this.yAxis = yAxis; - this.rotationAxis = rotationAxis; + this.rotationXAxis = rotationAxis; this.slowMode = slowMode; isOpenLoop = true; - addRequirements(this.subDrivetrain); + addRequirements(RobotContainer.driverStateMachineInstance); } @Override @@ -41,27 +41,81 @@ public void initialize() { @Override public void execute() { - ChassisSpeeds velocities = subDrivetrain.calculateVelocitiesFromInput( + RobotContainer.driverStateMachineInstance.setDriverState(DriverState.MANUAL); + ChassisSpeeds velocities = RobotContainer.drivetrainInstance.calculateVelocitiesFromInput( xAxis, yAxis, - rotationAxis, + rotationXAxis, slowMode, ConstField.isRedAlliance(), ConstDrivetrain.SLOW_MODE_MULTIPLIER, ConstDrivetrain.REAL_DRIVE_SPEED, ConstDrivetrain.TURN_SPEED); - subDriverStateMachine.setDriverState(DriverStateMachine.DriverState.MANUAL); + if (DriverStation.isAutonomousEnabled()) { + driveWithTargetRotation(velocities); + return; + } - subDrivetrain.drive(velocities); + driveWithSticks(velocities); + updateXbrake(); + } + + private void driveWithSticks(ChassisSpeeds velocities) { + boolean isRotateStickHit = RobotContainer.drivetrainInstance.isStickHit(rotationXAxis, + ConstDrivetrain.ROTATION_STICK_DEADBAND); + + if (isRotateStickHit) { + manualRotation(velocities); + } else { + correctRotation(velocities); + } + } + + private void manualRotation(ChassisSpeeds velocities) { + RobotContainer.drivetrainInstance.setIsManualRotationEnabled(true); + RobotContainer.drivetrainInstance.drive(velocities); + RobotContainer.drivetrainInstance + .setDriveRotation(RobotContainer.drivetrainInstance.getPose().getRotation().getMeasure()); + delayTimer.reset(); + } + + private void correctRotation(ChassisSpeeds velocities) { + delayTimer.start(); + boolean delayElapsed = delayTimer.hasElapsed(ConstDrivetrain.ROTATION_DELAY.magnitude()); + + if (delayElapsed) { + driveWithTargetRotation(velocities); + } else { + RobotContainer.drivetrainInstance.drive(velocities); + RobotContainer.drivetrainInstance + .setDriveRotation(RobotContainer.drivetrainInstance.getPose().getRotation().getMeasure()); + } + } + + private void driveWithTargetRotation(ChassisSpeeds velocities) { + RobotContainer.drivetrainInstance.drive( + velocities, + RobotContainer.drivetrainInstance.getTargetRotation(), + ConstDrivetrain.ROTATION_PID.kP, + ConstDrivetrain.ROTATION_PID.kI, + ConstDrivetrain.ROTATION_PID.kD); + } + + private void updateXbrake() { + boolean isStickHit = RobotContainer.drivetrainInstance.isStickHit(xAxis, yAxis, + ConstDrivetrain.ROTATION_STICK_DEADBAND) + || RobotContainer.drivetrainInstance.isStickHit(rotationXAxis, ConstDrivetrain.ROTATION_STICK_DEADBAND); + RobotContainer.drivetrainInstance.setXbrakeAllowed(!isStickHit); } @Override public void end(boolean interrupted) { + delayTimer.stop(); } @Override public boolean isFinished() { return false; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/PoseDrive.java b/src/main/java/frc/robot/commands/PoseDrive.java index 9fef923..ec54e94 100644 --- a/src/main/java/frc/robot/commands/PoseDrive.java +++ b/src/main/java/frc/robot/commands/PoseDrive.java @@ -10,35 +10,31 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.constants.ConstDrivetrain; import frc.robot.constants.ConstField; import frc.robot.constants.ConstPoseDrive.PoseDriveGroup; -import frc.robot.subsystems.DriverStateMachine; -import frc.robot.subsystems.Drivetrain; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class PoseDrive extends Command { /** Creates a new PoseDrive. */ - Drivetrain subDrivetrain; - DriverStateMachine subDriverStateMachine; + DoubleSupplier xAxis, yAxis, rotationAxis; BooleanSupplier slowMode; PoseDriveGroup poseGroup; Pose2d closestPose; public boolean isPoseAligned = false; - public PoseDrive(Drivetrain subDrivetrain, DriverStateMachine subDriverStateMachine, + public PoseDrive( DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier rotationAxis, BooleanSupplier slowMode, PoseDriveGroup poseGroup) { // Use addRequirements() here to declare subsystem dependencies. - this.subDrivetrain = subDrivetrain; - this.subDriverStateMachine = subDriverStateMachine; this.xAxis = xAxis; this.yAxis = yAxis; this.rotationAxis = rotationAxis; this.poseGroup = poseGroup; this.slowMode = slowMode; - addRequirements(this.subDriverStateMachine); + addRequirements(RobotContainer.driverStateMachineInstance); } // Called when the command is initially scheduled. @@ -49,11 +45,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - closestPose = subDrivetrain.getPose().nearest(poseGroup.targetPoseGroup); - subDrivetrain.lastDesiredPoseGroup = poseGroup; - subDrivetrain.lastDesiredTarget = closestPose; + closestPose = RobotContainer.drivetrainInstance.getPose().nearest(poseGroup.targetPoseGroup); + RobotContainer.drivetrainInstance.lastDesiredPoseGroup = poseGroup; + RobotContainer.drivetrainInstance.lastDesiredTarget = closestPose; - ChassisSpeeds velocities = subDrivetrain.calculateVelocitiesFromInput( + ChassisSpeeds velocities = RobotContainer.drivetrainInstance.calculateVelocitiesFromInput( xAxis, yAxis, rotationAxis, @@ -63,22 +59,22 @@ public void execute() { ConstDrivetrain.REAL_DRIVE_SPEED, ConstDrivetrain.TURN_SPEED); - boolean isInAutoDriveZone = subDrivetrain.isInAutoDriveZone( + boolean isInAutoDriveZone = RobotContainer.drivetrainInstance.isInAutoDriveZone( poseGroup.minDistanceBeforeDrive, closestPose); if (isInAutoDriveZone) { - subDrivetrain.autoAlign( + RobotContainer.drivetrainInstance.autoAlign( closestPose, velocities, poseGroup.lockX, poseGroup.lockY); - subDriverStateMachine.setDriverState(poseGroup.driveState); + RobotContainer.driverStateMachineInstance.setDriverState(poseGroup.driveState); } else { - subDrivetrain.rotationalAlign( + RobotContainer.drivetrainInstance.rotationalAlign( closestPose, velocities); - subDriverStateMachine.setDriverState(poseGroup.snapState); + RobotContainer.driverStateMachineInstance.setDriverState(poseGroup.snapState); } } @@ -93,8 +89,8 @@ public boolean isFinished() { if (closestPose == null) { return false; } - isPoseAligned = subDrivetrain.isAtPosition(closestPose, poseGroup.distanceTolerance) && - subDrivetrain.isAtRotation(closestPose.getRotation(), poseGroup.rotationTolerance); + isPoseAligned = RobotContainer.drivetrainInstance.isAtPosition(closestPose, poseGroup.distanceTolerance) && + RobotContainer.drivetrainInstance.isAtPosition(closestPose.getRotation(), poseGroup.rotationTolerance); return isPoseAligned; } } diff --git a/src/main/java/frc/robot/commands/ResetPose.java b/src/main/java/frc/robot/commands/ResetPose.java new file mode 100644 index 0000000..1dffe6b --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetPose.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstField; + +public class ResetPose extends Command { + + public ResetPose() { + addRequirements(RobotContainer.driverStateMachineInstance); + } + + @Override + public void initialize() { + RobotContainer.visionInstance.setVisionEnabled(false); + RobotContainer.drivetrainInstance.resetPoseAndYaw(getAlliancePose()); + } + + @Override + public void execute() { + RobotContainer.visionInstance.setIMUAssistMode(false); + } + + @Override + public void end(boolean interrupted) { + RobotContainer.visionInstance.setIMUAssistMode(true); + } + + @Override + public boolean isFinished() { + return false; + } + + public Pose2d getAlliancePose() { + return ConstField.FieldElementGroups.RESET_POSE_SET.getAlliancePoses().get(0); + } +} diff --git a/src/main/java/frc/robot/commands/states/None.java b/src/main/java/frc/robot/commands/states/None.java index 6864bfc..bc51739 100644 --- a/src/main/java/frc/robot/commands/states/None.java +++ b/src/main/java/frc/robot/commands/states/None.java @@ -5,24 +5,23 @@ package frc.robot.commands.states; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.subsystems.*; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class None extends Command { /** Creates a new None. */ - StateMachine globalStateMachine; - - public None(StateMachine globalStateMachine) { + public None() { // Use addRequirements() here to declare subsystem dependencies. - this.globalStateMachine = globalStateMachine; - addRequirements(globalStateMachine); + + addRequirements(RobotContainer.stateMachineInstance); } // Called when the command is initially scheduled. @Override public void initialize() { - globalStateMachine.setRobotState(StateMachine.RobotState.NONE); + RobotContainer.stateMachineInstance.setRobotState(StateMachine.RobotState.NONE); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/constants/ChoreoTraj.java b/src/main/java/frc/robot/constants/ChoreoTraj.java new file mode 100644 index 0000000..87cd3e7 --- /dev/null +++ b/src/main/java/frc/robot/constants/ChoreoTraj.java @@ -0,0 +1,59 @@ + +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import java.util.Map; +import java.util.OptionalInt; + +// If these imports cause errors because you're not using ChoreoLib, +// turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; + +/** + * A class containing the name, start pose, end pose, and total time of every + * Choreo trajectory. + * This prevents your code from referencing deleted or misspelled trajectories, + * and removes the need for JSON parsing to load a trajectory's essential data. + * DO NOT MODIFY THIS FILE YOURSELF! It is automatically generated by Choreo. + */ +public record ChoreoTraj( + String name, + OptionalInt segment, + double totalTimeSecs, + Pose2d initialPoseBlue, + Pose2d endPoseBlue) { + + /** + * A map between trajectory names and their corresponding data. + * This allows for trajectory data to be looked up with strings during runtime. + */ + public static final Map ALL_TRAJECTORIES = Map.ofEntries( + + ); + + /** + * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. + * WARNING: will raise an exception if not called with a valid segment index. + */ + public ChoreoTraj segment(int segment) { + var traj = ChoreoTraj.ALL_TRAJECTORIES.get(this.name + "$" + segment); + if (traj == null) { + throw new NullPointerException("Trajectory " + this.name + " does not have segment #" + segment + "."); + } + return traj; + } + + // If these methods cause errors because you're not using ChoreoLib, + // turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. + /** + * Load an AutoTrajectory directly from a ChoreoTraj, which may be a segment of + * a larger trajectory. + */ + public AutoTrajectory asAutoTraj(AutoRoutine routine) { + if (this.segment.isPresent()) { + return routine.trajectory(this.name, this.segment.getAsInt()); + } + return routine.trajectory(this.name); + } +} diff --git a/src/main/java/frc/robot/constants/ChoreoVars.java b/src/main/java/frc/robot/constants/ChoreoVars.java new file mode 100644 index 0000000..a9e113d --- /dev/null +++ b/src/main/java/frc/robot/constants/ChoreoVars.java @@ -0,0 +1,12 @@ +package frc.robot.constants; + +/** + * Generated file containing variables defined in Choreo. + * DO NOT MODIFY THIS FILE YOURSELF; instead, change these values + * in the Choreo GUI. + */ +public final class ChoreoVars { + + private ChoreoVars() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ConstDrivetrain.java b/src/main/java/frc/robot/constants/ConstDrivetrain.java index 14d5169..88e316a 100644 --- a/src/main/java/frc/robot/constants/ConstDrivetrain.java +++ b/src/main/java/frc/robot/constants/ConstDrivetrain.java @@ -9,6 +9,7 @@ import static edu.wpi.first.units.Units.KilogramSquareMeters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -18,6 +19,8 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.frcteam3255.components.swerve.SN_SwerveConstants.ModuleLocations; +import com.frcteam3255.components.swerve.SN_SwerveConstants.Ratios; import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; @@ -30,6 +33,7 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; /** @@ -48,60 +52,22 @@ * easier to update configuration values in a single location. */ public class ConstDrivetrain { + public static final double ROTATION_STICK_DEADBAND = 0.05; + public static final Time ROTATION_DELAY = Seconds.of(0.5); + // TODO: Swoffsets public static final Angle FRONT_LEFT_ABS_ENCODER_OFFSET = Rotations.of(-0.178466796875); public static final Angle FRONT_RIGHT_ABS_ENCODER_OFFSET = Rotations.of(-0.498779296875); public static final Angle BACK_LEFT_ABS_ENCODER_OFFSET = Rotations.of(-0.459716796875); public static final Angle BACK_RIGHT_ABS_ENCODER_OFFSET = Rotations.of(-0.31201171875); - // ====== TO MOVE TO SUPERCODE - START ====== - public static class ModuleLocations { - public static final Distance frame25x25 = Inches.of(19.75).div(2); - public static final Distance frame29x29 = Inches.of(23.75).div(2); - } - - public static class Ratios { - private final double steer; - private final double drive; - private final double couple; - - public Ratios(double steer, double drive, double couple) { - this.steer = steer; - this.drive = drive; - this.couple = couple; - } - - public double getSteer() { - return steer; - } - - public double getDrive() { - return drive; - } - - public double getCouple() { - return couple; - } - - public static class MK4I { - private static final double stage1 = 1. / (14. / 50.); - private static final double stage2L1 = 1. / (25. / 19.); - private static final double stage2L2 = 1. / (27. / 17.); - private static final double stage2L3 = 1. / (28. / 16.); - private static final double stage3 = 1. / (15. / 45.); - private static final double steer = 150. / 7.; - - private static final double driveL1 = stage1 * stage2L1 * stage3; - private static final double driveL2 = stage1 * stage2L2 * stage3; - private static final double driveL3 = stage1 * stage2L3 * stage3; - private static final double couple = stage1; - - public static final Ratios L1 = new Ratios(steer, driveL1, couple); - public static final Ratios L2 = new Ratios(steer, driveL2, couple); - public static final Ratios L3 = new Ratios(steer, driveL3, couple); - } + public static class PRACTICE_BOT { + // TODO: Swoffsets + public static final Angle FRONT_LEFT_ABS_ENCODER_OFFSET = Rotations.of(-0.199462890625); + public static final Angle FRONT_RIGHT_ABS_ENCODER_OFFSET = Rotations.of(0.474365234375); + public static final Angle BACK_LEFT_ABS_ENCODER_OFFSET = Rotations.of(-0.447265625); + public static final Angle BACK_RIGHT_ABS_ENCODER_OFFSET = Rotations.of(-0.3193359375); } - // ====== TO MOVE TO SUPERCODE - END ====== public static final double SLOW_MODE_MULTIPLIER = 0.5; @@ -243,4 +209,13 @@ public static class SIMULATION { public static final Voltage kDriveFrictionVoltage = Volts.of(0.2); } + public static class ROTATION_PID { + public static final double kP = 5.0; + public static final double kI = 0.0; + public static final double kD = 0; + } + + public static double isStickHitHighTol = 1.15; + public static double isStickHitLowTol = 0.15; + } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ConstField.java b/src/main/java/frc/robot/constants/ConstField.java index ff59cbc..e7d3c50 100644 --- a/src/main/java/frc/robot/constants/ConstField.java +++ b/src/main/java/frc/robot/constants/ConstField.java @@ -15,16 +15,23 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; /** - * Provides constants and utilities for field dimensions and alliance-specific pose management. + * Provides constants and utilities for field dimensions and alliance-specific + * pose management. *

- * This class defines the field size and contains logic for handling poses that need to be mirrored - * between the blue and red alliances. The {@link Pose2dAllianceSet} inner class allows you to define - * a set of poses for the blue alliance and automatically generates the corresponding mirrored poses + * This class defines the field size and contains logic for handling poses that + * need to be mirrored + * between the blue and red alliances. The {@link Pose2dAllianceSet} inner class + * allows you to define + * a set of poses for the blue alliance and automatically generates the + * corresponding mirrored poses * for the red alliance, as well as a combined set for both alliances. *

- * The field mirroring functionality is essential for autonomous routines and path planning, ensuring - * that robot positions and trajectories can be easily adapted for both sides of the field. The - * {@link #isRedAlliance()} method can be used to determine the current alliance and select the + * The field mirroring functionality is essential for autonomous routines and + * path planning, ensuring + * that robot positions and trajectories can be easily adapted for both sides of + * the field. The + * {@link #isRedAlliance()} method can be used to determine the current alliance + * and select the * appropriate pose set. */ public class ConstField { @@ -32,13 +39,18 @@ public class ConstField { // Wrapper for blue-side Pose2d arrays with helpers for red/all /** - * Represents a set of {@link Pose2d} objects for alliance-specific field positions. + * Represents a set of {@link Pose2d} objects for alliance-specific field + * positions. *

- * This class stores a list of blue-side poses and automatically computes the corresponding - * red-side poses by mirroring them across the field. It provides access to the blue, red, - * and combined lists of poses, allowing code to easily retrieve poses for the current alliance. + * This class stores a list of blue-side poses and automatically computes the + * corresponding + * red-side poses by mirroring them across the field. It provides access to the + * blue, red, + * and combined lists of poses, allowing code to easily retrieve poses for the + * current alliance. *

- * The transformation to red-side poses is performed by reflecting the blue-side pose + * The transformation to red-side poses is performed by reflecting the blue-side + * pose * across the field's length and width, and rotating the heading by 180 degrees. */ public static final class Pose2dAllianceSet { @@ -86,10 +98,20 @@ static Pose2dAllianceSet concat(Pose2dAllianceSet... sets) { } return new Pose2dAllianceSet(combined.toArray(new Pose2d[0])); } + + public List getAlliancePoses() { + List poses; + if (isRedAlliance()) { + poses = getRed(); + } else { + poses = getBlue(); + } + return poses; + } } - public static final Distance FIELD_LENGTH = Units.Feet.of(57).plus(Units.Inches.of(6.875)); - public static final Distance FIELD_WIDTH = Units.Feet.of(26).plus(Units.Inches.of(5)); + public static final Distance FIELD_LENGTH = Units.Inches.of(651.2); // TODO: Update from game manual + public static final Distance FIELD_WIDTH = Units.Inches.of(317.7); // TODO: Update from game manual /** * Boolean that controls when the path will be mirrored for the red diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 9b15163..457301d 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -20,12 +20,12 @@ /** Add your docs here. */ public class ConstMotion { - // public static final double STOP = 0;/ public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(125); public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(0); public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14); + public static final double STOP = 0; static { @@ -56,5 +56,4 @@ public class ConstMotion { HOOD_PIVOT_CONFIGURATION.Slot0.kG = 0.1; HOOD_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; } - } diff --git a/src/main/java/frc/robot/constants/ConstPoseDrive.java b/src/main/java/frc/robot/constants/ConstPoseDrive.java index 580bd3d..f654fb0 100644 --- a/src/main/java/frc/robot/constants/ConstPoseDrive.java +++ b/src/main/java/frc/robot/constants/ConstPoseDrive.java @@ -68,4 +68,4 @@ public static class PoseDriveGroup { EXAMPLE_POSE_DRIVE_GROUP.distanceTolerance = Units.Inches.of(1); EXAMPLE_POSE_DRIVE_GROUP.rotationTolerance = Units.Degrees.of(1); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/constants/ConstSystem.java b/src/main/java/frc/robot/constants/ConstSystem.java index b18a1f2..e031875 100644 --- a/src/main/java/frc/robot/constants/ConstSystem.java +++ b/src/main/java/frc/robot/constants/ConstSystem.java @@ -9,6 +9,8 @@ import edu.wpi.first.units.Units; public final class ConstSystem { + + public static final String PRACTICE_BOT_RIO = ""; // TODO: Replace with actual RIO name /** * Volts */ @@ -20,5 +22,4 @@ public static class constControllers { public static final double DRIVER_LEFT_STICK_DEADBAND = 0.05; public static final boolean SILENCE_JOYSTICK_WARNINGS = true; } - } diff --git a/src/main/java/frc/robot/constants/ConstVision.java b/src/main/java/frc/robot/constants/ConstVision.java index 9803489..90b7efc 100644 --- a/src/main/java/frc/robot/constants/ConstVision.java +++ b/src/main/java/frc/robot/constants/ConstVision.java @@ -17,6 +17,34 @@ */ public class ConstVision { + public static class IMUMode { + /** + * No internal IMU processing. MT2 uses interpolated yaw from robot's gyro sent + * via SetRobotOrientation(). + */ + public static final int EXTERNAL_ONLY = 0; + /** + * Internal IMU offset is calibrated to match external yaw each frame (seeding). + * MT2 still uses external yaw for botpose. + */ + public static final int EXTERNAL_SEED = 1; + /** Uses internal IMU's fused yaw only. No external input required. */ + public static final int INTERNAL_ONLY = 2; + /** + * Complementary filter fuses internal IMU with MT1 vision yaw. When MT1 gets a + * valid pose, it slowly corrects internal IMU drift. + */ + public static final int INTERNAL_MT1_ASSIST = 3; + /** + * Complementary filter fuses internal IMU with external yaw from + * SetRobotOrientation(). This is the recommended mode, as the internal IMU's + * 1khz update rate is utilized for frame-by-frame motion while the robot's IMU + * corrects for any drift over time. + */ + public static final int INTERNAL_EXTERNAL_ASSIST = 4; + } + + public static final double IMU_ASSIST_ALPHA_VALUE = 0.01; public static final String LIMELIGHT_RIGHT_NAME = "limelight-right"; public static final String LIMELIGHT_LEFT_NAME = "limelight-left"; public static final String LIMELIGHT_BACK_NAME = "limelight-back"; @@ -58,8 +86,11 @@ public class ConstVision { * The area that one tag (if its the only tag in the update) needs to exceed * before being accepted */ - public static final double AREA_THRESHOLD_FRONT = 0.1; + public static final double AREA_THRESHOLD_FRONT = 0.05; public static final double AREA_THRESHOLD_BACK = 0.05; + public static final int DisabledThrottle = 100; + public static final int TeleopThrottle = 0; + public static final Boolean USE_MEGA_TAG_2 = true; // The below values are accounted for in the limelight interface, NOT in code public static class LimelightRight { @@ -92,5 +123,4 @@ public static class LimelightBack { public static final Angle LL_PITCH = Units.Degrees.of(-20); public static final Angle LL_YAW = Units.Degrees.of(0); } - } diff --git a/src/main/java/frc/robot/subsystems/DriverStateMachine.java b/src/main/java/frc/robot/subsystems/DriverStateMachine.java index 4d0d2d7..29d963c 100644 --- a/src/main/java/frc/robot/subsystems/DriverStateMachine.java +++ b/src/main/java/frc/robot/subsystems/DriverStateMachine.java @@ -9,7 +9,6 @@ import java.util.function.Supplier; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,13 +21,7 @@ public class DriverStateMachine extends SubsystemBase { /** Creates a new DriverStateMachine. */ public static DriverState currentDriverState = DriverState.MANUAL; - @NotLogged - Drivetrain subDrivetrain; - @NotLogged - DriverStateMachine subDriverStateMachine = this; - - public DriverStateMachine(Drivetrain subDrivetrain) { - this.subDrivetrain = subDrivetrain; + public DriverStateMachine() { } public DriverState getDriverState() { @@ -59,11 +52,9 @@ public Supplier tryState( case EXAMPLE_POSE_DRIVE: case CHOREO: return () -> new DriveManual( - subDrivetrain, xAxis, yAxis, rotationAxis, - subDriverStateMachine, slowMode); } break; @@ -73,8 +64,6 @@ public Supplier tryState( case MANUAL: case EXAMPLE_POSE_DRIVE: return () -> new PoseDrive( - subDrivetrain, - subDriverStateMachine, xAxis, yAxis, rotationAxis, @@ -89,8 +78,6 @@ public Supplier tryState( case EXAMPLE_ROTATION_SNAP: case MANUAL: return () -> new PoseDrive( - subDrivetrain, - subDriverStateMachine, xAxis, yAxis, rotationAxis, diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index b13add7..3fefb45 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,27 +4,38 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; + +import java.util.function.DoubleSupplier; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; - +import com.frcteam3255.components.swerve.SN_SuperSwerveV2; import choreo.trajectory.SwerveSample; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Angle; import frc.robot.DeviceIDs; +import frc.robot.RobotContainer; import frc.robot.constants.ConstDrivetrain; import frc.robot.constants.ConstPoseDrive.PoseDriveGroup; +@Logged public class Drivetrain extends SN_SuperSwerveV2 { public PoseDriveGroup lastDesiredPoseGroup; public Pose2d lastDesiredTarget; + private Rotation2d targetDriveRotation = new Rotation2d(); + private boolean manualRotationEnabled = true; /** Creates a new Drivetrain. */ public static final SwerveModuleConstantsFactory constantCreator = new SwerveModuleConstantsFactory() @@ -53,7 +64,8 @@ public class Drivetrain extends SN_SuperSwerveV2 { DeviceIDs.drivetrainIDs.FRONT_LEFT_STEER_CAN, DeviceIDs.drivetrainIDs.FRONT_LEFT_DRIVE_CAN, DeviceIDs.drivetrainIDs.FRONT_LEFT_ABSOLUTE_ENCODER_CAN, - ConstDrivetrain.FRONT_LEFT_ABS_ENCODER_OFFSET, + (RobotContainer.isPracticeBot()) ? ConstDrivetrain.PRACTICE_BOT.FRONT_LEFT_ABS_ENCODER_OFFSET + : ConstDrivetrain.FRONT_LEFT_ABS_ENCODER_OFFSET, ConstDrivetrain.MODULE_OFFSET_LOCATIONS, ConstDrivetrain.MODULE_OFFSET_LOCATIONS, ConstDrivetrain.INVERT_LEFT_SIDE_DRIVE, @@ -64,7 +76,8 @@ public class Drivetrain extends SN_SuperSwerveV2 { DeviceIDs.drivetrainIDs.FRONT_RIGHT_STEER_CAN, DeviceIDs.drivetrainIDs.FRONT_RIGHT_DRIVE_CAN, DeviceIDs.drivetrainIDs.FRONT_RIGHT_ABSOLUTE_ENCODER_CAN, - ConstDrivetrain.FRONT_RIGHT_ABS_ENCODER_OFFSET, + (RobotContainer.isPracticeBot()) ? ConstDrivetrain.PRACTICE_BOT.FRONT_RIGHT_ABS_ENCODER_OFFSET + : ConstDrivetrain.FRONT_RIGHT_ABS_ENCODER_OFFSET, ConstDrivetrain.MODULE_OFFSET_LOCATIONS, ConstDrivetrain.MODULE_OFFSET_LOCATIONS.unaryMinus(), ConstDrivetrain.INVERT_RIGHT_SIDE_DRIVE, @@ -75,7 +88,8 @@ public class Drivetrain extends SN_SuperSwerveV2 { DeviceIDs.drivetrainIDs.BACK_LEFT_STEER_CAN, DeviceIDs.drivetrainIDs.BACK_LEFT_DRIVE_CAN, DeviceIDs.drivetrainIDs.BACK_LEFT_ABSOLUTE_ENCODER_CAN, - ConstDrivetrain.BACK_LEFT_ABS_ENCODER_OFFSET, + (RobotContainer.isPracticeBot()) ? ConstDrivetrain.PRACTICE_BOT.BACK_LEFT_ABS_ENCODER_OFFSET + : ConstDrivetrain.BACK_LEFT_ABS_ENCODER_OFFSET, ConstDrivetrain.MODULE_OFFSET_LOCATIONS.unaryMinus(), ConstDrivetrain.MODULE_OFFSET_LOCATIONS, ConstDrivetrain.INVERT_LEFT_SIDE_DRIVE, @@ -86,7 +100,8 @@ public class Drivetrain extends SN_SuperSwerveV2 { DeviceIDs.drivetrainIDs.BACK_RIGHT_STEER_CAN, DeviceIDs.drivetrainIDs.BACK_RIGHT_DRIVE_CAN, DeviceIDs.drivetrainIDs.BACK_RIGHT_ABSOLUTE_ENCODER_CAN, - ConstDrivetrain.BACK_RIGHT_ABS_ENCODER_OFFSET, + (RobotContainer.isPracticeBot()) ? ConstDrivetrain.PRACTICE_BOT.BACK_RIGHT_ABS_ENCODER_OFFSET + : ConstDrivetrain.BACK_RIGHT_ABS_ENCODER_OFFSET, ConstDrivetrain.MODULE_OFFSET_LOCATIONS.unaryMinus(), ConstDrivetrain.MODULE_OFFSET_LOCATIONS.unaryMinus(), ConstDrivetrain.INVERT_RIGHT_SIDE_DRIVE, @@ -97,6 +112,18 @@ public class Drivetrain extends SN_SuperSwerveV2 { .withPigeon2Id(DeviceIDs.drivetrainIDs.PIGEON_CAN) .withPigeon2Configs(ConstDrivetrain.pigeonConfigs); + // Exposed motors for Epilogue logging + public final TalonFX frontLeftDrive; + public final TalonFX frontLeftSteer; + public final TalonFX frontRightDrive; + public final TalonFX frontRightSteer; + public final TalonFX backLeftDrive; + public final TalonFX backLeftSteer; + public final TalonFX backRightDrive; + public final TalonFX backRightSteer; + private Angle resetYawValue = Degrees.zero(); + private boolean isXbrakeAllowed = true; + public Drivetrain() { super( DrivetrainConstants, @@ -104,25 +131,38 @@ public Drivetrain() { FrontRight, BackLeft, BackRight); + + // Initialize motor references for Epilogue logging + // Front Left (index 0) + frontLeftDrive = getModule(0).getDriveMotor(); + frontLeftSteer = getModule(0).getSteerMotor(); + // Front Right (index 1) + frontRightDrive = getModule(1).getDriveMotor(); + frontRightSteer = getModule(1).getSteerMotor(); + // Back Left (index 2) + backLeftDrive = getModule(2).getDriveMotor(); + backLeftSteer = getModule(2).getSteerMotor(); + // Back Right (index 3) + backRightDrive = getModule(3).getDriveMotor(); + backRightSteer = getModule(3).getSteerMotor(); } - /** - * Follows a trajectory by calculating the desired chassis speeds based on the - * current pose - * of the robot and the target pose provided in the trajectory sample. - * - * @param sample The trajectory sample containing the desired target pose and - * other relevant data. - * This is used to determine the robot's next movement. - */ public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot - Pose2d desiredTarget = sample.getPose(); - ChassisSpeeds automatedDTVelocity = ConstDrivetrain.AUTO_ALIGN.PATH_AUTO_ALIGN_CONTROLLER.calculate(getPose(), - desiredTarget, 0, - desiredTarget.getRotation()); + Pose2d pose = getPose(); + double targetHeading = sample.heading; - drive(automatedDTVelocity); + if (!manualRotationEnabled) { // keep the !, manualRotationEnabled is false in prepanywhere + targetHeading = targetDriveRotation.getRadians(); + } + + // Generate the next speeds for the robot + ChassisSpeeds speeds = new ChassisSpeeds( + sample.vx + ConstDrivetrain.AUTO_ALIGN.POSE_TRANS_CONTROLLER.calculate(pose.getX(), sample.x), + sample.vy + ConstDrivetrain.AUTO_ALIGN.POSE_TRANS_CONTROLLER.calculate(pose.getY(), sample.y), + sample.omega + ConstDrivetrain.AUTO_ALIGN.POSE_ROTATION_CONTROLLER.calculate(pose.getRotation().getRadians(), + targetHeading)); + drive(speeds); } public void rotationalAlign(Pose2d closestPose, ChassisSpeeds velocities) { @@ -155,4 +195,46 @@ public void autoAlign( drive(automatedDTVelocity); } + public void setXbrakeAllowed(boolean isAllowed) { + this.isXbrakeAllowed = isAllowed; + } + + public boolean isXbrakeAllowed() { + return isXbrakeAllowed; + } + + public void setDriveRotation(Angle rotation) { + this.targetDriveRotation = Rotation2d.fromDegrees(rotation.in(Degrees)); + } + + public Rotation2d getTargetRotation() { + return this.targetDriveRotation; + } + + public Angle getDrivetrainRotation() { + return getPose().getRotation().getMeasure(); + } + + public boolean isManualRotationEnabled() { + return manualRotationEnabled; + } + + public void setIsManualRotationEnabled(boolean set) { + manualRotationEnabled = set; + } + + public void resetPoseAndYaw(Pose2d pose) { + resetPose(pose); + resetYawValue = pose.getRotation().getMeasure(); + getPigeon2().setYaw(resetYawValue); + setDriveRotation(resetYawValue); + } + + public Angle pigeonYaw() { + return getPigeon2().getYaw().getValue(); + } + + public Rotation2d getRawHeading() { + return getState().RawHeading; + } } diff --git a/src/main/java/frc/robot/subsystems/RobotPoses.java b/src/main/java/frc/robot/subsystems/RobotPoses.java index 258fd69..7aab65d 100644 --- a/src/main/java/frc/robot/subsystems/RobotPoses.java +++ b/src/main/java/frc/robot/subsystems/RobotPoses.java @@ -5,30 +5,32 @@ package frc.robot.subsystems; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.ConstSystem; +import frc.robot.RobotContainer; @Logged public class RobotPoses extends SubsystemBase { /** Creates a new RobotPoses. */ + Field2d field2d = new Field2d(); + FieldObject2d robotObject = field2d.getObject("Robot"); - @NotLogged - Drivetrain subDrivetrain; + Pose3d modelDrivetrain = Pose3d.kZero; - Pose3d comp0Drivetrain = Pose3d.kZero; - Pose3d comp1Bumpers = Pose3d.kZero.plus(ConstSystem.ROBOT_TO_BUMPERS); + public RobotPoses() { - public RobotPoses(Drivetrain subDrivetrain) { - this.subDrivetrain = subDrivetrain; + SmartDashboard.putData("Field", field2d); } @Override public void periodic() { + robotObject.setPose(RobotContainer.drivetrainInstance.getPose()); // This method will be called once per scheduler run // Robot Positions - comp0Drivetrain = new Pose3d(subDrivetrain.getPose()); + modelDrivetrain = new Pose3d(RobotContainer.drivetrainInstance.getPose()); } } diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index 067aafd..a015571 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -110,7 +110,8 @@ public boolean isFlyWheelAtSpeed(AngularVelocity tolerance) { } + @Override public void periodic() { - + // This method will be called once per scheduler run } } diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index 1f8bb05..b4b2d3a 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -5,7 +5,6 @@ package frc.robot.subsystems; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -14,16 +13,11 @@ @Logged public class StateMachine extends SubsystemBase { public static RobotState currentRobotState; - @NotLogged - Drivetrain subDrivetrain; - @NotLogged - StateMachine subStateMachine = this; /** Creates a new StateMachine. */ - public StateMachine(Drivetrain subDrivetrain) { + public StateMachine() { currentRobotState = RobotState.NONE; - this.subDrivetrain = subDrivetrain; } public void setRobotState(RobotState robotState) { @@ -39,7 +33,7 @@ public Command tryState(RobotState desiredState) { case NONE: switch (currentRobotState) { case NONE: - return new None(subStateMachine); + return new None(); } break; } diff --git a/src/main/java/frc/robot/subsystems/Telemetry.java b/src/main/java/frc/robot/subsystems/Telemetry.java new file mode 100644 index 0000000..b0e6699 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Telemetry.java @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +@Logged +public class Telemetry extends SubsystemBase { + /** Creates a new telemetry. */ + public Telemetry() { + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + public double getMatchTime() { + return DriverStation.getMatchTime(); + } + + public Voltage batteryVoltage() { + return RobotController.getMeasureBatteryVoltage(); + } + + public boolean isBrownedOut() { + return RobotController.isBrownedOut(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 7e0d80a..1ec38ce 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -6,14 +6,14 @@ import java.util.Optional; -import com.frcteam3255.utils.LimelightHelpers; -import com.frcteam3255.utils.LimelightHelpers.PoseEstimate; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.frcteam3255.utils.LimelightHelpers; +import com.frcteam3255.utils.LimelightHelpers.*; import frc.robot.constants.ConstVision; @Logged @@ -21,6 +21,7 @@ public class Vision extends SubsystemBase { PoseEstimate lastEstimateRight = new PoseEstimate(); PoseEstimate lastEstimateLeft = new PoseEstimate(); PoseEstimate lastEstimateBack = new PoseEstimate(); + private boolean visionEnabled = true; // Not logged, as they turn to false immediately after being read @NotLogged @@ -34,7 +35,21 @@ public class Vision extends SubsystemBase { Pose2d leftPose = new Pose2d(); Pose2d backPose = new Pose2d(); - private boolean useMegaTag2 = true; + int rightTagCount = 0; + int leftTagCount = 0; + int backTagCount = 0; + + String limelightInUse = LL_INUSE.NONE.toString(); + + public String getLimelightInUse() { + return limelightInUse; + } + + public enum LL_INUSE { + RIGHT, LEFT, BACK, NONE + } + + private boolean useMegaTag2 = ConstVision.USE_MEGA_TAG_2; public Vision() { } @@ -47,6 +62,18 @@ public void setMegaTag2(boolean useMegaTag2) { this.useMegaTag2 = useMegaTag2; } + public void setIMUAssistMode(boolean useAssist) { + if (useAssist == true) { + LimelightHelpers.SetIMUMode(ConstVision.LIMELIGHT_RIGHT_NAME, ConstVision.IMUMode.INTERNAL_MT1_ASSIST); + LimelightHelpers.SetIMUMode(ConstVision.LIMELIGHT_LEFT_NAME, ConstVision.IMUMode.INTERNAL_MT1_ASSIST); + LimelightHelpers.SetIMUMode(ConstVision.LIMELIGHT_BACK_NAME, ConstVision.IMUMode.INTERNAL_MT1_ASSIST); + } else { + LimelightHelpers.SetIMUMode(ConstVision.LIMELIGHT_RIGHT_NAME, ConstVision.IMUMode.EXTERNAL_SEED); + LimelightHelpers.SetIMUMode(ConstVision.LIMELIGHT_LEFT_NAME, ConstVision.IMUMode.EXTERNAL_SEED); + LimelightHelpers.SetIMUMode(ConstVision.LIMELIGHT_BACK_NAME, ConstVision.IMUMode.EXTERNAL_SEED); + } + } + /** * Determines if a given pose estimate should be rejected. * @@ -63,11 +90,25 @@ public boolean rejectUpdate(PoseEstimate poseEstimate, AngularVelocity gyroRate, return true; } + if (poseEstimate == null + || poseEstimate.pose == null + || poseEstimate.pose.getTranslation() == null + || !Double.isFinite(poseEstimate.pose.getTranslation().getX()) + || !Double.isFinite(poseEstimate.pose.getTranslation().getY())) { + System.err.println("********REJECTING POSE ESTIMATE: pose contained non-finite X or Y (NaN/Inf)********"); + return true; + } // No tags :< if (poseEstimate.tagCount == 0) { return true; } + // If MegaTag 2 do not reject if other items passed + if (useMegaTag2) { + return false; + } + + // If MegaTag 1 have additional checks // 1 Tag with a large area if (poseEstimate.tagCount == 1 && poseEstimate.avgTagArea > areaThreshold) { return false; @@ -120,39 +161,71 @@ public void setCurrentEstimates(AngularVelocity gyroRate) { lastEstimateRight = currentEstimateRight; rightPose = currentEstimateRight.pose; newRightEstimate = true; + rightTagCount = currentEstimateRight.tagCount; + } else { + rightTagCount = 0; } if (currentEstimateLeft != null && !rejectUpdate(currentEstimateLeft, gyroRate, ConstVision.AREA_THRESHOLD_FRONT)) { lastEstimateLeft = currentEstimateLeft; leftPose = currentEstimateLeft.pose; newLeftEstimate = true; + leftTagCount = currentEstimateLeft.tagCount; + } else { + leftTagCount = 0; } if (currentEstimateBack != null && !rejectUpdate(currentEstimateBack, gyroRate, ConstVision.AREA_THRESHOLD_BACK)) { lastEstimateBack = currentEstimateBack; backPose = currentEstimateBack.pose; newBackEstimate = true; + backTagCount = currentEstimateBack.tagCount; + } else { + backTagCount = 0; } } + public boolean isVisionEnabled() { + return visionEnabled; + } + + /** + * Sets the enabled status of the vision subsystem. + *

+ * Vision is set to disabled when we reset pose, as we assume that vision/pose + * is messed up before driver resets pose. + *

+ * Please set it back to enabled when using a prep which relies on + * vision. + * + * @param enabled The enabled status to set. + */ + public void setVisionEnabled(boolean enabled) { + this.visionEnabled = enabled; + } + public Optional determinePoseEstimate(AngularVelocity gyroRate) { setCurrentEstimates(gyroRate); // No valid pose estimates :( if (!newRightEstimate && !newLeftEstimate && !newBackEstimate) { + limelightInUse = LL_INUSE.NONE.toString(); return Optional.empty(); } else if (newRightEstimate && !newLeftEstimate && !newBackEstimate) { // One valid pose estimate (right) + limelightInUse = LL_INUSE.RIGHT.toString(); newRightEstimate = false; return Optional.of(lastEstimateRight); } else if (!newRightEstimate && newLeftEstimate && !newBackEstimate) { // One valid pose estimate (left) + limelightInUse = LL_INUSE.LEFT.toString(); newLeftEstimate = false; return Optional.of(lastEstimateLeft); } else if (!newRightEstimate && !newLeftEstimate && newBackEstimate) { // One valid pose estimate (back) - newLeftEstimate = false; + limelightInUse = LL_INUSE.BACK.toString(); + newBackEstimate = false; return Optional.of(lastEstimateBack); } else { @@ -160,21 +233,34 @@ public Optional determinePoseEstimate(AngularVelocity gyroRate) { newRightEstimate = false; newLeftEstimate = false; newBackEstimate = false; - if (lastEstimateRight.avgTagDist < lastEstimateLeft.avgTagDist - && lastEstimateRight.avgTagDist < lastEstimateBack.avgTagDist) { + + if (rightTagCount >= leftTagCount + && rightTagCount >= backTagCount) { + limelightInUse = LL_INUSE.RIGHT.toString(); return Optional.of(lastEstimateRight); - } else if (lastEstimateLeft.avgTagDist < lastEstimateRight.avgTagDist - && lastEstimateLeft.avgTagDist < lastEstimateBack.avgTagDist) { + } else if (leftTagCount >= rightTagCount + && leftTagCount >= backTagCount) { + limelightInUse = LL_INUSE.LEFT.toString(); return Optional.of(lastEstimateLeft); - } else if (lastEstimateBack.avgTagDist < lastEstimateRight.avgTagDist - && lastEstimateBack.avgTagDist < lastEstimateLeft.avgTagDist) { + } else if (backTagCount >= rightTagCount + && backTagCount >= leftTagCount) { + limelightInUse = LL_INUSE.BACK.toString(); return Optional.of(lastEstimateBack); } else { + limelightInUse = LL_INUSE.NONE.toString(); return Optional.empty(); } } } + public int getTotalTagCount() { + return rightTagCount + leftTagCount + backTagCount; + } + + public boolean seesTags() { + return getTotalTagCount() > 0; + } + @Override public void periodic() { } diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json new file mode 100644 index 0000000..9a106ab --- /dev/null +++ b/vendordeps/ChoreoLib2026.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2026.json", + "name": "ChoreoLib", + "version": "2026.0.3", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2026.0.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2026.0.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json new file mode 100644 index 0000000..f72fa41 --- /dev/null +++ b/vendordeps/PathplannerLib-2026.1.2.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2026.1.2.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-26.2.0.json b/vendordeps/Phoenix6-26.2.0.json new file mode 100644 index 0000000..e4bde96 --- /dev/null +++ b/vendordeps/Phoenix6-26.2.0.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.2.0.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.2.0", + "frcYear": "2026", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.2.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.2.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.2.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.2.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.2.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.2.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.2.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.2.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.2.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.2.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.2.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.2.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.2.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.2.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/SuperCORE.json b/vendordeps/SuperCORE.json index cdcd1ee..db7f831 100644 --- a/vendordeps/SuperCORE.json +++ b/vendordeps/SuperCORE.json @@ -1,8 +1,8 @@ { "fileName": "SuperCORE.json", "name": "SuperCORE", - "version": "2025.4.0", - "frcYear": 2025, + "version": "2026.4.1", + "frcYear": 2026, "uuid": "f82a9412-6f53-4bc0-960c-6895a5236f57", "mavenUrls": [ "https://frcteam3255.github.io/SuperCORE/releases/" @@ -12,7 +12,7 @@ { "groupId": "com.frcteam3255", "artifactId": "supercore", - "version": "2025.4.0" + "version": "2026.4.1" } ], "jniDependencies": [], diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 3718e0a..d90630e 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ @@ -25,6 +25,7 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "linuxsystemcore", "linuxathena", "linuxarm32", "linuxarm64", From a7b15e68853cbba980a1222e2536018fa76f69ed Mon Sep 17 00:00:00 2001 From: Wu-Fan-529 <3681247216@qq.com> Date: Sun, 24 May 2026 12:22:44 +0800 Subject: [PATCH 2/2] qf --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../java/frc/robot/constants/ConstMotion.java | 2 - .../java/frc/robot/constants/ConstRotors.java | 3 - .../java/frc/robot/subsystems/Drivetrain.java | 1 - .../java/frc/robot/subsystems/Rotors.java | 11 +- .../robot/subsystems/SN_SuperSwerveV2.java | 442 ---------------- vendordeps/ChoreoLib2025.json | 44 -- vendordeps/PathplannerLib-2026.1.2.json | 38 -- vendordeps/PathplannerLib.json | 38 -- vendordeps/Phoenix6-frc2025-latest.json | 479 ------------------ 10 files changed, 8 insertions(+), 1056 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/SN_SuperSwerveV2.java delete mode 100644 vendordeps/ChoreoLib2025.json delete mode 100644 vendordeps/PathplannerLib-2026.1.2.json delete mode 100644 vendordeps/PathplannerLib.json delete mode 100644 vendordeps/Phoenix6-frc2025-latest.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0a0dd9..31d534f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,19 +12,18 @@ import choreo.auto.AutoFactory; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.DeferredCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.DeviceIDs.controllerIDs; import frc.robot.commands.AddVisionMeasurement; import frc.robot.commands.states.Intaking; import frc.robot.commands.states.RetractIntake; +import frc.robot.constants.ConstSystem; import frc.robot.constants.ConstSystem.constControllers; import frc.robot.subsystems.DriverStateMachine; import frc.robot.subsystems.DriverStateMachine.DriverState; @@ -47,7 +46,6 @@ public class RobotContainer { private final SN_XboxController conDriver = new SN_XboxController(controllerIDs.DRIVER_USB); public static final Rotors rotorsInstance = new Rotors(); - public static final Motion motionInstance = new Motion(); private final Rotors loggedRotorsInstance = rotorsInstance; public static final Motion motionInstance = new Motion(); private final Motion loggedMotionInstance = motionInstance; diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 457301d..0e5a5a0 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -16,8 +16,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Angle; - /** Add your docs here. */ public class ConstMotion { public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 90246be..21cfa56 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -10,9 +10,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.AngularVelocity; - /** Add your docs here. */ public class ConstRotors { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 3fefb45..e575d46 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.Degrees; -import java.util.function.DoubleSupplier; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; diff --git a/src/main/java/frc/robot/subsystems/Rotors.java b/src/main/java/frc/robot/subsystems/Rotors.java index a015571..47d633e 100644 --- a/src/main/java/frc/robot/subsystems/Rotors.java +++ b/src/main/java/frc/robot/subsystems/Rotors.java @@ -4,10 +4,11 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; + import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; @@ -30,10 +31,10 @@ public class Rotors extends SubsystemBase { final TalonFX flywheelBottomEast = new TalonFX((rotorIDs.FLYWHEEL_BOTTOM_EAST_CAN)); AngularVelocity lastDesiredFlyWheelSpeed = Units.RPM.of(0); AngularVelocity lastDesiredTransferRollersSpeed = Units.RPM.of(0); - Follower flywheelEastFollower = new Follower(flywheelTopEast.getDeviceID(), false); - Follower flywheelWestFollower = new Follower(flywheelTopEast.getDeviceID(), true); - Follower transferRollersEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); - Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), true); + Follower flywheelEastFollower = new Follower(flywheelTopEast.getDeviceID(), MotorAlignmentValue.Aligned); + Follower flywheelWestFollower = new Follower(flywheelTopEast.getDeviceID(), MotorAlignmentValue.Opposed); + Follower transferRollersEastFollower = new Follower(intakeRollersEast.getDeviceID(), MotorAlignmentValue.Opposed); + Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), MotorAlignmentValue.Opposed); final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0); final MotionMagicVelocityVoltage transferRollersVelocityRequest = new MotionMagicVelocityVoltage(0); final MotionMagicVelocityVoltage serializerVelocityRequest = new MotionMagicVelocityVoltage(0); diff --git a/src/main/java/frc/robot/subsystems/SN_SuperSwerveV2.java b/src/main/java/frc/robot/subsystems/SN_SuperSwerveV2.java deleted file mode 100644 index ea3695f..0000000 --- a/src/main/java/frc/robot/subsystems/SN_SuperSwerveV2.java +++ /dev/null @@ -1,442 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. - */ -public class SN_SuperSwerveV2 extends SwerveDrivetrain implements Subsystem { - // ========================================================== - // ****************** CTRE GENERATED CODE ******************* - // ************************ GLOBALS ************************ - // ========================================================== - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - // ========================================================== - // ****************** CTRE GENERATED CODE ******************* - // ********************** CONSTRUCTORS ********************** - // NOTE: Originally Extended TunerConstants.TunerSwerveDrivetrain - // Modified to extend SwerveDrivetrain directly - // ========================================================== - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

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

- * This constructs the underlying hardware devices, so users should not - * construct - * the devices themselves. If they need the devices, they can access them - * through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve - * drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz - * on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry - * calculation - * in the form [x, y, theta]ᵀ, with units in - * meters - * and radians - * @param visionStandardDeviation The standard deviation for vision - * calculation - * in the form [x, y, theta]ᵀ, with units in - * meters - * and radians - * @param modules Constants for each specific module - */ - public SN_SuperSwerveV2( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - // ========================================================== - // ****************** CTRE GENERATED CODE ******************* - // ************************ FUNCTIONS ************************ - // ========================================================== - /** - * Returns a command that applies the specified control request to this swerve - * drivetrain. - * - * @param requestSupplier Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply - * it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts - * mid-match. - * Otherwise, only check and apply the operator perspective if the DS is - * disabled. - * This ensures driving behavior doesn't change until an explicit disable event - * occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); - m_hasAppliedOperatorPerspective = true; - }); - } - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate - * while still accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision - * camera. - * @param timestampSeconds The timestamp of the vision measurement in - * seconds. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate - * while still accounting for measurement noise. - *

- * Note that the vision measurement standard deviations passed into this method - * will continue to apply to future measurements until a subsequent call to - * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the - * vision camera. - * @param timestampSeconds The timestamp of the vision measurement in - * seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement - * in the form [x, y, theta]ᵀ, with units in - * meters and radians. - */ - @Override - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), - visionMeasurementStdDevs); - } - - // ========================================================== - // ****************** CTRE GENERATED CODE ******************* - // ************************* SYSID ************************* - // ========================================================== - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - /* - * SysId routine for characterizing translation. This is used to find PID gains - * for the drive motors. - */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this)); - - /* - * SysId routine for characterizing steer. This is used to find PID gains for - * the steer motors. - */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this)); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle - * HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on - * importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - // ========================================================= - // ********************** SUPERCORE *********************** - // Code made by the SuperNURDs. Additions on to CTRE Swerve. - // ========================================================= - // Swerve Requests were originally in CTRE Generated Swerve RobotContainer - private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric(); - private final SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.FieldCentricFacingAngle fieldCentricFacingAngleRequest = new SwerveRequest.FieldCentricFacingAngle(); - - public void drive(ChassisSpeeds chassisSpeeds) { - setControl(fieldCentricRequest - .withVelocityX(chassisSpeeds.vxMetersPerSecond) - .withVelocityY(chassisSpeeds.vyMetersPerSecond) - .withRotationalRate(chassisSpeeds.omegaRadiansPerSecond)); - } - - public void drive(ChassisSpeeds chassisSpeeds, Rotation2d facingAngle, double kP, double kI, double kD) { - setControl(fieldCentricFacingAngleRequest - .withVelocityX(chassisSpeeds.vxMetersPerSecond) - .withVelocityY(chassisSpeeds.vyMetersPerSecond) - .withHeadingPID(kP, kI, kD) - .withTargetDirection(facingAngle)); - } - - public void xBrake() { - setControl(brakeRequest); - } - - /** - * @return The current rate of rotation for the Pigeon 2. Units: Degrees - * per Second - */ - public AngularVelocity getGyroRate() { - return this.getPigeon2().getAngularVelocityZWorld().getValue(); - } - - /** - * Return the current estimated pose from the pose estimator. - * - * @return The current estimated pose - */ - public Pose2d getPose() { - return this.getState().Pose; - } - - /** - * Calculates the chassis velocities based on joystick inputs and other - * parameters. - * - * @param xAxisSupplier A DoubleSupplier providing the x-axis input for - * forward/backward movement. - * @param yAxisSupplier A DoubleSupplier providing the y-axis input for - * left/right movement. - * @param rotationAxisSupplier A DoubleSupplier providing the rotation input for - * turning. - * @param slowMode A BooleanSupplier indicating whether the slow - * mode is active. - * @param isRed A boolean indicating if the robot is on the red - * alliance (reverses controls if true). - * @param SLOW_MODE_MULTIPLIER A multiplier applied to velocities when slow mode - * is active. - * @param REAL_DRIVE_SPEED The maximum linear velocity of the robot in - * meters per second. - * @param TURN_SPEED The maximum angular velocity of the robot in - * radians per second. - * @return A ChassisSpeeds object containing the calculated x, y, and rotational - * velocities. - */ - public ChassisSpeeds calculateVelocitiesFromInput( - DoubleSupplier xAxisSupplier, - DoubleSupplier yAxisSupplier, - DoubleSupplier rotationAxisSupplier, - BooleanSupplier slowMode, - boolean isRed, - double SLOW_MODE_MULTIPLIER, - LinearVelocity REAL_DRIVE_SPEED, - AngularVelocity TURN_SPEED) { - - double redAllianceMultiplier = isRed ? -1 : 1; - double slowModeMultiplier = slowMode.getAsBoolean() ? SLOW_MODE_MULTIPLIER : 1.0; - - double xVelocity = xAxisSupplier.getAsDouble() * REAL_DRIVE_SPEED.in(Units.MetersPerSecond) - * redAllianceMultiplier * slowModeMultiplier; - double yVelocity = -yAxisSupplier.getAsDouble() * REAL_DRIVE_SPEED.in(Units.MetersPerSecond) - * redAllianceMultiplier * slowModeMultiplier; - double rotationVelocity = -rotationAxisSupplier.getAsDouble() - * TURN_SPEED.in(Units.RadiansPerSecond); - - return new ChassisSpeeds(xVelocity, yVelocity, rotationVelocity); - } - - /** - * Determines whether the robot is within the specified auto-drive zone based on - * the distance - * to a target pose. - * - * @param autoDriveMaxDistance The maximum allowable distance for the auto-drive - * zone. If null, - * the method will return false. - * @param target The target pose to calculate the distance from - * the robot's current pose. - * @return True if the robot's current pose is within the specified maximum - * distance from the target pose, - * false otherwise. - */ - public boolean isInAutoDriveZone(Distance autoDriveMaxDistance, Pose2d target) { - if (autoDriveMaxDistance == null) { - return false; - } - Distance distanceFromPose = Units.Meters - .of(this.getPose().getTranslation().getDistance(target.getTranslation())); - return distanceFromPose.lt(autoDriveMaxDistance); - } - - public boolean isAtRotation(Rotation2d desiredRotation, Angle tolerance) { - return (this.getPose().getRotation().getMeasure() - .compareTo(desiredRotation.getMeasure().minus(tolerance)) > 0) - && this.getPose().getRotation().getMeasure().compareTo(desiredRotation.getMeasure().plus(tolerance)) < 0; - } - - public boolean isAtPosition(Pose2d desiredPose2d, Distance tolerance) { - return Units.Meters.of(this.getPose().getTranslation().getDistance(desiredPose2d.getTranslation())) - .lte(tolerance); - } - -} diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json deleted file mode 100644 index 834cf0f..0000000 --- a/vendordeps/ChoreoLib2025.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "fileName": "ChoreoLib2025.json", - "name": "ChoreoLib", - "version": "2025.0.3", - "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2025", - "mavenUrls": [ - "https://lib.choreo.autos/dep", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", - "javaDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-java", - "version": "2025.0.3" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.11.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-cpp", - "version": "2025.0.3", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLib-2026.1.2.json deleted file mode 100644 index f72fa41..0000000 --- a/vendordeps/PathplannerLib-2026.1.2.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib-2026.1.2.json", - "name": "PathplannerLib", - "version": "2026.1.2", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2026.1.2" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2026.1.2", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index c496379..0000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2025.2.7", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2025", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2025.2.7" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2025.2.7", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json deleted file mode 100644 index 6f40c84..0000000 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ /dev/null @@ -1,479 +0,0 @@ -{ - "fileName": "Phoenix6-frc2025-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.4.0", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.4.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.4.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.4.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.4.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.4.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.4.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.4.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.4.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.4.0", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.4.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.4.0", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.4.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.4.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.4.0", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.4.0", - "libName": "CTRE_SimProCANdi", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "25.4.0", - "libName": "CTRE_SimProCANdle", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file