diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index d78b6e7..e85c0ea 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -48,7 +48,7 @@ diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 9af7112..a72b2c8 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2024", - "teamNumber": 2204 -} \ No newline at end of file + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2024", + "teamNumber": 2204 +} diff --git a/build.gradle b/build.gradle index 03ea45b..e60635a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,11 @@ +import groovy.json.JsonSlurper plugins { id "java" id "org.jetbrains.kotlin.jvm" version "1.9.22" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "com.peterabeles.gversion" version "1.10.2" + id "com.diffplug.spotless" version "6.25.0" id "idea" } @@ -52,6 +55,29 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true +// Configuration for AdvantageKit +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() +} + + +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -70,6 +96,9 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() + def akitJson = new JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" + implementation "org.jetbrains.kotlin:kotlin-stdlib-jdk8" testImplementation platform("org.junit:junit-bom:5.10.1") @@ -91,7 +120,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE @@ -127,4 +160,57 @@ idea { // Exclude the .vscode directory from indexing and search excludeDirs+=file(".vscode" ) } -} \ No newline at end of file +} + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "org.rambots" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Los_Angeles" + indent = " " +} + +// Spotless formatting +project.compileJava.dependsOn(spotlessApply) +spotless { + java { + target fileTree(".") { + include "**/*.java" + exclude "**/build/**", "**/build-*/**" + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree(".") { + include "**/*.gradle" + exclude "**/build/**", "**/build-*/**" + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + json { + target fileTree(".") { + include "**/*.json" + exclude "**/build/**", "**/build-*/**" + } + gson().indentWithSpaces(2) + } + format "misc", { + target fileTree(".") { + include "**/*.md", "**/.gitignore" + exclude "**/build/**", "**/build-*/**" + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..c4b7efd --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..18ae6d0 --- /dev/null +++ b/simgui.json @@ -0,0 +1,12 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Auto Choices": "String Chooser", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java new file mode 100644 index 0000000..5e2740b --- /dev/null +++ b/src/main/java/org/rambots/BuildConstants.java @@ -0,0 +1,17 @@ +package org.rambots; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "frc-2024"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 1; + public static final String GIT_SHA = "b55720df88c29f28dfcd9e76a0256378797c67ac"; + public static final String GIT_DATE = "2024-01-12 08:28:13 PST"; + public static final String GIT_BRANCH = "james/swerve"; + public static final String BUILD_DATE = "2024-03-02 15:32:31 PST"; + public static final long BUILD_UNIX_TIME = 1709422351326L; + public static final int DIRTY = 1; + + private BuildConstants() {} +} diff --git a/src/main/java/org/rambots/Constants.java b/src/main/java/org/rambots/Constants.java new file mode 100644 index 0000000..de0d348 --- /dev/null +++ b/src/main/java/org/rambots/Constants.java @@ -0,0 +1,72 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots; + +import edu.wpi.first.wpilibj.RobotBase; +import org.rambots.util.Alert; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final int loopPeriodMs = 20; + private static RobotType robotType = RobotType.SIMBOT; + public static final boolean tuningMode = true; + public static final boolean characterizationMode = false; + + public static RobotType getRobot() { + if (RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert("Invalid Robot Selected, using COMPBOT as default", Alert.AlertType.ERROR) + .set(true); + robotType = RobotType.COMPBOT; + } + return robotType; + } + + public static Mode getMode() { + return switch (getRobot()) { + case COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; + } + + public enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + public enum RobotType { + SIMBOT, + COMPBOT + } + + /** Checks whether the robot the correct robot is selected when deploying. */ + public static void main(String... args) { + if (robotType == RobotType.SIMBOT) { + System.err.println("Cannot deploy, invalid robot selected: " + robotType.toString()); + System.exit(1); + } + } +} diff --git a/src/main/java/org/rambots/Robot.kt b/src/main/java/org/rambots/Robot.kt index ec9f7d1..bd099e5 100644 --- a/src/main/java/org/rambots/Robot.kt +++ b/src/main/java/org/rambots/Robot.kt @@ -1,12 +1,20 @@ package org.rambots +import com.pathplanner.lib.pathfinding.Pathfinding import edu.wpi.first.hal.FRCNetComm.tInstances import edu.wpi.first.hal.FRCNetComm.tResourceType import edu.wpi.first.hal.HAL -import edu.wpi.first.wpilibj.TimedRobot import edu.wpi.first.wpilibj.util.WPILibVersion import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler +import org.littletonrobotics.junction.LogFileUtil +import org.littletonrobotics.junction.LoggedRobot +import org.littletonrobotics.junction.Logger +import org.littletonrobotics.junction.networktables.NT4Publisher +import org.littletonrobotics.junction.wpilog.WPILOGReader +import org.littletonrobotics.junction.wpilog.WPILOGWriter +import org.rambots.util.LocalADStarAK + /** * The VM is configured to automatically run this object (which basically functions as a singleton class), @@ -18,78 +26,98 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler * the `Main.kt` file in the project. (If you use the IDE's Rename or Move refactorings when renaming the * object or package, it will get changed everywhere.) */ -object Robot : TimedRobot() -{ +object Robot : LoggedRobot() { private var autonomousCommand: Command? = null - override fun robotInit() - { + override fun robotInit() { // Report the use of the Kotlin Language for "FRC Usage Report" statistics HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Kotlin, 0, WPILibVersion.Version) + + + Pathfinding.setPathfinder(LocalADStarAK()); + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + + when (BuildConstants.DIRTY) { + 0 -> Logger.recordMetadata("GitDirty", "All changes committed") + 1 -> Logger.recordMetadata("GitDirty", "Uncomitted changes") + else -> Logger.recordMetadata("GitDirty", "Unknown") + } + + when (Constants.getMode()) { + Constants.Mode.REAL -> { + Logger.addDataReceiver(WPILOGWriter()) + Logger.addDataReceiver(NT4Publisher()) + } + Constants.Mode.SIM -> { + Logger.addDataReceiver(NT4Publisher()) + } + Constants.Mode.REPLAY -> { + setUseTiming(false) // Run as fast as possible + val logPath = LogFileUtil.findReplayLog() + Logger.setReplaySource(WPILOGReader(logPath)) + Logger.addDataReceiver(WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))) + } + } + + Logger.start() + // Access the RobotContainer object so that it is initialized. This will perform all our // button bindings, and put our autonomous chooser on the dashboard. RobotContainer - } - override fun robotPeriodic() - { + override fun robotPeriodic() { CommandScheduler.getInstance().run() } - override fun disabledInit() - { + override fun disabledInit() { } - override fun disabledPeriodic() - { + override fun disabledPeriodic() { } - override fun autonomousInit() - { - autonomousCommand = RobotContainer.getAutonomousCommand() + override fun autonomousInit() { + autonomousCommand = RobotContainer.autonomousCommand autonomousCommand?.schedule() } - override fun autonomousPeriodic() - { + override fun autonomousPeriodic() { } - override fun teleopInit() - { + override fun teleopInit() { autonomousCommand?.cancel() } /** This method is called periodically during operator control. */ - override fun teleopPeriodic() - { + override fun teleopPeriodic() { } - override fun testInit() - { + override fun testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll() } - override fun testPeriodic() - { + override fun testPeriodic() { } - override fun simulationInit() - { + override fun simulationInit() { } - override fun simulationPeriodic() - { + override fun simulationPeriodic() { } } \ No newline at end of file diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 8ccf912..d54ad12 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -1,34 +1,238 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. package org.rambots +import com.pathplanner.lib.auto.AutoBuilder +import edu.wpi.first.math.geometry.* import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.button.CommandXboxController +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser +import org.rambots.commands.DriveCommands +import org.rambots.commands.DriveToPoint +import org.rambots.commands.MultiDistanceShot +import org.rambots.commands.PathFinderAndFollow +import org.rambots.subsystems.drive.* +import org.rambots.subsystems.drive.DriveConstants.moduleConfigs +import org.rambots.subsystems.flywheel.Flywheel +import org.rambots.subsystems.flywheel.FlywheelIO +import org.rambots.subsystems.flywheel.FlywheelIOSim +import org.rambots.subsystems.flywheel.FlywheelIOTalonFX +import org.rambots.subsystems.vision.AprilTagVision +import org.rambots.subsystems.vision.AprilTagVisionIO +import org.rambots.subsystems.vision.AprilTagVisionIOLimelight +import org.rambots.subsystems.vision.AprilTagVisionIOPhotonVisionSIM +import org.rambots.util.FieldConstants +import org.rambots.util.VisionHelpers.TimestampedVisionUpdate +import java.util.function.Consumer + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the [Robot] * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - * - * In Kotlin, it is recommended that all your Subsystems are Kotlin objects. As such, there - * can only ever be a single instance. This eliminates the need to create reference variables - * to the various subsystems in this container to pass into to commands. The commands can just - * directly reference the (single instance of the) object. + * subsystems, commands, and button mappings) should be declared here. */ -object RobotContainer -{ - init - { - configureBindings() - } +object RobotContainer { + // Subsystems + private var drive: Drive + private var flywheel: Flywheel + private var aprilTagVision: AprilTagVision + private val driveMode = DriveController() + + // Controller + private val controller = CommandXboxController(0) + + // Dashboard inputs + private val autoChooser: LoggedDashboardChooser + + // private final LoggedTunableNumber flywheelSpeedInput = + // new LoggedTunableNumber("Flywheel Speed", 1500.0); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + init { + when (Constants.getMode()) { + Constants.Mode.REAL -> { + // Real robot, instantiate hardware IO implementations + drive = Drive( + GyroIOPigeon2(true), + ModuleIOTalonFX(moduleConfigs[0]), + ModuleIOTalonFX(moduleConfigs[1]), + ModuleIOTalonFX(moduleConfigs[2]), + ModuleIOTalonFX(moduleConfigs[3]) + ) + + // flywheel = new Flywheel(new FlywheelIOSparkMax()); + // drive = new Drive( + // new GyroIOPigeon2(true), + // new ModuleIOTalonFX(0), + // new ModuleIOTalonFX(1), + // new ModuleIOTalonFX(2), + // new ModuleIOTalonFX(3)); + flywheel = Flywheel(FlywheelIOTalonFX()) + aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight")) + } + + Constants.Mode.SIM -> { + // Sim robot, instantiate physics sim IO implementations + drive = + Drive( + object : GyroIO {}, + ModuleIOSim(), + ModuleIOSim(), + ModuleIOSim(), + ModuleIOSim() + ) + flywheel = Flywheel(FlywheelIOSim()) + aprilTagVision = + AprilTagVision( + AprilTagVisionIOPhotonVisionSIM( + "photonCamera1", + Transform3d(Translation3d(0.5, 0.0, 0.5), Rotation3d(0.0, 0.0, 0.0)) + ) { drive.getDrive() }) + } + + else -> { + // Replayed robot, disable IO implementations + drive = + Drive( + object : GyroIO {}, + object : ModuleIO {}, + object : ModuleIO {}, + object : ModuleIO {}, + object : ModuleIO {}) + flywheel = Flywheel(object : FlywheelIO {}) + aprilTagVision = AprilTagVision(object : AprilTagVisionIO {}) + } + } + // Set up auto routines + // NamedCommands.registerCommand( + // "Run Flywheel", + // Commands.startEnd( + // () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel) + // .withTimeout(5.0)); + autoChooser = LoggedDashboardChooser("Auto Choices", AutoBuilder.buildAutoChooser()) - /** Use this method to define your `trigger->command` mappings. */ - private fun configureBindings() - { + // Set up SysId routines + autoChooser.addOption( + "Drive SysId (Quasistatic Forward)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + ) + autoChooser.addOption( + "Drive SysId (Quasistatic Reverse)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + ) + autoChooser.addOption( + "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward) + ) + autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse) + ) + // autoChooser.addOption( + // "Flywheel SysId (Quasistatic Forward)", + // flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // autoChooser.addOption( + // "Flywheel SysId (Quasistatic Reverse)", + // flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + // autoChooser.addOption( + // "Flywheel SysId (Dynamic Forward)", + // flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // autoChooser.addOption( + // "Flywheel SysId (Dynamic Reverse)", + // flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Configure the button bindings + aprilTagVision.setDataInterfaces(Consumer { visionData: List? -> + drive.addVisionData( + visionData + ) + }) + driveMode.setPoseSupplier { drive.pose } + driveMode.disableHeadingControl() + configureButtonBindings() } - fun getAutonomousCommand(): Command? - { - // TODO: Implement properly - return null + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a [GenericHID] or one of its subclasses ([ ] or [XboxController]), and then passing it to a [ ]. + */ + private fun configureButtonBindings() { + drive.defaultCommand = DriveCommands.joystickDrive( + drive, + driveMode, + { -controller.leftY }, + { -controller.leftX }, + { -controller.rightX }) + controller.leftBumper().whileTrue(Commands.runOnce({ driveMode.toggleDriveMode() })) + + controller.a().whileTrue(PathFinderAndFollow(driveMode.driveModeType)) + + controller + .b() + .whileTrue( + Commands.startEnd( + { driveMode.enableHeadingControl() }, { driveMode.disableHeadingControl() }) + ) + + controller + .y() + .whileTrue( + Commands.runOnce( + { + drive!!.setAutoStartPose( + Pose2d(Translation2d(4.0, 5.0), Rotation2d.fromDegrees(0.0)) + ) + }) + ) + controller + .povDown() + .whileTrue( + DriveToPoint( + drive, Pose2d(Translation2d(2.954, 3.621), Rotation2d.fromRadians(2.617)) + ) + ) + + controller + .povUp() + .whileTrue( + MultiDistanceShot( + { drive!!.pose }, FieldConstants.Speaker.centerSpeakerOpening, flywheel + ) + ) + + // controller + // .b() + // .onTrue( + // Commands.runOnce( + // () -> + // drive.setPose( + // new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + // drive) + // .ignoringDisable(true)); + // controller + // .a() + // .whileTrue( + // Commands.startEnd( + // () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); } -} \ No newline at end of file + + val autonomousCommand: Command + /** + * Use this to pass the autonomous command to the main [Robot] class. + * + * @return the command to run in autonomous + */ + get() = autoChooser.get() + +} diff --git a/src/main/java/org/rambots/commands/DriveCommands.java b/src/main/java/org/rambots/commands/DriveCommands.java new file mode 100644 index 0000000..694bad5 --- /dev/null +++ b/src/main/java/org/rambots/commands/DriveCommands.java @@ -0,0 +1,92 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.commands; + +import static org.rambots.subsystems.drive.DriveConstants.drivetrainConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import java.util.function.DoubleSupplier; +import org.rambots.subsystems.drive.Drive; +import org.rambots.subsystems.drive.DriveController; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + + private DriveCommands() {} + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DriveController driveMode, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + if (driveMode.isHeadingControlled()) { + final var targetAngle = driveMode.getHeadingAngle(); + omega = + Drive.getThetaController() + .calculate( + drive.getPose().getRotation().getRadians(), targetAngle.get().getRadians()); + if (Drive.getThetaController().atGoal()) { + omega = 0; + } + omega = Math.copySign(Math.min(1, Math.abs(omega)), omega); + } + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * drivetrainConfig.maxLinearVelocity(), + linearVelocity.getY() * drivetrainConfig.maxLinearVelocity(), + omega * drivetrainConfig.maxAngularVelocity(), + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive); + } +} diff --git a/src/main/java/org/rambots/commands/DriveToPoint.java b/src/main/java/org/rambots/commands/DriveToPoint.java new file mode 100644 index 0000000..7f7b889 --- /dev/null +++ b/src/main/java/org/rambots/commands/DriveToPoint.java @@ -0,0 +1,62 @@ +// 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 org.rambots.commands; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import org.rambots.subsystems.drive.Drive; +import org.rambots.util.AllianceFlipUtil; + +public class DriveToPoint extends Command { + // THIS IS JUST A DOCUMENT FOR TESTING pathfindToPose. I RECOMMEND YOU USE pathfindThenFollowPath + // INSTEAD + Drive drive; + Pose2d targetPose; + Command pathRun; + private Command scoreCommand; + + /** Creates a new ShootPoint. */ + public DriveToPoint(Drive drive, Pose2d targetPose) { + // Use addRequirements() here to declare subsystem dependencies. + // For shooting you would also want to pass in your shooter subsystem + this.drive = drive; + this.targetPose = targetPose; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + pathRun = + AutoBuilder.pathfindToPose( + AllianceFlipUtil.apply(targetPose), + new PathConstraints(4.0, 4.0, Units.degreesToRadians(360), Units.degreesToRadians(540)), + 0, + 0.0); + scoreCommand = Commands.sequence(pathRun); + scoreCommand.schedule(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + super.end(interrupted); + scoreCommand.cancel(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // This should be the last command in the sequence + return pathRun.isFinished(); + } +} diff --git a/src/main/java/org/rambots/commands/MultiDistanceShot.java b/src/main/java/org/rambots/commands/MultiDistanceShot.java new file mode 100644 index 0000000..3519664 --- /dev/null +++ b/src/main/java/org/rambots/commands/MultiDistanceShot.java @@ -0,0 +1,95 @@ +// 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 org.rambots.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.rambots.subsystems.flywheel.Flywheel; +import org.rambots.util.AllianceFlipUtil; + +/** A command that shoots game piece from multi-distance position from the target. */ +public class MultiDistanceShot extends Command { + Supplier poseSupplier; + Pose2d targetPose; + Flywheel flywheel; + InterpolatingDoubleTreeMap distanceMap = new InterpolatingDoubleTreeMap(); + + double distance; + double speed; + + /** + * Creates a new MultiDistanceShot command. + * + * @param poseSupplier The supplier for the robot's current pose. + * @param targetPose The target pose to shoot at. + * @param flywheel The flywheel subsystem. + */ + public MultiDistanceShot(Supplier poseSupplier, Pose2d targetPose, Flywheel flywheel) { + this.poseSupplier = poseSupplier; + this.targetPose = targetPose; + this.flywheel = flywheel; + + // Populate the distance map with distance-speed pairs + distanceMap.put(1.0, 10.0); + distanceMap.put(2.3, 15.7); + distanceMap.put(3.6, 21.9); + distanceMap.put(4.9, 27.8); + distanceMap.put(6.2, 33.6); + distanceMap.put(7.5, 39.4); + } + + @Override + public void initialize() { + // Apply any necessary transformations to the target pose + targetPose = AllianceFlipUtil.apply(targetPose); + } + + @Override + public void execute() { + // Calculate the distance from the current pose to the target pose + distance = poseSupplier.get().getTranslation().getDistance(targetPose.getTranslation()); + + // Get the corresponding speed from the distance-speed map + speed = distanceMap.get(distance); + + // Run the flywheel at the calculated speed + flywheel.runVelocity(speed); + } + + @Override + public void end(boolean interrupted) { + // Stop the flywheel when the command ends + flywheel.stop(); + } + + @Override + public boolean isFinished() { + // The command never finishes on its own + return false; + } + + /** + * Gets the distance from the current pose to the target pose. + * + * @return The distance in units. + */ + @AutoLogOutput(key = "Shooter/DistanceToTarget") + public double getDistance() { + return distance; + } + + /** + * Gets the speed of the flywheel. + * + * @return The speed in units per second. + */ + @AutoLogOutput(key = "Shooter/Speed") + public double getSpeed() { + return speed; + } +} diff --git a/src/main/java/org/rambots/commands/PathFinderAndFollow.java b/src/main/java/org/rambots/commands/PathFinderAndFollow.java new file mode 100644 index 0000000..99db6d1 --- /dev/null +++ b/src/main/java/org/rambots/commands/PathFinderAndFollow.java @@ -0,0 +1,71 @@ +// 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 org.rambots.commands; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import java.util.function.Supplier; +import org.rambots.subsystems.drive.DriveController; + +/** A command that runs pathfindThenFollowPath based on the current drive mode. */ +public class PathFinderAndFollow extends Command { + private final Supplier driveModeSupplier; + private Command scoreCommand; + private Command pathRun; + private DriveController.DriveModeType driveMode; + + /** + * Creates a new PathFinderAndFollow command. + * + * @param driveModeSupplier a supplier for the drive mode type + */ + public PathFinderAndFollow(Supplier driveModeSupplier) { + this.driveModeSupplier = driveModeSupplier; + } + + @Override + public void initialize() { + runNewAutonPath(); + } + + @Override + public void execute() { + DriveController.DriveModeType currentDriveMode = driveModeSupplier.get(); + if (driveMode != currentDriveMode) { + scoreCommand.cancel(); + runNewAutonPath(); + } + } + + @Override + public void end(boolean interrupted) { + super.end(interrupted); + scoreCommand.cancel(); + } + + @Override + public boolean isFinished() { + return pathRun.isFinished(); + } + + /** Runs a new autonomous path based on the current drive mode. */ + public void runNewAutonPath() { + driveMode = driveModeSupplier.get(); + String pathName = + driveMode == DriveController.DriveModeType.SPEAKER + ? "Speaker Placement Path" + : "Amp Placement Path"; + PathPlannerPath ampPath = PathPlannerPath.fromPathFile(pathName); + PathConstraints constraints = + new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); + pathRun = AutoBuilder.pathfindThenFollowPath(ampPath, constraints, 0.0); + scoreCommand = Commands.sequence(pathRun); + scoreCommand.schedule(); + } +} diff --git a/src/main/java/org/rambots/subsystems/.gitkeep b/src/main/java/org/rambots/subsystems/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java new file mode 100644 index 0000000..44883bc --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/Drive.java @@ -0,0 +1,349 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static edu.wpi.first.units.Units.*; +import static org.rambots.subsystems.drive.DriveConstants.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.List; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.rambots.util.VisionHelpers; + +public class Drive extends SubsystemBase { + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + + private static ProfiledPIDController thetaController = + new ProfiledPIDController( + headingControllerConstants.Kp(), + 0, + headingControllerConstants.Kd(), + new TrapezoidProfile.Constraints( + drivetrainConfig.maxAngularVelocity(), drivetrainConfig.maxAngularAcceleration())); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + lastModulePositions, + new Pose2d(), + stateStdDevs, + new Matrix<>( + VecBuilder.fill(xyStdDevCoefficient, xyStdDevCoefficient, thetaStdDevCoefficient))); + + private SwerveDrivePoseEstimator odometryDrive = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Start threads (no-op for each if no signals have been created) + PhoenixOdometryThread.getInstance().start(); + SparkMaxOdometryThread.getInstance().start(); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configureHolonomic( + this::getPose, + this::setAutoStartPose, + () -> kinematics.toChassisSpeeds(getModuleStates()), + this::runVelocity, + new HolonomicPathFollowerConfig( + new PIDConstants( + PPtranslationConstants.kP, PPtranslationConstants.kI, PPtranslationConstants.kD), + new PIDConstants( + PProtationConstants.kP, PProtationConstants.kI, PProtationConstants.kD), + drivetrainConfig.maxLinearVelocity(), + drivetrainConfig.driveBaseRadius(), + new ReplanningConfig()), + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red, + this); + PathPlannerLogging.setLogActivePathCallback( + activePath -> + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]))); + PathPlannerLogging.setLogTargetPoseCallback( + targetPose -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + state -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + voltage -> { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(voltage.in(Volts)); + } + }, + null, + this)); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(Units.degreesToRadians(1.5)); + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + odometryLock.unlock(); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + odometryDrive.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, drivetrainConfig.maxLinearVelocity()); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = moduleTranslations[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** + * Returns a command to run a quasistatic test in the specified direction. + * + * @param direction The direction to run the quasistatic test. + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** + * Returns a command to run a dynamic test in the specified direction. + * + * @param direction The direction to run the dynamic test. + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current pose estimation. */ + @AutoLogOutput(key = "Odometry/PoseEstimation") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Drive") + public Pose2d getDrive() { + return odometryDrive.getEstimatedPosition(); + } + + /** Returns the current poseEstimator rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** + * Resets the current poseEstimator pose. + * + * @param pose The pose to reset to. + */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Resets the current odometry and poseEstimator pose. + * + * @param pose The pose to reset to. + */ + public void setAutoStartPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + odometryDrive.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement( + Pose2d visionPose, double timestamp, Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement(visionPose, timestamp, visionMeasurementStdDevs); + } + + /** + * Adds vision data to the pose esimation. + * + * @param visionData The vision data to add. + */ + public void addVisionData(List visionData) { + visionData.forEach( + visionUpdate -> + addVisionMeasurement( + visionUpdate.pose(), visionUpdate.timestamp(), visionUpdate.stdDevs())); + } + + public static ProfiledPIDController getThetaController() { + return thetaController; + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..ba00616 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java @@ -0,0 +1,178 @@ +package org.rambots.subsystems.drive; + +import com.pathplanner.lib.util.PIDConstants; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import org.rambots.Constants; + +/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ +public final class DriveConstants { + public static DrivetrainConfig drivetrainConfig = + switch (Constants.getRobot()) { + default -> + new DrivetrainConfig( + Units.inchesToMeters(2.0), + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(12.16), + Units.feetToMeters(21.32), + 7.93, + 29.89); + }; + public static final double wheelRadius = Units.inchesToMeters(2.0); + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + new Translation2d( + drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0), + new Translation2d( + drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0), + new Translation2d( + -drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0), + new Translation2d( + -drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0) + }; + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(moduleTranslations); + public static final double odometryFrequency = + switch (Constants.getRobot()) { + case SIMBOT -> 50.0; + case COMPBOT -> 250.0; + }; + public static final Matrix stateStdDevs = + switch (Constants.getRobot()) { + default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); + }; + public static final double xyStdDevCoefficient = + switch (Constants.getRobot()) { + default -> 0.01; + }; + public static final double thetaStdDevCoefficient = + switch (Constants.getRobot()) { + default -> 0.01; + }; + + public static final int gyroID = 13; + + // Turn to "" for no canbus name + public static final String canbus = "*"; + + public static ModuleConfig[] moduleConfigs = + switch (Constants.getRobot()) { + case COMPBOT -> + new ModuleConfig[] { + new ModuleConfig( + 1, 2, 9, Rotation2d.fromRotations(-.247).plus(Rotation2d.fromDegrees(180)), true), + new ModuleConfig( + 3, + 4, + 10, + Rotation2d.fromRotations(-.188).plus(Rotation2d.fromDegrees(180)), + true), + new ModuleConfig( + 5, + 6, + 11, + Rotation2d.fromRotations(-.382).plus(Rotation2d.fromDegrees(180)), + true), + new ModuleConfig( + 7, 8, 12, Rotation2d.fromRotations(.334).plus(Rotation2d.fromDegrees(180)), true) + }; + case SIMBOT -> { + ModuleConfig[] configs = new ModuleConfig[4]; + for (int i = 0; i < configs.length; i++) + configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); + yield configs; + } + }; + + public static final ModuleConstants moduleConstants = + switch (Constants.getRobot()) { + case COMPBOT -> + new ModuleConstants( + 0.1, + 0.13, + 0.1, + 0.0, + 10.0, + 0.0, + SwerveXReductions.OldGen736.reduction, + SwerveXReductions.TURN.reduction); + case SIMBOT -> + new ModuleConstants( + 0.014, + 0.134, + 0.1, + 0.0, + 10.0, + 0.0, + SwerveXReductions.OldGen736.reduction, + SwerveXReductions.TURN.reduction); + }; + + public static HeadingControllerConstants headingControllerConstants = + switch (Constants.getRobot()) { + case COMPBOT -> new HeadingControllerConstants(3.0, 0.0); + case SIMBOT -> new HeadingControllerConstants(3.0, 0.0); + }; + + public static final PIDConstants PPtranslationConstants = + switch (Constants.getRobot()) { + case COMPBOT -> new PIDConstants(10, 0.0, 0.0); + case SIMBOT -> new PIDConstants(10, 0.0, 0.0); + }; + + public static final PIDConstants PProtationConstants = + switch (Constants.getRobot()) { + case COMPBOT -> new PIDConstants(10, 0.0, 0.0); + case SIMBOT -> new PIDConstants(10, 0.0, 0.0); + }; + + public record DrivetrainConfig( + double wheelRadius, + double trackwidthX, + double trackwidthY, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + public double driveBaseRadius() { + return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); + } + } + + public record ModuleConfig( + int driveID, + int turnID, + int absoluteEncoderChannel, + Rotation2d absoluteEncoderOffset, + boolean turnMotorInverted) {} + + public record ModuleConstants( + double ffKs, + double ffKv, + double driveKp, + double drivekD, + double turnKp, + double turnkD, + double driveReduction, + double turnReduction) {} + + public record HeadingControllerConstants(double Kp, double Kd) {} + + private enum SwerveXReductions { + OldGen736((42.0 / 11.0) * (18.0 / 28.0) * (45.0 / 15.0)), + TURN(15.43); + + final double reduction; + + SwerveXReductions(double reduction) { + this.reduction = reduction; + } + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/DriveController.java b/src/main/java/org/rambots/subsystems/drive/DriveController.java new file mode 100644 index 0000000..bc73538 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/DriveController.java @@ -0,0 +1,129 @@ +package org.rambots.subsystems.drive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Optional; +import java.util.function.Supplier; +import org.rambots.util.AllianceFlipUtil; +import org.rambots.util.FieldConstants; + +/** + * The DriveController class represents a controller for the robot's drive system. It provides + * methods to control the heading and drive mode of the robot. + */ +public class DriveController { + private Optional> headingSupplier = Optional.empty(); + private Supplier poseSupplier = Pose2d::new; + private DriveModeType driveModeType = DriveModeType.SPEAKER; + + /** + * Sets the heading supplier that provides the desired heading for the robot. + * + * @param headingSupplier The supplier that provides the desired heading. + */ + public void setHeadingSupplier(Supplier headingSupplier) { + this.headingSupplier = Optional.of(headingSupplier); + } + + /** + * Sets the pose supplier that provides the current pose of the robot. + * + * @param poseSupplier The supplier that provides the current pose. + */ + public void setPoseSupplier(Supplier poseSupplier) { + this.poseSupplier = poseSupplier; + } + + /** + * Checks if the heading is being controlled. + * + * @return True if the heading is being controlled, false otherwise. + */ + public boolean isHeadingControlled() { + return this.headingSupplier.isPresent(); + } + + /** + * Gets the current drive mode. + * + * @return The supplier that provides the current drive mode. + */ + public Supplier getDriveModeType() { + return () -> this.driveModeType; + } + + /** + * Gets the current heading angle. + * + * @return The supplier that provides the current heading angle. + */ + public Supplier getHeadingAngle() { + return headingSupplier.get(); + } + + /** + * Sets the drive mode. + * + * @param driveModeType The drive mode to set. + */ + public void setDriveMode(DriveModeType driveModeType) { + this.driveModeType = driveModeType; + updateHeading(); + } + + /** Toggles the drive mode between AMP and SPEAKER. */ + public void toggleDriveMode() { + if (getDriveModeType().get() == DriveModeType.AMP) { + setDriveMode(DriveModeType.SPEAKER); + } else { + setDriveMode(DriveModeType.AMP); + } + } + + /** Enables heading control based on the current drive mode. */ + public void enableHeadingControl() { + if (this.driveModeType == DriveModeType.AMP) { + enableAmpHeading(); + } else { + enableSpeakerHeading(); + } + } + + /** Disables heading control (heading control is disabled). */ + public void disableHeadingControl() { + this.headingSupplier = Optional.empty(); + } + + /** Updates the heading if it is being controlled. */ + private void updateHeading() { + if (isHeadingControlled()) { + enableHeadingControl(); + } + } + + /** Turns on heading control and sets the heading to AMP mode (90 degrees). */ + private void enableAmpHeading() { + setHeadingSupplier(() -> Rotation2d.fromDegrees(90)); + } + + /** Turns on heading control and sets the heading to SPEAKER mode. */ + private void enableSpeakerHeading() { + setHeadingSupplier( + () -> + new Rotation2d( + poseSupplier.get().getX() + - AllianceFlipUtil.apply( + FieldConstants.Speaker.centerSpeakerOpening.getTranslation()) + .getX(), + poseSupplier.get().getY() + - AllianceFlipUtil.apply( + FieldConstants.Speaker.centerSpeakerOpening.getTranslation()) + .getY())); + } + + /** Possible Drive Modes. */ + public enum DriveModeType { + AMP, + SPEAKER, + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/GyroIO.java b/src/main/java/org/rambots/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..b41f703 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/GyroIO.java @@ -0,0 +1,30 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double yawVelocityRadPerSec = 0.0; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/org/rambots/subsystems/drive/GyroIONavX2.java b/src/main/java/org/rambots/subsystems/drive/GyroIONavX2.java new file mode 100644 index 0000000..886a458 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/GyroIONavX2.java @@ -0,0 +1,42 @@ +package org.rambots.subsystems.drive; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.SPI; +import java.util.OptionalDouble; +import java.util.Queue; + +public class GyroIONavX2 implements GyroIO { + private final AHRS navx = new AHRS(SPI.Port.kMXP); + private final Queue yawPositionQueue; + + public GyroIONavX2() { + navx.reset(); + navx.resetDisplacement(); + navx.zeroYaw(); + + yawPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + boolean valid = navx.isConnected(); + if (valid) { + return OptionalDouble.of(navx.getYaw()); + } else { + return OptionalDouble.empty(); + } + }); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = navx.isConnected(); + inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRawGyroZ()); + inputs.odometryYawPositions = + yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); + + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..810bb75 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,75 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.OptionalDouble; +import java.util.Queue; + +/** IO implementation for Pigeon2 */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(gyroID, canbus); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2(boolean phoenixDrive) { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(odometryFrequency); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + if (phoenixDrive) { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); + } else { + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + boolean valid = yaw.refresh().getStatus().isOK(); + if (valid) { + return OptionalDouble.of(yaw.getValueAsDouble()); + } else { + return OptionalDouble.empty(); + } + }); + } + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/Module.java b/src/main/java/org/rambots/subsystems/drive/Module.java new file mode 100644 index 0000000..5bd16a5 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/Module.java @@ -0,0 +1,202 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.wheelRadius; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.littletonrobotics.junction.Logger; +import org.rambots.Constants; + +public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.getMode()) { + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + driveFeedback = new PIDController(0.1, 0.0, 0.0); + turnFeedback = new PIDController(10.0, 0.0, 0.0); + break; + default: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + turnFeedback = new PIDController(0.0, 0.0, 0.0); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + /** + * Update inputs without running the rest of the periodic logic. This is useful since these + * updates need to be properly thread-locked. + */ + public void updateInputs() { + io.updateInputs(inputs); + } + + public void periodic() { + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90 degrees, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / wheelRadius; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadius; + Rotation2d angle = + inputs.odometryTurnPositions[i].plus( + turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = optimizedState.angle; + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * wheelRadius; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * wheelRadius; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/ModuleIO.java b/src/main/java/org/rambots/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..580b8c1 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/ModuleIO.java @@ -0,0 +1,52 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/org/rambots/subsystems/drive/ModuleIOSim.java b/src/main/java/org/rambots/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..9fb2b1a --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,76 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.moduleConstants; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** + * Physics sim implementation of module IO. + * + *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized + * to a random value. The flywheel sims are not physically accurate, but provide a decent + * approximation for the behavior of the module. + */ +public class ModuleIOSim implements ModuleIO { + private static final double LOOP_PERIOD_SECS = 0.02; + + private DCMotorSim driveSim = + new DCMotorSim(DCMotor.getNEO(1), moduleConstants.driveReduction(), 0.025); + private DCMotorSim turnSim = + new DCMotorSim(DCMotor.getNEO(1), moduleConstants.turnReduction(), 0.004); + + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(LOOP_PERIOD_SECS); + turnSim.update(LOOP_PERIOD_SECS); + + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + + inputs.turnAbsolutePosition = + new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } + + @Override + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } + + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + turnSim.setInputVoltage(turnAppliedVolts); + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java new file mode 100644 index 0000000..00fb8f0 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java @@ -0,0 +1,181 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.moduleConstants; +import static org.rambots.subsystems.drive.DriveConstants.odometryFrequency; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; +import java.util.OptionalDouble; +import java.util.Queue; + +/** + * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO + * or NEO 550), and analog absolute encoder connected to the RIO + * + *

NOTE: This implementation should be used as a starting point and adapted to different hardware + * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") + * + *

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOSparkMax implements ModuleIO { + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(DriveConstants.ModuleConfig config) { + // Init motor & encoder objects + driveSparkMax = new CANSparkMax(config.driveID(), MotorType.kBrushless); + turnSparkMax = new CANSparkMax(config.turnID(), MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); // MUST BE CALIBRATED + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(config.turnMotorInverted()); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = driveEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal( + () -> { + double value = turnRelativeEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition()) / moduleConstants.driveReduction(); + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) + / moduleConstants.driveReduction(); + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + + inputs.turnAbsolutePosition = + new Rotation2d( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations( + turnRelativeEncoder.getPosition() / moduleConstants.turnReduction()); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / moduleConstants.turnReduction(); + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value) / moduleConstants.driveReduction()) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map( + (Double value) -> Rotation2d.fromRotations(value / moduleConstants.turnReduction())) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java new file mode 100644 index 0000000..7528a0e --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java @@ -0,0 +1,197 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.canbus; +import static org.rambots.subsystems.drive.DriveConstants.moduleConstants; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder + * + *

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

To calibrate the absolute encoder offsets, point the modules straight (such that forward + * motion on the drive motor will propel the robot forward) and copy the reported values from the + * absolute encoders using AdvantageScope. These values are logged under + * "/Drive/ModuleX/TurnAbsolutePositionRad" + */ +public class ModuleIOTalonFX implements ModuleIO { + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOTalonFX(DriveConstants.ModuleConfig config) { + driveTalon = new TalonFX(config.driveID(), canbus); + turnTalon = new TalonFX(config.turnID(), canbus); + cancoder = new CANcoder(config.absoluteEncoderChannel(), canbus); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; + turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; + turnTalon.getConfigurator().apply(turnConfig); + setTurnBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.odometryFrequency, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) + / moduleConstants.driveReduction(); + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) + / moduleConstants.driveReduction(); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnPosition.getValueAsDouble() / moduleConstants.turnReduction()); + inputs.turnVelocityRadPerSec = + Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / moduleConstants.turnReduction(); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value) / moduleConstants.driveReduction()) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map( + (Double value) -> Rotation2d.fromRotations(value / moduleConstants.turnReduction())) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = + DriveConstants.moduleConfigs[0].turnMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java new file mode 100644 index 0000000..564bb86 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java @@ -0,0 +1,140 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.odometryFrequency; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.ParentDevice; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + private boolean isCANFD = false; + + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } + + @Override + public void start() { + if (!timestampQueues.isEmpty()) { + super.start(); + } + } + + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + ArrayBlockingQueue queue = new ArrayBlockingQueue<>(10); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + isCANFD = CANBus.isNetworkFD(device.getNetwork()); + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + public Queue makeTimestampQueue() { + ArrayBlockingQueue queue = new ArrayBlockingQueue<>(10); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD) { + BaseStatusSignal.waitForAll(2.0 / odometryFrequency, signals); + } else { + // "waitForAll" does not support blocking on multiple + // signals with a bus that is not CAN FD, regardless + // of Pro licensing. No reasoning for this behavior + // is provided by the documentation. + Thread.sleep((long) (1000.0 / odometryFrequency)); + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drive.odometryLock.lock(); + try { + double timestamp = Logger.getRealTimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } +} diff --git a/src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java new file mode 100644 index 0000000..53cea1a --- /dev/null +++ b/src/main/java/org/rambots/subsystems/drive/SparkMaxOdometryThread.java @@ -0,0 +1,107 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.drive; + +import static org.rambots.subsystems.drive.DriveConstants.odometryFrequency; + +import edu.wpi.first.wpilibj.Notifier; +import java.util.ArrayList; +import java.util.List; +import java.util.OptionalDouble; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for devices like the SparkMax that require polling rather than a + * blocking thread. A Notifier thread is used to gather samples with consistent timing. + */ +public class SparkMaxOdometryThread { + private List> signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + private List> timestampQueues = new ArrayList<>(); + + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; + + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } + + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + } + + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic(1.0 / odometryFrequency); + } + } + + public Queue registerSignal(Supplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + private void periodic() { + Drive.odometryLock.lock(); + double timestamp = Logger.getRealTimestamp() / 1e6; + try { + double[] values = new double[signals.size()]; + boolean isValid = true; + for (int i = 0; i < signals.size(); i++) { + OptionalDouble value = signals.get(i).get(); + if (value.isPresent()) { + values[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } + } + if (isValid) { + for (int i = 0; i < signals.size(); i++) { + queues.get(i).offer(values[i]); + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + Drive.odometryLock.unlock(); + } + } +} diff --git a/src/main/java/org/rambots/subsystems/flywheel/Flywheel.java b/src/main/java/org/rambots/subsystems/flywheel/Flywheel.java new file mode 100644 index 0000000..10d92d9 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/flywheel/Flywheel.java @@ -0,0 +1,110 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.flywheel; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.rambots.Constants; + +public class Flywheel extends SubsystemBase { + private final FlywheelIO io; + private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); + private final SimpleMotorFeedforward ffModel; + private final SysIdRoutine sysId; + + /** Creates a new Flywheel. */ + public Flywheel(FlywheelIO io) { + this.io = io; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.getMode()) { + case REAL: + case REPLAY: + ffModel = new SimpleMotorFeedforward(0.1, 0.05); + io.configurePID(1.0, 0.0, 0.0); + break; + case SIM: + ffModel = new SimpleMotorFeedforward(0.0, 0.03); + io.configurePID(0.5, 0.0, 0.0); + break; + default: + ffModel = new SimpleMotorFeedforward(0.0, 0.0); + break; + } + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Flywheel", inputs); + } + + /** Run open loop at the specified voltage. */ + public void runVolts(double volts) { + io.setVoltage(volts); + } + + /** Run closed loop at the specified velocity. */ + public void runVelocity(double velocityRPM) { + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); + io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + + // Log flywheel setpoint + Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); + } + + /** Stops the flywheel. */ + public void stop() { + io.stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Returns the current velocity in RPM. */ + @AutoLogOutput + public double getVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); + } + + /** Returns the current velocity in radians per second. */ + public double getCharacterizationVelocity() { + return inputs.velocityRadPerSec; + } +} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java new file mode 100644 index 0000000..11b44b7 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java @@ -0,0 +1,41 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + @AutoLog + public static class FlywheelIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FlywheelIOInputs inputs) {} + + /** Run open loop at the specified voltage. */ + public default void setVoltage(double volts) {} + + /** Run closed loop at the specified velocity. */ + public default void setVelocity(double velocityRadPerSec, double ffVolts) {} + + /** Stop in open loop. */ + public default void stop() {} + + /** Set velocity PID constants. */ + public default void configurePID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..55bb4a6 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java @@ -0,0 +1,68 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.flywheel; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class FlywheelIOSim implements FlywheelIO { + private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); + private PIDController pid = new PIDController(0.0, 0.0, 0.0); + + private boolean closedLoop = false; + private double ffVolts = 0.0; + private double appliedVolts = 0.0; + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + if (closedLoop) { + appliedVolts = + MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); + sim.setInputVoltage(appliedVolts); + } + + sim.update(0.02); + + inputs.positionRad = 0.0; + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + } + + @Override + public void setVoltage(double volts) { + closedLoop = false; + appliedVolts = volts; + sim.setInputVoltage(volts); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + closedLoop = true; + pid.setSetpoint(velocityRadPerSec); + this.ffVolts = ffVolts; + } + + @Override + public void stop() { + setVoltage(0.0); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java new file mode 100644 index 0000000..045c30c --- /dev/null +++ b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java @@ -0,0 +1,89 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.flywheel; + +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.math.util.Units; + +/** + * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with + * "CANSparkFlex". + */ +public class FlywheelIOSparkMax implements FlywheelIO { + private static final double GEAR_RATIO = 1.5; + + private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); + private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); + private final RelativeEncoder encoder = leader.getEncoder(); + private final SparkPIDController pid = leader.getPIDController(); + + public FlywheelIOSparkMax() { + leader.restoreFactoryDefaults(); + follower.restoreFactoryDefaults(); + + leader.setCANTimeout(250); + follower.setCANTimeout(250); + + leader.setInverted(false); + follower.follow(leader, false); + + leader.enableVoltageCompensation(12.0); + leader.setSmartCurrentLimit(30); + + leader.burnFlash(); + follower.burnFlash(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); + inputs.velocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); + inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); + inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; + } + + @Override + public void setVoltage(double volts) { + leader.setVoltage(volts); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + pid.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + ControlType.kVelocity, + 0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + leader.stopMotor(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setP(kP, 0); + pid.setI(kI, 0); + pid.setD(kD, 0); + pid.setFF(0, 0); + } +} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 0000000..606bc31 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,98 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package org.rambots.subsystems.flywheel; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + +public class FlywheelIOTalonFX implements FlywheelIO { + private static final double GEAR_RATIO = 1.5; + + private final TalonFX leader = new TalonFX(0); + private final TalonFX follower = new TalonFX(1); + + private final StatusSignal leaderPosition = leader.getPosition(); + private final StatusSignal leaderVelocity = leader.getVelocity(); + private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); + private final StatusSignal leaderCurrent = leader.getStatorCurrent(); + private final StatusSignal followerCurrent = follower.getStatorCurrent(); + + public FlywheelIOTalonFX() { + var config = new TalonFXConfiguration(); + config.CurrentLimits.StatorCurrentLimit = 30.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + leader.getConfigurator().apply(config); + follower.getConfigurator().apply(config); + follower.setControl(new Follower(leader.getDeviceID(), false)); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + leader.optimizeBusUtilization(); + follower.optimizeBusUtilization(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + BaseStatusSignal.refreshAll( + leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); + inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; + inputs.velocityRadPerSec = + Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; + inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); + inputs.currentAmps = + new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; + } + + @Override + public void setVoltage(double volts) { + leader.setControl(new VoltageOut(volts)); + } + + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + leader.setControl( + new VelocityVoltage( + Units.radiansToRotations(velocityRadPerSec), + 0.0, + true, + ffVolts, + 0, + false, + false, + false)); + } + + @Override + public void stop() { + leader.stopMotor(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + var config = new Slot0Configs(); + config.kP = kP; + config.kI = kI; + config.kD = kD; + leader.getConfigurator().apply(config); + } +} diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java new file mode 100644 index 0000000..dee6906 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java @@ -0,0 +1,235 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.rambots.subsystems.vision; + +import static org.rambots.subsystems.drive.DriveConstants.thetaStdDevCoefficient; +import static org.rambots.subsystems.drive.DriveConstants.xyStdDevCoefficient; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Optional; +import java.util.function.Consumer; +import org.littletonrobotics.junction.Logger; +import org.rambots.util.FieldConstants; +import org.rambots.util.VisionHelpers; + +public class AprilTagVision extends SubsystemBase { + + // Time interval for logging tag poses + private static final double targetLogTimeSecs = 0.1; + + // Margin around the field border + private static final double fieldBorderMargin = 0.5; + + // Margin for the z-axis + private static final double zMargin = 0.75; + + // Path for logging vision data + private static final String VISION_PATH = "AprilTagVision/Inst"; + + private boolean enableVisionUpdates = true; + + private Consumer> visionConsumer = x -> {}; + private Map lastFrameTimes = new HashMap<>(); + private Map lastTagDetectionTimes = new HashMap<>(); + + private final AprilTagVisionIO[] io; + private final AprilTagVisionIO.AprilTagVisionIOInputs[] inputs; + + public void setDataInterfaces( + Consumer> visionConsumer) { + this.visionConsumer = visionConsumer; + } + + public AprilTagVision(AprilTagVisionIO... io) { + System.out.println("[Init] Creating AprilTagVision"); + this.io = io; + inputs = new AprilTagVisionIO.AprilTagVisionIOInputs[io.length]; + for (int i = 0; i < io.length; i++) { + inputs[i] = new AprilTagVisionIO.AprilTagVisionIOInputs(); + } + + // Create map of last frame times for instances + for (int i = 0; i < io.length; i++) { + lastFrameTimes.put(i, 0.0); + } + + // Create map of last detection times for tags + FieldConstants.aprilTags.getTags().forEach(tag -> lastTagDetectionTimes.put(tag.ID, 0.0)); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs(VISION_PATH + Integer.toString(i), inputs[i]); + } + List visionUpdates = processPoseEstimates(); + sendResultsToPoseEstimator(visionUpdates); + } + + /** + * Process the pose estimates and generate vision updates. + * + * @return List of timestamped vision updates + */ + private List processPoseEstimates() { + List visionUpdates = new ArrayList<>(); + for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) { + for (VisionHelpers.PoseEstimate poseEstimates : inputs[instanceIndex].poseEstimates) { + if (shouldSkipPoseEstimate(poseEstimates)) { + continue; + } + double timestamp = poseEstimates.timestampSeconds(); + Pose3d robotPose = poseEstimates.pose(); + List tagPoses = getTagPoses(poseEstimates); + double xyStdDev = calculateXYStdDev(poseEstimates, tagPoses.size()); + double thetaStdDev = calculateThetaStdDev(poseEstimates, tagPoses.size()); + visionUpdates.add( + new VisionHelpers.TimestampedVisionUpdate( + timestamp, robotPose.toPose2d(), VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); + logData(instanceIndex, timestamp, robotPose, tagPoses); + } + } + return visionUpdates; + } + + /** + * Check if the pose estimate should be skipped. + * + * @param poseEstimates The pose estimate + * @return True if the pose estimate should be skipped, false otherwise + */ + private boolean shouldSkipPoseEstimate(VisionHelpers.PoseEstimate poseEstimates) { + return poseEstimates.tagIDs().length < 1 + || poseEstimates.pose() == null + || isOutsideFieldBorder(poseEstimates.pose()); + } + + /** + * Check if the robot pose is outside the field border. + * + * @param robotPose The robot pose + * @return True if the robot pose is outside the field border, false otherwise + */ + private boolean isOutsideFieldBorder(Pose3d robotPose) { + return robotPose.getX() < -fieldBorderMargin + || robotPose.getX() > FieldConstants.fieldLength + fieldBorderMargin + || robotPose.getY() < -fieldBorderMargin + || robotPose.getY() > FieldConstants.fieldWidth + fieldBorderMargin + || robotPose.getZ() < -zMargin + || robotPose.getZ() > zMargin; + } + + /** + * Get the poses of the detected tags. + * + * @param poseEstimates The pose estimate + * @return List of tag poses + */ + private List getTagPoses(VisionHelpers.PoseEstimate poseEstimates) { + List tagPoses = new ArrayList<>(); + Arrays.stream(poseEstimates.tagIDs()) + .forEachOrdered( + tagId -> { + lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp()); + Optional tagPose = FieldConstants.aprilTags.getTagPose(tagId); + tagPose.ifPresent(tagPoses::add); + }); + return tagPoses; + } + + /** + * Calculate the standard deviation of the x and y coordinates. + * + * @param poseEstimates The pose estimate + * @param tagPosesSize The number of detected tag poses + * @return The standard deviation of the x and y coordinates + */ + private double calculateXYStdDev(VisionHelpers.PoseEstimate poseEstimates, int tagPosesSize) { + return xyStdDevCoefficient * Math.pow(poseEstimates.averageTagDistance(), 2.0) / tagPosesSize; + } + + /** + * Calculate the standard deviation of the theta coordinate. + * + * @param poseEstimates The pose estimate + * @param tagPosesSize The number of detected tag poses + * @return The standard deviation of the theta coordinate + */ + private double calculateThetaStdDev(VisionHelpers.PoseEstimate poseEstimates, int tagPosesSize) { + return thetaStdDevCoefficient + * Math.pow(poseEstimates.averageTagDistance(), 2.0) + / tagPosesSize; + } + + /** + * Log the data for a specific instance. + * + * @param instanceIndex The index of the instance + * @param timestamp The timestamp of the data + * @param robotPose The robot pose + * @param tagPoses The tag poses + */ + private void logData( + int instanceIndex, double timestamp, Pose3d robotPose, List tagPoses) { + Logger.recordOutput( + VISION_PATH + Integer.toString(instanceIndex) + "/LatencySecs", + Timer.getFPGATimestamp() - timestamp); + Logger.recordOutput( + VISION_PATH + Integer.toString(instanceIndex) + "/RobotPose", robotPose.toPose2d()); + Logger.recordOutput(VISION_PATH + Integer.toString(instanceIndex) + "/RobotPose3D", robotPose); + Logger.recordOutput( + VISION_PATH + Integer.toString(instanceIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[tagPoses.size()])); + logTagPoses(); + } + + /** Log the poses of the detected tags. */ + private void logTagPoses() { + List allTagPoses = new ArrayList<>(); + for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { + if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { + Optional tagPose = FieldConstants.aprilTags.getTagPose(detectionEntry.getKey()); + if (tagPose.isPresent()) { + allTagPoses.add(tagPose.get()); + } + } + } + Logger.recordOutput( + "AprilTagVision/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); + } + + /** + * Send the vision updates to the pose estimator. + * + * @param visionUpdates The list of vision updates + */ + private void sendResultsToPoseEstimator( + List visionUpdates) { + if (enableVisionUpdates) { + visionConsumer.accept(visionUpdates); + } + } + + /** + * Set whether to enable vision updates. + * + * @param enableVisionUpdates True to enable vision updates, false otherwise + */ + public void setEnableVisionUpdates(boolean enableVisionUpdates) { + this.enableVisionUpdates = enableVisionUpdates; + } +} diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIO.java new file mode 100644 index 0000000..04ea672 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIO.java @@ -0,0 +1,58 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.rambots.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import java.util.ArrayList; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.rambots.util.LimelightHelpers; +import org.rambots.util.VisionHelpers; + +public interface AprilTagVisionIO { + class AprilTagVisionIOInputs implements LoggableInputs { + + ArrayList poseEstimates = new ArrayList<>(); + + @Override + public void toLog(LogTable table) { + table.put("poseEstimates", poseEstimates.size()); + for (VisionHelpers.PoseEstimate poseEstimate : poseEstimates) { + int posePosition = poseEstimates.indexOf(poseEstimate); + table.put( + "estimatedPose/" + Integer.toString(posePosition), + VisionHelpers.getPose3dToArray(poseEstimate.pose())); + table.put( + "captureTimestamp/" + Integer.toString(posePosition), poseEstimate.timestampSeconds()); + table.put("tagIDs/" + Integer.toString(posePosition), poseEstimate.tagIDs()); + table.put( + "averageTagDistance/" + Integer.toString(posePosition), + poseEstimate.averageTagDistance()); + } + table.put("valid", !poseEstimates.isEmpty()); + } + + @Override + public void fromLog(LogTable table) { + int estimatedPoseCount = table.get("poseEstimates", 0); + for (int i = 0; i < estimatedPoseCount; i++) { + Pose3d poseEstimation = + LimelightHelpers.toPose3D( + table.get("estimatedPose/" + Integer.toString(i), new double[] {})); + double timestamp = table.get("captureTimestamp/" + Integer.toString(i), 0.0); + double averageTagDistance = table.get("averageTagDistance/" + Integer.toString(i), 0.0); + int[] tagIDs = table.get("tagIDs/" + Integer.toString(i), new int[] {}); + poseEstimates.add( + new VisionHelpers.PoseEstimate(poseEstimation, timestamp, averageTagDistance, tagIDs)); + } + table.get("valid", false); + } + } + + default void updateInputs(AprilTagVisionIOInputs inputs) {} +} diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java new file mode 100644 index 0000000..2c134b3 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java @@ -0,0 +1,108 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.rambots.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.networktables.TimestampedString; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.ArrayList; +import java.util.Optional; +import org.rambots.util.LimelightHelpers; +import org.rambots.util.VisionHelpers; + +/** This class represents the implementation of AprilTagVisionIO using Limelight camera. */ +public class AprilTagVisionIOLimelight implements AprilTagVisionIO { + + String limelightName; + private final StringSubscriber observationSubscriber; + + /** + * Constructs a new AprilTagVisionIOLimelight instance. + * + * @param identifier The identifier of the Limelight camera. + */ + public AprilTagVisionIOLimelight(String identifier) { + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable(identifier); + LimelightHelpers.setPipelineIndex(limelightName, 0); + + observationSubscriber = + limelightTable + .getStringTopic("json") + .subscribe("", PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); + } + + /** + * Updates the inputs for AprilTag vision. + * + * @param inputs The AprilTagVisionIOInputs object containing the inputs. + */ + @Override + public void updateInputs(AprilTagVisionIOInputs inputs) { + TimestampedString[] queue = + observationSubscriber.readQueue(); // Reads the queue of timestamped strings + ArrayList poseEstimates = + new ArrayList<>(); // Creates an empty ArrayList to store pose estimates + + // Iterates over each timestamped string in the queue + for (int i = 0; i < Math.min(queue.length, 3); i++) { + TimestampedString timestampedString = queue[i]; + double timestamp = timestampedString.timestamp / 1e6; // Converts the timestamp to seconds + LimelightHelpers.Results results = + LimelightHelpers.parseJsonDump(timestampedString.value) + .targetingResults; // Parses the JSON dump and retrieves the targeting results + Optional allianceOptional = + DriverStation.getAlliance(); // Retrieves the alliance information from the DriverStation + + // Checks if there are no targets or if the alliance information is not present + if (results.targets_Fiducials.length == 0 || !allianceOptional.isPresent()) { + continue; // Skips to the next iteration of the loop + } + + double latencyMS = + results.latency_capture + + results.latency_pipeline; // Calculates the total latency in milliseconds + Pose3d poseEstimation = + results.getBotPose3d_wpiBlue(); // Retrieves the pose estimation for the robot + double averageTagDistance = 0.0; // Initializes the average tag distance to 0.0 + timestamp -= (latencyMS / 1e3); // Adjusts the timestamp by subtracting the latency in seconds + + int[] tagIDs = + new int[results.targets_Fiducials.length]; // Creates an array to store the tag IDs + + // Iterates over each target in the targeting results + for (int aprilTags = 0; aprilTags < results.targets_Fiducials.length; aprilTags++) { + tagIDs[aprilTags] = + (int) + results.targets_Fiducials[aprilTags].fiducialID; // Retrieves and stores the tag ID + averageTagDistance += + results + .targets_Fiducials[aprilTags] + .getTargetPose_CameraSpace() + .getTranslation() + .getNorm(); // Calculates the sum of the tag distances + } + + averageTagDistance /= tagIDs.length; // Calculates the average tag distance + poseEstimates.add( + new VisionHelpers.PoseEstimate( + poseEstimation, + timestamp, + averageTagDistance, + tagIDs)); // Creates a new PoseEstimate object and adds it to the poseEstimates + // ArrayList + } + + inputs.poseEstimates = + poseEstimates; // Assigns the poseEstimates ArrayList to the inputs.poseEstimates variable + } +} diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOPhotonVisionSIM.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOPhotonVisionSIM.java new file mode 100644 index 0000000..9495510 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOPhotonVisionSIM.java @@ -0,0 +1,145 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.rambots.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import java.util.ArrayList; +import java.util.Optional; +import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; +import org.rambots.util.FieldConstants; +import org.rambots.util.VisionHelpers; + +public class AprilTagVisionIOPhotonVisionSIM implements AprilTagVisionIO { + private final PhotonCamera camera; + private final PhotonPoseEstimator photonEstimator; + private final VisionSystemSim visionSim; + private final PhotonCameraSim cameraSim; + + private double lastEstTimestamp = 0; + private final Supplier poseSupplier; + + /** + * Constructs a new AprilTagVisionIOPhotonVisionSIM instance. + * + * @param identifier The identifier of the PhotonCamera. + * @param robotToCamera The transform from the robot's coordinate system to the camera's + * coordinate system. + * @param poseSupplier The supplier of the robot's pose. + */ + public AprilTagVisionIOPhotonVisionSIM( + String identifier, Transform3d robotToCamera, Supplier poseSupplier) { + camera = new PhotonCamera(identifier); + photonEstimator = + new PhotonPoseEstimator( + FieldConstants.aprilTags, + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + robotToCamera); + photonEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + // Create the vision system simulation which handles cameras and targets on the + // field. + visionSim = new VisionSystemSim("main"); + // Add all the AprilTags inside the tag layout as visible targets to this + // simulated field. + visionSim.addAprilTags(FieldConstants.aprilTags); + // Create simulated camera properties. These can be set to mimic your actual + // camera. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(90)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(15); + cameraProp.setAvgLatencyMs(50); + cameraProp.setLatencyStdDevMs(15); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values + // with visible + // targets. + cameraSim = new PhotonCameraSim(camera, cameraProp); + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(cameraSim, robotToCamera); + cameraSim.enableDrawWireframe(true); + this.poseSupplier = poseSupplier; + } + + /** + * Updates the inputs for AprilTag vision. + * + * @param inputs The AprilTagVisionIOInputs object containing the inputs. + */ + @Override + public void updateInputs(AprilTagVisionIOInputs inputs) { + visionSim.update(poseSupplier.get()); + PhotonPipelineResult results = cameraSim.getCamera().getLatestResult(); + ArrayList poseEstimates = new ArrayList<>(); + double timestamp = results.getTimestampSeconds(); + Optional allianceOptional = DriverStation.getAlliance(); + if (!results.targets.isEmpty() && allianceOptional.isPresent()) { + double latencyMS = results.getLatencyMillis(); + Pose3d poseEstimation; + Optional estimatedPose = getEstimatedGlobalPose(); + if (estimatedPose.isEmpty()) { + return; + } + poseEstimation = estimatedPose.get().estimatedPose; + double averageTagDistance = 0.0; + timestamp -= (latencyMS / 1e3); + int[] tagIDs = new int[results.targets.size()]; + for (int i = 0; i < results.targets.size(); i++) { + tagIDs[i] = results.targets.get(i).getFiducialId(); + var tagPose = photonEstimator.getFieldTags().getTagPose(tagIDs[i]); + if (tagPose.isEmpty()) { + continue; + } + averageTagDistance += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(poseEstimation.getTranslation().toTranslation2d()); + } + averageTagDistance /= tagIDs.length; + poseEstimates.add( + new VisionHelpers.PoseEstimate(poseEstimation, timestamp, averageTagDistance, tagIDs)); + inputs.poseEstimates = poseEstimates; + } + } + + /** Updates the PhotonPoseEstimator and returns the estimated global pose. */ + public Optional getEstimatedGlobalPose() { + var visionEst = photonEstimator.update(); + double latestTimestamp = camera.getLatestResult().getTimestampSeconds(); + boolean newResult = Math.abs(latestTimestamp - lastEstTimestamp) > 1e-5; + visionEst.ifPresentOrElse( + est -> + getSimDebugField().getObject("VisionEstimation").setPose(est.estimatedPose.toPose2d()), + () -> { + if (newResult) getSimDebugField().getObject("VisionEstimation").setPoses(); + }); + if (newResult) lastEstTimestamp = latestTimestamp; + return visionEst; + } + + /** A Field2d for visualizing our robot and objects on the field. */ + public Field2d getSimDebugField() { + if (!RobotBase.isSimulation()) return null; + return visionSim.getDebugField(); + } +} diff --git a/src/main/java/org/rambots/util/Alert.java b/src/main/java/org/rambots/util/Alert.java new file mode 100644 index 0000000..07159b4 --- /dev/null +++ b/src/main/java/org/rambots/util/Alert.java @@ -0,0 +1,147 @@ +package org.rambots.util; + +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Predicate; + +/** Class for managing persistent alerts to be sent over NetworkTables. */ +public class Alert { + private static Map groups = new HashMap(); + + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, + * the appropriate entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate + * entries will be added to NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) { + if (!groups.containsKey(group)) { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also + * be sent to the console. + */ + public void set(boolean active) { + if (active && !this.active) { + activeStartTime = Timer.getFPGATimestamp(); + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case INFO: + System.out.println(text); + break; + } + } + this.active = active; + } + + /** Updates current alert text. */ + public void setText(String text) { + if (active && !text.equals(this.text)) { + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case INFO: + System.out.println(text); + break; + } + } + this.text = text; + } + + private static class SendableAlerts implements Sendable { + public final List alerts = new ArrayList<>(); + + public String[] getStrings(AlertType type) { + Predicate activeFilter = (Alert x) -> x.type == type && x.active; + Comparator timeSorter = + (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); + return alerts.stream() + .filter(activeFilter) + .sorted(timeSorter) + .map((Alert a) -> a.text) + .toArray(String[]::new); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); + } + } + + /** Represents an alert's level of urgency. */ + public static enum AlertType { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + */ + ERROR, + + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + */ + WARNING, + + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type + * for problems which are unlikely to affect the robot's functionality, or any other alerts + * which do not fall under "ERROR" or "WARNING". + */ + INFO + } +} diff --git a/src/main/java/org/rambots/util/AllianceFlipUtil.java b/src/main/java/org/rambots/util/AllianceFlipUtil.java new file mode 100644 index 0000000..62b2f4a --- /dev/null +++ b/src/main/java/org/rambots/util/AllianceFlipUtil.java @@ -0,0 +1,77 @@ +package org.rambots.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; + +/** Utility functions for flipping from the blue to red alliance. */ +public class AllianceFlipUtil { + /** + * Flips an x coordinate to the correct side of the field based on the current alliance color. + * + * @param xCoordinate The x coordinate to be flipped. + * @return The flipped x coordinate. + */ + public static double apply(double xCoordinate) { + if (shouldFlip()) { + return FieldConstants.fieldLength - xCoordinate; + } else { + return xCoordinate; + } + } + + /** + * Flips a translation to the correct side of the field based on the current alliance color. + * + * @param translation The translation to be flipped. + * @return The flipped translation. + */ + public static Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(apply(translation.getX()), translation.getY()); + } else { + return translation; + } + } + + /** + * Flips a rotation based on the current alliance color. + * + * @param rotation The rotation to be flipped. + * @return The flipped rotation. + */ + public static Rotation2d apply(Rotation2d rotation) { + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else { + return rotation; + } + } + + /** + * Flips a pose to the correct side of the field based on the current alliance color. + * + * @param pose The pose to be flipped. + * @return The flipped pose. + */ + public static Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } else { + return pose; + } + } + + /** + * Checks if the alliance color should be flipped. + * + * @return True if the alliance color should be flipped, false otherwise. + */ + public static boolean shouldFlip() { + Optional optional = DriverStation.getAlliance(); + return optional.isPresent() && optional.get() == Alliance.Red; + } +} diff --git a/src/main/java/org/rambots/util/FieldConstants.java b/src/main/java/org/rambots/util/FieldConstants.java new file mode 100644 index 0000000..988f6b5 --- /dev/null +++ b/src/main/java/org/rambots/util/FieldConstants.java @@ -0,0 +1,96 @@ +package org.rambots.util; + +import static edu.wpi.first.apriltag.AprilTagFields.k2024Crescendo; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import java.io.IOException; + +/** + * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets + * of corners start in the lower left moving clockwise. All units in Meters
+ *
+ * + *

All translations and poses are stored with the origin at the rightmost point on the BLUE + * ALLIANCE wall.
+ *
+ * Length refers to the x direction (as described by wpilib)
+ * Width refers to the y direction (as described by wpilib) + */ +public class FieldConstants { + public static double fieldLength = Units.inchesToMeters(651.223); + public static double fieldWidth = Units.inchesToMeters(323.277); + public static double wingX = Units.inchesToMeters(229.201); + public static double podiumX = Units.inchesToMeters(126.75); + public static double startingLineX = Units.inchesToMeters(74.111); + + public static Translation2d ampCenter = + new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + + /** Staging locations for each note */ + public static final class StagingLocations { + public static double centerlineX = fieldLength / 2.0; + + // need to update + public static double centerlineFirstY = Units.inchesToMeters(29.638); + public static double centerlineSeparationY = Units.inchesToMeters(66); + public static double spikeX = Units.inchesToMeters(114); + // need + public static double spikeFirstY = Units.inchesToMeters(161.638); + public static double spikeSeparationY = Units.inchesToMeters(57); + + public static Translation2d[] centerlineTranslations = new Translation2d[5]; + public static Translation2d[] spikeTranslations = new Translation2d[3]; + + static { + for (int i = 0; i < centerlineTranslations.length; i++) { + centerlineTranslations[i] = + new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); + } + } + + static { + for (int i = 0; i < spikeTranslations.length; i++) { + spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY)); + } + } + } + + /** Each corner of the speaker * */ + public static final class Speaker { + + /** Center of the speaker opening (blue alliance) */ + public static Pose2d centerSpeakerOpening = + new Pose2d(0.0, fieldWidth - Units.inchesToMeters(104.0), new Rotation2d()); + } + + // corners (blue alliance origin) + public static Translation3d topRightSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(13.091)); + + public static Translation3d topLeftSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(197.765), + Units.inchesToMeters(83.091)); + + public static Translation3d bottomRightSpeaker = + new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); + public static Translation3d bottomLeftSpeaker = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + + public static double aprilTagWidth = Units.inchesToMeters(6.50); + public static AprilTagFieldLayout aprilTags; + + static { + try { + aprilTags = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); + } + } +} diff --git a/src/main/java/org/rambots/util/LimelightHelpers.java b/src/main/java/org/rambots/util/LimelightHelpers.java new file mode 100644 index 0000000..e2d010a --- /dev/null +++ b/src/main/java/org/rambots/util/LimelightHelpers.java @@ -0,0 +1,790 @@ +// LimelightHelpers v1.2.1 (March 1, 2023) + +package org.rambots.util; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode {} + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() {} + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() {} + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public LimelightResults() { + targetingResults = new Results(); + } + } + + private static ObjectMapper mapper; + + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + public static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d( + Units.degreesToRadians(inData[3]), + Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** The LEDs will be controlled by Limelight pipeline settings, and not by robot code. */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + /** + * Sets the crop window. The crop window in the UI must be completely open for dynamic cropping to + * work. + */ + public static void setCropWindow( + String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace( + String limelightName, + double forward, + double side, + double up, + double roll, + double pitch, + double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** Asynchronously take snapshot. */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync( + () -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** Parses Limelight's JSON results dump into a LimelightResults Object */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } + + public static LimelightResults parseJsonDump(String jsonDump) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(jsonDump, LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} diff --git a/src/main/java/org/rambots/util/LocalADStarAK.java b/src/main/java/org/rambots/util/LocalADStarAK.java new file mode 100644 index 0000000..9df7522 --- /dev/null +++ b/src/main/java/org/rambots/util/LocalADStarAK.java @@ -0,0 +1,150 @@ +package org.rambots.util; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/org/rambots/util/LoggedTunableNumber.java b/src/main/java/org/rambots/util/LoggedTunableNumber.java new file mode 100644 index 0000000..b0c28a8 --- /dev/null +++ b/src/main/java/org/rambots/util/LoggedTunableNumber.java @@ -0,0 +1,94 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.rambots.util; + +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import org.rambots.Constants; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Constants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } +} diff --git a/src/main/java/org/rambots/util/VisionHelpers.java b/src/main/java/org/rambots/util/VisionHelpers.java new file mode 100644 index 0000000..53912ff --- /dev/null +++ b/src/main/java/org/rambots/util/VisionHelpers.java @@ -0,0 +1,140 @@ +package org.rambots.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import java.util.Arrays; +import java.util.Objects; + +/** + * The VisionHelpers class provides utility methods and record classes for vision-related + * operations. + */ +public class VisionHelpers { + + /** + * Represents a pose estimate with additional information. + * + * @param pose The pose (position and orientation) estimate. + * @param timestampSeconds The timestamp in seconds when the pose estimate was recorded. + * @param averageTagDistance The average distance to the detected tags. + * @param tagIDs The IDs of the detected tags. + */ + public record PoseEstimate( + /** The pose (position and orientation) estimate. */ + Pose3d pose, + /** The timestamp in seconds when the pose estimate was recorded. */ + double timestampSeconds, + /** The average distance to the detected tags. */ + double averageTagDistance, + /** The IDs of the detected tags. */ + int[] tagIDs) { + + /** + * Checks if this pose estimate is equal to another object. + * + * @param obj The object to compare. + * @return True if the objects are equal, false otherwise. + */ + @Override + public boolean equals(Object obj) { + if (this == obj) { + return true; + } + if (obj == null || getClass() != obj.getClass()) { + return false; + } + PoseEstimate other = (PoseEstimate) obj; + return Arrays.equals(tagIDs, other.tagIDs) + && Objects.equals(pose, other.pose) + && Double.compare(timestampSeconds, other.timestampSeconds) == 0 + && Double.compare(averageTagDistance, other.averageTagDistance) == 0; + } + + /** + * Computes the hash code of this pose estimate. + * + * @return The hash code value. + */ + @Override + public int hashCode() { + return Objects.hash( + Arrays.hashCode(getPose3dToArray(pose)), + timestampSeconds, + averageTagDistance, + Arrays.hashCode(tagIDs)); + } + + /** + * Returns a string representation of this pose estimate. + * + * @return The string representation. + */ + @Override + public String toString() { + return "PoseEstimate{" + + "pose=" + + pose.toString() + + ", timestampSeconds=" + + Double.toString(timestampSeconds) + + ", averageTagDistance=" + + Double.toString(averageTagDistance) + + ", tagIDs=" + + Arrays.toString(tagIDs) + + '}'; + } + } + + /** + * Converts a Pose3d object to an array of doubles. + * + * @param pose The Pose3d object to convert. + * @return The array of doubles representing the pose. + */ + public static double[] getPose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Represents a timestamped vision update with pose and standard deviations. + * + * @param timestamp The timestamp of the vision update. + * @param pose The pose estimate. + * @param stdDevs The standard deviations matrix. + */ + public record TimestampedVisionUpdate( + /** The timestamp of the vision update. */ + double timestamp, + /** The pose estimate. */ + Pose2d pose, + /** The standard deviations matrix. */ + Matrix stdDevs) { + + /** + * Returns a string representation of this vision update. + * + * @return The string representation. + */ + @Override + public String toString() { + return "VisionUpdate{" + + "timestamp=" + + Double.toString(timestamp) + + ", pose=" + + pose.toString() + + ", stdDevs=" + + stdDevs.toString() + + '}'; + } + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..c2d5892 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,42 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.1.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.1.0" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.1.0" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.1.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..a142879 --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..544956a --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..f572dcc --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 69a4079..42d1c3e 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,339 +1,339 @@ { - "fileName": "Phoenix6.json", - "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", - "frcYear": 2024, - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", - "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "24.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "24.1.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.1.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.1.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.1.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.1.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.1.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.1.0", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.1.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.1.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.1.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.1.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.1.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.1.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.1.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.1.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.1.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.1.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.1.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.1.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.1.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..6bb009c --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.1", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..d2fb2a5 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.8", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.8", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.8", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.8" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.8" + } + ] +}