From 57db9ac2c8be3a8ffe6d08a70fc06e6ad363077a Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Sat, 4 Jan 2025 03:14:58 -0500 Subject: [PATCH] New stuff (#22) * delete unused stuff * Refactor SwerveDrive to use DriveConstants for kinematics and remove unused test files * import stuff Co-authored-by: Jack * simulation stuff * format * noice * format * added some of the more important docstrings.. the rest will be added later --------- Co-authored-by: Jack --- .wpilib/wpilib_preferences.json | 10 +- build.gradle | 219 ++--- simgui-ds.json | 5 + simgui.json | 5 +- src/main/java/frc/robot/Constants.java | 89 +- src/main/java/frc/robot/Robot.java | 131 +-- src/main/java/frc/robot/RobotContainer.java | 324 ++++++- .../robot/commands/drive/DriveCommand.java | 4 +- .../commands/drive/DriveCommandBase.java | 15 +- .../extras/debug/LoggedTunableNumber.java | 122 --- .../interpolators/BilinearInterpolator.java | 266 ------ .../AdvancedSwerveModuleState.java | 27 + .../robot/extras/setpointGen/SPGCalcs.java | 285 ++++++ .../extras/setpointGen/SwerveSetpoint.java | 30 + .../setpointGen/SwerveSetpointGenerator.java | 363 ++++++++ .../simulation/field/SimulatedField.java | 22 + .../swerve/BrushlessMotorSim.java | 629 ++++++++----- .../mechanismSim/swerve/GyroSimulation.java | 2 +- .../swerve/SwerveModuleSimulation.java | 554 ++++++++--- .../robot/extras/util/AllianceFlipper.java | 10 +- .../java/frc/robot/extras/util/GeomUtil.java | 31 + .../frc/robot/extras/util/JoystickUtil.java | 102 +++ .../java/frc/robot/extras/util/MathUtil.java | 3 +- .../util/ProceduralStructGenerator.java | 857 ++++++++++++++++++ .../java/frc/robot/extras/util/TimeUtil.java | 24 +- .../extras/vision/MegatagPoseEstimate.java | 99 ++ .../subsystems/swerve/SwerveConstants.java | 220 ++--- .../robot/subsystems/swerve/SwerveDrive.java | 185 ++-- .../robot/subsystems/swerve/SwerveModule.java | 34 +- .../{gyroIO => gyro}/GyroInterface.java | 5 +- .../swerve/{gyroIO => gyro}/PhysicalGyro.java | 20 +- .../{gyroIO => gyro}/SimulatedGyro.java | 9 +- .../{moduleIO => module}/ModuleInterface.java | 5 +- .../{moduleIO => module}/PhysicalModule.java | 33 +- .../{moduleIO => module}/SimulatedModule.java | 65 +- .../swerve/odometryThread/OdometryThread.java | 5 +- .../odometryThread/OdometryThreadReal.java | 3 +- .../subsystems/vision/PhysicalVision.java | 415 +++++++++ .../subsystems/vision/SimulatedVision.java | 213 +++++ .../subsystems/vision/ThreadManager.java | 80 ++ .../frc/robot/subsystems/vision/Vision.java | 54 -- .../subsystems/vision/VisionConstants.java | 109 ++- .../frc/robot/subsystems/vision/VisionIO.java | 34 - .../robot/subsystems/vision/VisionIOReal.java | 439 --------- .../robot/subsystems/vision/VisionIOSim.java | 44 - .../subsystems/vision/VisionInterface.java | 95 ++ .../subsystems/vision/VisionSubsystem.java | 60 ++ src/test/java/SwerveDriveTest.java | 83 -- src/test/java/SwerveModuleTest.java | 39 - vendordeps/AdvantageKit.json | 72 +- vendordeps/ChoreoLib2025Beta.json | 86 +- vendordeps/Dyn4j.json | 44 +- vendordeps/NavX-2025.1.1-beta-1.json | 39 - vendordeps/Phoenix6-25.0.0-beta-4.json | 389 ++++++++ vendordeps/Phoenix6-replay-25.0.0-beta-1.json | 386 -------- vendordeps/Studica-2025.1.1-beta-4.json | 71 ++ vendordeps/photonlib.json | 71 ++ 57 files changed, 5102 insertions(+), 2533 deletions(-) delete mode 100644 src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java delete mode 100644 src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java create mode 100644 src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java create mode 100644 src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java create mode 100644 src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java create mode 100644 src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java create mode 100644 src/main/java/frc/robot/extras/util/JoystickUtil.java create mode 100644 src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java create mode 100644 src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/GyroInterface.java (86%) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/PhysicalGyro.java (71%) rename src/main/java/frc/robot/subsystems/swerve/{gyroIO => gyro}/SimulatedGyro.java (78%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/ModuleInterface.java (87%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/PhysicalModule.java (88%) rename src/main/java/frc/robot/subsystems/swerve/{moduleIO => module}/SimulatedModule.java (58%) create mode 100644 src/main/java/frc/robot/subsystems/vision/PhysicalVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/SimulatedVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/ThreadManager.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionInterface.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java delete mode 100644 src/test/java/SwerveDriveTest.java delete mode 100644 src/test/java/SwerveModuleTest.java delete mode 100644 vendordeps/NavX-2025.1.1-beta-1.json create mode 100644 vendordeps/Phoenix6-25.0.0-beta-4.json delete mode 100644 vendordeps/Phoenix6-replay-25.0.0-beta-1.json create mode 100644 vendordeps/Studica-2025.1.1-beta-4.json create mode 100644 vendordeps/photonlib.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index acab627..6b13a10 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2025beta", - "teamNumber": 4829 -} \ No newline at end of file + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025beta", + "teamNumber": 4829 +} diff --git a/build.gradle b/build.gradle index 6762242..1abcd7b 100644 --- a/build.gradle +++ b/build.gradle @@ -1,43 +1,11 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.25.0" + id "pmd" id "io.freefair.lombok" version "8.4" id "com.google.protobuf" version "0.9.4" - id "pmd" -} - -def grpcVersion = '1.61.0' // CURRENT_GRPC_VERSION -def protobufVersion = '3.25.1' -def protocVersion = protobufVersion - -protobuf { - protoc { - artifact = "com.google.protobuf:protoc:${protobufVersion}" - } - plugins { - grpc { - artifact = "io.grpc:protoc-gen-grpc-java:${grpcVersion}" - } - } - generateProtoTasks { - all()*.plugins { - grpc {} - } - } -} - -sourceSets { - main { - java { - srcDirs 'build/generated/source/proto/main/grpc' - srcDirs 'build/generated/source/proto/main/java' - } - proto { - srcDirs "trajectory_native/proto" - } - } } java { @@ -64,14 +32,25 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - gcType = edu.wpi.first.gradlerio.deploy.roborio.GarbageCollectorType.G1_Base + // https://www.chiefdelphi.com/t/2024-wpilib-feedback/464322/141 + final MAX_JAVA_HEAP_SIZE_MB = 100; + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + + // Set the minimum heap size to the maximum heap size to avoid resizing + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") } // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - predeploy << { execute 'rm -rf /home/lvuser/deploy' } files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' + // Change to true to delete files on roboRIO that no + // longer exist in deploy directory on roboRIO + deleteOldFiles = false } } } @@ -98,62 +77,15 @@ repositories { mavenLocal() } -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" classpath = sourceSets.main.runtimeClasspath } -compileJava.finalizedBy checkAkitInstall - - -//Check robot type when deploying -task(checkRobot, dependsOn: "classes", type: JavaExec) { - main = "frc.robot.Constants" - classpath = sourceSets.main.runtimeClasspath -} - -deployroborio.dependsOn(checkRobot) - -//Create commit with working changes on event branches -task(eventDeploy) { - doLast { - if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { - - def branchPrefix = "event" - def branch = 'git branch --show-current'.execute().text.trim() - def commitMessage = "Update at '${new Date().toString()}'" - - if (branch.startsWith(branchPrefix)) { - exec { - workingDir(projectDir) - executable 'git' - args 'add', '-A' - } - exec { - workingDir(projectDir) - executable 'git' - args 'commit', '-m', commitMessage - ignoreExitValue = true - } - - println "Committed to branch: '$branch'" - println "Commit message: '$commitMessage'" - } else { - println "Not on an event branch, skipping commit" - } - } else { - println "Not running deploy task, skipping commit" - } - } -} -createVersionFile.dependsOn(eventDeploy) // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -171,25 +103,13 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher:1.10.2' + implementation "gov.nist.math:jama:1.0.3" - - // Mockito for mocking objects in unit tests - testImplementation 'org.mockito:mockito-core:4.8.1' - testImplementation 'org.mockito:mockito-junit-jupiter:4.8.1' - - implementation 'gov.nist.math:jama:1.0.3' - - implementation "io.grpc:grpc-protobuf:${grpcVersion}" - implementation "io.grpc:grpc-services:${grpcVersion}" - implementation "io.grpc:grpc-stub:${grpcVersion}" - implementation "javax.annotation:javax.annotation-api:1.3.2" - runtimeOnly "io.grpc:grpc-netty-shaded:${grpcVersion}" - implementation "com.google.guava:guava:31.1-jre" + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } test { @@ -198,7 +118,12 @@ test { } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true +// +// The sim GUI is *disabled* by default to support running +// AdvantageKit log replay from the command line. Set the +// value to "true" to enable the sim GUI by default (this +// is the standard WPILib behavior). +wpi.sim.addGui().defaultEnabled = false wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') @@ -226,22 +151,58 @@ tasks.withType(JavaCompile) { } // Create version file -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "frc.robot" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" - indent = " " -} +// project.compileJava.dependsOn(createVersionFile) +// gversion { +// srcDir = "src/main/java/" +// classPackage = "frc.robot" +// className = "BuildConstants" +// dateFormat = "yyyy-MM-dd HH:mm:ss z" +// timeZone = "America/New_York" +// indent = " " +// } + +// TODO: set this shit up +// Create commit with working changes on event branches +// task(eventDeploy) { +// doLast { +// if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { +// def branchPrefix = "event" +// def branch = 'git branch --show-current'.execute().text.trim() +// def commitMessage = "Update at '${new Date().toString()}'" + +// if (branch.startsWith(branchPrefix)) { +// exec { +// workingDir(projectDir) +// executable 'git' +// args 'add', '-A' +// } +// exec { +// workingDir(projectDir) +// executable 'git' +// args 'commit', '-m', commitMessage +// ignoreExitValue = true +// } + +// println "Committed to branch: '$branch'" +// println "Commit message: '$commitMessage'" +// } else { +// println "Not on an event branch, skipping commit" +// } +// } else { +// println "Not running deploy task, skipping commit" +// } +// } +// } +// createVersionFile.dependsOn(eventDeploy) + // Spotless formatting +project.compileJava.dependsOn(spotlessApply) spotless { enforceCheck = false java { - target fileTree('.') { - include '**/*.java' - exclude '**/build/**', '**/build-*/**' + target fileTree(".") { + include "**/*.java" + exclude "**/build/**", "**/build-*/**" } toggleOffOn() googleJavaFormat() @@ -250,36 +211,32 @@ spotless { endWithNewline() } groovyGradle { - target fileTree('.') { - include '**/*.gradle' - exclude '**/build/**', '**/build-*/**' + target fileTree(".") { + include "**/*.gradle" + exclude "**/build/**", "**/build-*/**" } greclipse() indentWithSpaces(4) trimTrailingWhitespace() endWithNewline() } - format 'xml', { - target fileTree('.') { - include '**/*.xml' - exclude '**/build/**', '**/build-*/**' + json { + target fileTree(".") { + include "**/*.json" + exclude "**/build/**", "**/build-*/**" } - eclipseWtp('xml') - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() + gson().indentWithSpaces(2) } - format 'misc', { - target fileTree('.') { - include '**/*.md', '**/.gitignore' - exclude '**/build/**', '**/build-*/**' + format "misc", { + target fileTree(".") { + include "**/*.md", "**/.gitignore" + exclude "**/build/**", "**/build-*/**" } trimTrailingWhitespace() indentWithSpaces(2) endWithNewline() } } - // PMD configuration pmd { ignoreFailures = true diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/simgui.json b/simgui.json index 5f9d275..896ae55 100644 --- a/simgui.json +++ b/simgui.json @@ -1,7 +1,10 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/AdvantageKit/RealOutputs/Alerts": "Alerts", + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" } }, "NetworkTables Info": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eefde9d..cf81f13 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,22 +1,28 @@ package frc.robot; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.swerve.SwerveConstants.*; +import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; public final class Constants { - public class LogPaths { + public static final class LogPaths { public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/"; public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; } - public static final Mode currentMode = Mode.REAL; + public static final Mode CURRENT_MODE = Mode.SIM; public static enum Mode { /** Running on a real robot. */ REAL, + /** Running a physics simulator. */ SIM, + /** Replaying from a log file. */ REPLAY } @@ -41,6 +47,85 @@ public static final class HardwareConstants { public static final double MIN_FALCON_DEADBAND = 0.001; } + /** + * stores the constants and PID configs for chassis because we want an all-real simulation for the + * chassis, the numbers are required to be considerably precise + */ + public class SimulationConstants { + /** + * numbers that needs to be changed to fit each robot TODO: change these numbers to match your + * robot + */ + public static final double WHEEL_COEFFICIENT_OF_FRICTION = 0.95, + ROBOT_MASS_KG = 60; // with bumpers + + /** TODO: change motor type to match your robot */ + public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60(1), + STEER_MOTOR = DCMotor.getFalcon500(1); + + public static final double WHEEL_RADIUS_METERS = ModuleConstants.WHEEL_DIAMETER_METERS / 2.0, + DRIVE_GEAR_RATIO = ModuleConstants.DRIVE_GEAR_RATIO, + STEER_GEAR_RATIO = 11.0, + TIME_ROBOT_STOP_ROTATING_SECONDS = 0.06, + STEER_FRICTION_VOLTAGE = 0.12, + DRIVE_FRICTION_VOLTAGE = ModuleConstants.DRIVE_S, + DRIVE_INERTIA = 0.01, + STEER_INERTIA = 0.01; + + /* adjust current limit */ + public static final double DRIVE_CURRENT_LIMIT = ModuleConstants.DRIVE_STATOR_LIMIT; + // public static final double STEER_CURRENT_LIMIT = ModuleConstants.ST; + + /* equations that calculates some constants for the simulator (don't modify) */ + private static final double GRAVITY_CONSTANT = 9.81; + public static final double DRIVE_BASE_RADIUS = DriveConstants.MODULE_TRANSLATIONS[0].getNorm(), + /* friction_force = normal_force * coefficient_of_friction */ + MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION, + MAX_FRICTION_FORCE_PER_MODULE = + MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG / DriveConstants.MODULE_TRANSLATIONS.length, + /* force = torque / distance */ + MAX_PROPELLING_FORCE_NEWTONS = + DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, + /* floor_speed = wheel_angular_velocity * wheel_radius */ + CHASSIS_MAX_VELOCITY = + DRIVE_MOTOR.freeSpeedRadPerSec + / DRIVE_GEAR_RATIO + * WHEEL_RADIUS_METERS, // calculate using choreo + CHASSIS_MAX_ACCELERATION_MPS_SQ = + Math.min( + MAX_FRICTION_ACCELERATION, // cannot exceed max friction + MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG), + CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS, + CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = + CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS, + CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = + CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC / TIME_ROBOT_STOP_ROTATING_SECONDS; + + /* for collision detection in simulation */ + public static final double BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5), + BUMPER_LENGTH_METERS = Units.inchesToMeters(36), + /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */ + BUMPER_COEFFICIENT_OF_FRICTION = 0.75, + /* https://simple.wikipedia.org/wiki/Coefficient_of_restitution */ + BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08; + + /* Gyro Sim */ + public static final double GYRO_ANGULAR_ACCELERATION_THRESHOLD_SKIDDING_RAD_PER_SEC_SQ = 100; + public static final double SKIDDING_AMOUNT_AT_THRESHOLD_RAD = Math.toRadians(1.2); + /* + * https://store.ctr-electronics.com/pigeon-2/ + * for a well-installed one with vibration reduction, only 0.4 degree + * but most teams just install it directly on the rigid chassis frame (including my team :D) + * so at least 1.2 degrees of drifting in 1 minutes for an average angular velocity of 60 degrees/second + * which is the average velocity during normal swerve-circular-offense + * */ + public static final double NORMAL_GYRO_DRIFT_IN_1_MIN_Std_Dev_RAD = Math.toRadians(1.2); + public static final double AVERAGE_VELOCITY_RAD_PER_SEC_DURING_TEST = Math.toRadians(60); + + public static final int SIMULATION_TICKS_IN_1_PERIOD = 5; + public static final double SIMULATED_PERIOD_SECONDS = 0.02; + } + /** * This is where constants used to describe the game's field go. This will have the dimensions of * the field, but also the coordinates of obstacles, game pieces, or other places of interest. diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 41a77a0..5dd899d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,18 +1,14 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants.Mode; +import frc.robot.extras.simulation.field.SimulatedField; +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; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -23,62 +19,82 @@ public class Robot extends LoggedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; - private static final Mode JAVA_SIM_MODE = Mode.SIM; - public static final Mode CURRENT_ROBOT_MODE = isReal() ? Mode.REAL : JAVA_SIM_MODE; - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value - - if (isReal()) { - // Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - // new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging - } else { - setUseTiming(false); // Run as fast as possible - // String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope - // (or prompt the user) - // Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - // Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // - // Save outputs to a new log + public Robot() { + // 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); + + // This tells you if you have uncommitted changes in your project + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; } - // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the - // "Understanding Data Flow" page - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may - // be added. - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - // uncomment the line below if there is a USB camera plugged into the RoboRIO - // CameraServer.startAutomaticCapture(); + // Set up data receivers & replay source + switch (Constants.CURRENT_MODE) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + // Gets data from network tables + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // Start AdvantageKit logger + Logger.start(); + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); } - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ + /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. + // TODO: test this! + // Switch thread to high priority to improve loop timing + // Threads.setCurrentThreadPriority(true, 99); + + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. CommandScheduler.getInstance().run(); + + // Return to normal thread priority + // Threads.setCurrentThreadPriority(false, 10); } - /** This function is called once each time the robot enters Disabled mode. */ + /** This function is called once when the robot is disabled. */ @Override public void disabledInit() {} + /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() {} @@ -97,6 +113,7 @@ public void autonomousInit() { @Override public void autonomousPeriodic() {} + /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { // This makes sure that the autonomous stops running when @@ -106,13 +123,15 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - // m_robotContainer.teleopInit(); + + m_robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() {} + /** This function is called once when test mode is enabled. */ @Override public void testInit() { // Cancels all running commands at the start of test mode. @@ -123,6 +142,14 @@ public void testInit() { @Override public void testPeriodic() {} + /** This function is called once when the robot is first started up. */ @Override - public void simulationPeriodic() {} + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() { + SimulatedField.getInstance().simulationPeriodic(); + m_robotContainer.updateFieldSimAndDisplay(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..0f249f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,20 +1,328 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; +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.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.SimulationConstants; +import frc.robot.commands.drive.DriveCommand; +import frc.robot.extras.simulation.field.SimulatedField; +import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; +import frc.robot.extras.util.JoystickUtil; +import frc.robot.subsystems.swerve.SwerveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +import frc.robot.subsystems.swerve.SwerveDrive; +import frc.robot.subsystems.swerve.gyro.GyroInterface; +import frc.robot.subsystems.swerve.gyro.PhysicalGyro; +import frc.robot.subsystems.swerve.gyro.SimulatedGyro; +import frc.robot.subsystems.swerve.module.ModuleInterface; +import frc.robot.subsystems.swerve.module.PhysicalModule; +import frc.robot.subsystems.swerve.module.SimulatedModule; +import frc.robot.subsystems.vision.PhysicalVision; +import frc.robot.subsystems.vision.SimulatedVision; +import frc.robot.subsystems.vision.VisionInterface; +import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; public class RobotContainer { + + private final VisionSubsystem visionSubsystem; + private final SwerveDrive swerveDrive; + private final CommandXboxController operatorController = new CommandXboxController(1); + // private final Indexer indexer = new Indexer(new IndexerIOTalonFX()); + // private final Intake intake = new Intake(new IntakeIOTalonFX()); + // private final Pivot pivot = new Pivot(new PivotIOTalonFX()); + // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX()); + private final CommandXboxController driverController = new CommandXboxController(0); + + // Simulation, we store them here in the robot container + // private final SimulatedField simulatedArena; + private final SwerveDriveSimulation swerveDriveSimulation; + private final GyroSimulation gyroSimulation; + + // Subsystems + // private final XboxController driverController = new XboxController(0); + public RobotContainer() { - configureBindings(); + switch (Constants.CURRENT_MODE) { + case REAL -> { + /* Real robot, instantiate hardware IO implementations */ + + /* Disable Simulations */ + // this.simulatedArena = null; + this.gyroSimulation = null; + this.swerveDriveSimulation = null; + + swerveDrive = + new SwerveDrive( + new PhysicalGyro(), + new PhysicalModule(SwerveConstants.moduleConfigs[0]), + new PhysicalModule(SwerveConstants.moduleConfigs[1]), + new PhysicalModule(SwerveConstants.moduleConfigs[2]), + new PhysicalModule(SwerveConstants.moduleConfigs[3])); + visionSubsystem = new VisionSubsystem(new PhysicalVision()); + } + + case SIM -> { + /* Sim robot, instantiate physics sim IO implementations */ + + /* create simulations */ + /* create simulation for pigeon2 IMU (different IMUs have different measurement erros) */ + this.gyroSimulation = GyroSimulation.createNavX2(); + /* create a swerve drive simulation */ + this.swerveDriveSimulation = + new SwerveDriveSimulation( + SimulationConstants.ROBOT_MASS_KG, + DriveConstants.TRACK_WIDTH, + DriveConstants.WHEEL_BASE, + DriveConstants.TRACK_WIDTH + .2, + DriveConstants.WHEEL_BASE + .2, + SwerveModuleSimulation.getModule( + DCMotor.getFalcon500(1), + DCMotor.getFalcon500(1), + 60, + WHEEL_GRIP.TIRE_WHEEL, + ModuleConstants.DRIVE_GEAR_RATIO), + gyroSimulation, + new Pose2d(3, 3, new Rotation2d())); + SimulatedField.getInstance().addDriveTrainSimulation(swerveDriveSimulation); + swerveDrive = + new SwerveDrive( + new SimulatedGyro( + gyroSimulation), // SimulatedGyro is a wrapper around gyro simulation, that + // reads + // the simulation result + /* SimulatedModule are edited such that they also wraps around module simulations */ + new SimulatedModule(swerveDriveSimulation.getModules()[0]), + new SimulatedModule(swerveDriveSimulation.getModules()[1]), + new SimulatedModule(swerveDriveSimulation.getModules()[2]), + new SimulatedModule(swerveDriveSimulation.getModules()[3])); + + visionSubsystem = + new VisionSubsystem( + new SimulatedVision(() -> swerveDriveSimulation.getSimulatedDriveTrainPose())); + + SimulatedField.getInstance().resetFieldForAuto(); + resetFieldAndOdometryForAuto( + new Pose2d(1.3980597257614136, 5.493067741394043, Rotation2d.fromRadians(3.1415))); + } + + default -> { + visionSubsystem = new VisionSubsystem(new VisionInterface() {}); + /* Replayed robot, disable IO implementations */ + + /* physics simulations are also not needed */ + this.gyroSimulation = null; + this.swerveDriveSimulation = null; + // this.simulatedArena = null; + swerveDrive = + new SwerveDrive( + new GyroInterface() {}, + new ModuleInterface() {}, + new ModuleInterface() {}, + new ModuleInterface() {}, + new ModuleInterface() {}); + } + } + } + + private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance) { + final Pose2d startingPose = robotStartingPoseAtBlueAlliance; + + if (swerveDriveSimulation != null) { + swerveDriveSimulation.setSimulationWorldPose(startingPose); + SimulatedField.getInstance().resetFieldForAuto(); + updateFieldSimAndDisplay(); + } + + // swerveDrive.periodic(); + swerveDrive.setPose(startingPose); + } + + public void teleopInit() { + configureButtonBindings(); } - private void configureBindings() {} + // public void intakeCallback(boolean hasNote) { + // if (hasNote) { + // driverController.setRumble(RumbleType.kBothRumble, 0.1); + // operatorController.setRumble(RumbleType.kBothRumble, 1); + // } else { + // driverController.setRumble(RumbleType.kBothRumble, 0); + // operatorController.setRumble(RumbleType.kBothRumble, 0); + // } + // } + private void configureButtonBindings() { + DoubleSupplier driverLeftStickX = driverController::getLeftX; + DoubleSupplier driverLeftStickY = driverController::getLeftY; + DoubleSupplier driverRightStickX = driverController::getRightX; + DoubleSupplier driverLeftStick[] = + new DoubleSupplier[] { + () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[0], + () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[1] + }; + + DoubleSupplier operatorLeftStickX = operatorController::getLeftX; + DoubleSupplier operatorRightStickY = operatorController::getRightY; + + Trigger driverRightBumper = new Trigger(driverController.rightBumper()); + Trigger driverRightDirectionPad = new Trigger(driverController.pov(90)); + Trigger driverDownDirectionPad = new Trigger(driverController.pov(180)); + Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270)); + + // // autodrive + // Trigger driverAButton = new Trigger(driverController::getAButton); + // lol whatever + // // intake + // Trigger operatorLeftTrigger = new Trigger(()->operatorController.getLeftTriggerAxis() > 0.2); + // Trigger operatorLeftBumper = new Trigger(operatorController::getLeftBumper); + // // amp and speaker + // Trigger operatorBButton = new Trigger(operatorController::getBButton); + // Trigger operatorRightBumper = new Trigger(operatorController::getRightBumper); + // Trigger operatorRightTrigger = new Trigger(()->operatorController.getRightTriggerAxis() > + // 0.2); + // Trigger driverRightTrigger = new Trigger(()->driverController.getRightTriggerAxis() > 0.2); + + // // manual pivot and intake rollers + // Trigger operatorAButton = new Trigger(operatorController::getAButton); + // Trigger operatorXButton = new Trigger(operatorController::getXButton); + // Trigger operatorYButton = new Trigger(operatorController::getYButton); + // DoubleSupplier operatorRightStickY = operatorController::getRightY; + // // unused + // Trigger operatorUpDirectionPad = new Trigger(()->operatorController.getPOV() == 0); + // Trigger operatorLeftDirectionPad = new Trigger(()->operatorController.getPOV() == 270); + // Trigger operatorDownDirectionPad = new Trigger(()->operatorController.getPOV() == 180); + // Trigger driverLeftTrigger = new Trigger(()->driverController.getLeftTriggerAxis() > 0.2); + Trigger driverLeftBumper = new Trigger(driverController.leftBumper()); + // Trigger driverBButton = new Trigger(driverController::getBButton); + // Trigger driverYButton = new Trigger(driverController::getYButton); + // DoubleSupplier operatorLeftStickY = operatorController::getLeftY; + + // //DRIVER BUTTONS + + // // driving + + Command driveCommand = + new DriveCommand( + swerveDrive, + visionSubsystem, + driverLeftStick[1], + driverLeftStick[0], + () -> JoystickUtil.modifyAxis(driverRightStickX, 3), + () -> !driverRightBumper.getAsBoolean(), + () -> driverLeftBumper.getAsBoolean()); + swerveDrive.setDefaultCommand(driveCommand); + + // // shooterSubsystem.setDefaultCommand(new FlywheelSpinUpAuto(shooterSubsystem, + // visionSubsystem)); + + // driverLeftTrigger.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem, + // shooterSubsystem, false, ledSubsystem, this::intakeCallback)); + // driverLeftTrigger.whileFalse(new TowerIntake(intakeSubsystem, pivotSubsystem, + // shooterSubsystem, false, ledSubsystem, this::intakeCallback).withTimeout(0.3)); + // // Amp Lineup + // driverAButton.whileTrue(new AutoAlignWithAmp(swerveDrive, visionSubsystem)); + // // Spinup for shoot + // driverRightTrigger.whileTrue(new SpinUpForSpeaker(swerveDrive, shooterSubsystem, + // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, + // ledSubsystem)); + + // // driverLeftBumper.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, + // pivotSubsystem, visionSubsystem, driverLeftStickX, operatorLeftStickY, driverRightBumper, + // ledSubsystem)); + // // driverRightTrigger.whileTrue(new ShootWhileMove(swerveDrive, shooterSubsystem, + // pivotSubsystem, visionSubsystem, driverLeftStick, driverYButton, ledSubsystem)); + + // // Resets the robot angle in the odometry, factors in which alliance the robot is on + driverRightDirectionPad.onTrue( + new InstantCommand( + () -> + swerveDrive.setPose( + new Pose2d( + swerveDrive.getPose().getX(), + swerveDrive.getPose().getY(), + Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))))); + driverController + .x() + .onTrue( + new InstantCommand( + () -> swerveDrive.setPose(swerveDriveSimulation.getSimulatedDriveTrainPose()))); + // // // Reset robot odometry based on vision pose measurement from april tags + driverLeftDirectionPad.onTrue( + new InstantCommand(() -> swerveDrive.setPose(visionSubsystem.getLastSeenPose()))); + // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new + // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265))))); + // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, + // visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, ledSubsystem)); + + // // driverXButton. + // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem, + // visionSubsystem, driverLeftStickY, operatorLeftStickY, driverYButton, ledSubsystem)); + // // driverDownDirectionPad.whileTrue(new IntakeFromShooter(shooterSubsystem, + // intakeSubsystem)); + // // driverYButton.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, pivotSubsystem, + // visionSubsystem, driverLeftStickX, operatorLeftStickY, driverRightBumper, ledSubsystem)); + // // OPERATOR BUTTONS + + // // speaker + // operatorRightTrigger.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, + // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, + // ledSubsystem)); + // // amp + // operatorRightBumper.whileTrue(new ShootAmp(shooterSubsystem, pivotSubsystem, ledSubsystem, + // operatorBButton)); + // // fender shot + // operatorUpDirectionPad.whileTrue(new SubwooferShot(swerveDrive, shooterSubsystem, + // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightStickX, + // driverRightBumper, ledSubsystem)); + // // intake (aka SUCC_BUTTON) + // operatorLeftTrigger.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem, + // shooterSubsystem, false, ledSubsystem, this::intakeCallback)); + // operatorLeftTrigger.whileFalse(new TowerIntake(intakeSubsystem, pivotSubsystem, + // shooterSubsystem, false, ledSubsystem, this::intakeCallback).withTimeout(0.2)); + // // outtake (aka UNSUCC_BUTTON) + // operatorLeftBumper.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem, + // shooterSubsystem, true, ledSubsystem, this::intakeCallback)); + // // manual pivot (possible climb, unlikely) + // operatorAButton.whileTrue(new ManualPivot(pivotSubsystem, + // ()->modifyAxisCubed(operatorRightStickY))); + // operatorDownDirectionPad.whileTrue(new ManualPivot(pivotSubsystem, ()->-0.2)); + // // manual rollers + // operatorYButton.whileTrue(new ManualIntake(intakeSubsystem, true)); + // operatorXButton.whileTrue(new ManualIntake(intakeSubsystem, false)); + + // operatorBButton.onTrue(new StopShooterAndIntake(intakeSubsystem, pivotSubsystem, + // shooterSubsystem)); + } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + // Resets the pose factoring in the robot side + // This is just a failsafe, pose should be reset at the beginning of auto + swerveDrive.setPose( + new Pose2d( + swerveDrive.getPose().getX(), + swerveDrive.getPose().getY(), + Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset()))); + // return autoChooser.getSelected(); + // return new DriveForwardAndBack(swerveDrive); + return null; + } + + public void updateFieldSimAndDisplay() { + if (swerveDriveSimulation == null) return; + Logger.recordOutput( + "FieldSimulation/RobotPosition", swerveDriveSimulation.getSimulatedDriveTrainPose()); + Logger.recordOutput( + "FieldSimulation/Notes", + SimulatedField.getInstance().getGamePiecesByType("Note").toArray(Pose3d[]::new)); } } diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java index 4d1b522..83b1ac9 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java @@ -2,7 +2,7 @@ import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveDrive; -import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionSubsystem; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -29,7 +29,7 @@ public class DriveCommand extends DriveCommandBase { */ public DriveCommand( SwerveDrive driveSubsystem, - Vision visionSubsystem, + VisionSubsystem visionSubsystem, DoubleSupplier leftJoystickY, DoubleSupplier leftJoystickX, DoubleSupplier rightJoystickX, diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java index ea557e1..b8136a3 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java @@ -6,8 +6,9 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.extras.interpolators.MultiLinearInterpolator; import frc.robot.subsystems.swerve.SwerveDrive; -import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionConstants; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import frc.robot.subsystems.vision.VisionSubsystem; public abstract class DriveCommandBase extends Command { @@ -16,7 +17,7 @@ public abstract class DriveCommandBase extends Command { private final MultiLinearInterpolator twoAprilTagLookupTable = new MultiLinearInterpolator(VisionConstants.TWO_APRIL_TAG_LOOKUP_TABLE); - private final Vision vision; + private final VisionSubsystem vision; private final SwerveDrive swerveDrive; private double lastTimeStampSeconds = 0; @@ -27,7 +28,7 @@ public abstract class DriveCommandBase extends Command { * @param driveSubsystem The subsystem for the swerve drive * @param vision The subsystem for vision measurements */ - public DriveCommandBase(SwerveDrive swerveDrive, Vision vision) { + public DriveCommandBase(SwerveDrive swerveDrive, VisionSubsystem vision) { this.swerveDrive = swerveDrive; this.vision = vision; // It is important that you do addRequirements(driveSubsystem, vision) in whatever @@ -40,12 +41,12 @@ public void execute() { swerveDrive.addPoseEstimatorSwerveMeasurement(); vision.setHeadingInfo( swerveDrive.getPose().getRotation().getDegrees(), swerveDrive.getGyroRate()); - calculatePoseFromLimelight(VisionConstants.SHOOTER_LIMELIGHT_NUMBER); - calculatePoseFromLimelight(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); - calculatePoseFromLimelight(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); + calculatePoseFromLimelight(Limelight.SHOOTER); + calculatePoseFromLimelight(Limelight.FRONT_LEFT); + calculatePoseFromLimelight(Limelight.FRONT_RIGHT); } - public void calculatePoseFromLimelight(int limelightNumber) { + public void calculatePoseFromLimelight(Limelight limelightNumber) { double currentTimeStampSeconds = lastTimeStampSeconds; // Updates the robot's odometry with april tags diff --git a/src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java b/src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java deleted file mode 100644 index c0f62f1..0000000 --- a/src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java +++ /dev/null @@ -1,122 +0,0 @@ -// 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 frc.robot.extras.debug; - -import java.util.Arrays; -import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; - -/** - * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or - * value not in dashboard. - */ -public class LoggedTunableNumber implements DoubleSupplier { - 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 (hasDefault) { - 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 hasDefault ? 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; - } - - /** - * Runs action if any of the tunableNumbers have changed - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * - * objects. Recommended approach is to pass the result of "hashCode()" - * @param action Callback to run when any of the tunable numbers have changed. Access tunable - * numbers in order inputted in method - * @param tunableNumbers All tunable numbers to check - */ - public static void ifChanged( - int id, Consumer action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { - action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); - } - } - - /** Runs action if any of the tunableNumbers have changed */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { - ifChanged(id, values -> action.run(), tunableNumbers); - } - - @Override - public double getAsDouble() { - return get(); - } -} diff --git a/src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java b/src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java deleted file mode 100644 index 4e65ecc..0000000 --- a/src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java +++ /dev/null @@ -1,266 +0,0 @@ -// Copyright (c) LukeLib - -package frc.robot.extras.interpolators; - -public class BilinearInterpolator { - - private final double[][] lookupTable; - private final double dataRowSize; - - /** - * Handles finding values through a lookup table in a linear fashion. - * - * @param lookupTable an array containing {x_input, y_input, x_val, y_val} points. The collected - * data must be in ascending y order, collected from left to right, reading up. - */ - public BilinearInterpolator(double[][] lookupTable) { - this.lookupTable = lookupTable; - // If you don't use rows of seven, replace the numbers 6, 7, 8 in the commands with the - // dataRowSize variable with dataRowSize - 1, dataRowSize, dataRowSize + 1 - // TODO: POST Comp Incorporate this variable - this.dataRowSize = lookupTable[0].length; - } - - /** - * Returns an interpolated array from the lookup table corresponding to the given input x and y - * value. - */ - public double[] getLookupValue(double inputXValue, double inputYValue) { - // if the note center is approximately in the middle, then only need to do y linear - // interpolation (no bilinear :) - if (inputXValue >= 155 && inputXValue <= 165) { - for (int i = 0; i < lookupTable.length - 8; i += 7) { - // check if it is equal to a collected data point - if (inputYValue == lookupTable[i][1]) { - return new double[] {lookupTable[i][2], lookupTable[i][3]}; - } - - // interpolate in purely y-direction - else if (inputYValue < lookupTable[i][1] && inputYValue > lookupTable[i + 7][1]) { - // if x matches up with a data pt, interpolate between above's data pts' y's to determine - // pose - double ySlope = - (lookupTable[i + 7][3] - lookupTable[i][3]) - / (lookupTable[i + 7][1] - lookupTable[i][1]); - double yYIntercept = lookupTable[i][3]; - return new double[] { - lookupTable[i][2], ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept - }; - } - } - // in case nothing works or like the note x is good but its too far away in y direction (test) - return new double[] {0.0, -1.0}; - } - - // if the note center is less than half the x_pixels (160) - if (inputXValue < 160) { - // if note center is less than halfway, then we reflect across y-axis - inputXValue = 320 - inputXValue; - // Loop through the second row to the second to top most row drawing diagonal vectors. Cannot - // start at top/bottom because there cannot be diagonal vectors in both directions - for (int i = 7; i < lookupTable.length - 7; i++) { - // check if it is equivalent to collected data point - if (inputXValue == lookupTable[i][0]) { - if (inputYValue == lookupTable[i][1]) { - return new double[] {-(lookupTable[i][2]), lookupTable[i][3]}; - } - } - - // bilinear interpolation approximation used diagnoal vectored pt (facing top right) - if (inputXValue > lookupTable[i][0] - && inputYValue < lookupTable[i][1] - && inputXValue < lookupTable[i + 8][0] - && inputYValue > lookupTable[i + 8][1]) { - // Interpolate diagonal points - double xSlope = - (lookupTable[i + 8][2] - lookupTable[i][2]) - / (lookupTable[i + 8][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - double ySlope = - (lookupTable[i + 8][3] - lookupTable[i][3]) - / (lookupTable[i + 8][1] - lookupTable[i][1]); - double yYIntercept = lookupTable[i][3]; - - return new double[] { - -(xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept), - ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept - }; - } - - // bilinear interpolation approximation used diagnoal in opposite direction vectored pt - // (facing bottom right) - if (inputXValue > lookupTable[i][0] - && inputYValue > lookupTable[i][1] - && inputXValue < lookupTable[i - 6][0] - && inputYValue < lookupTable[i - 6][1]) { - // Interpolate diagonal points - double xSlope = - (lookupTable[i - 6][2] - lookupTable[i][2]) - / (lookupTable[i - 6][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - double ySlope = - (lookupTable[i - 6][3] - lookupTable[i][3]) - / (lookupTable[i - 6][1] - lookupTable[i][1]); - double yYIntercept = lookupTable[i][3]; - - return new double[] { - -(xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept), - ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept - }; - } - } - - // check for first row for only x interpolation - for (int i = 0; i < 7; i++) { - if (inputYValue == lookupTable[i][1]) { - if (inputXValue == lookupTable[i][0]) { - return new double[] {-lookupTable[i][2], lookupTable[i][3]}; - } else { - // if y matches up with a data pt, interpolate between adjacent (right for over 160) - // data pts' x's to determine pose - double xSlope = - (lookupTable[i + 1][2] - lookupTable[i][2]) - / (lookupTable[i + 1][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - return new double[] { - -(xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept), lookupTable[i][3] - }; - } - } - } - - // check for last row just to check for x only interpolation or point checking. (You can't do - // y_interpolation or diagonal) - for (int i = lookupTable.length - 7; i < lookupTable.length; i++) { - if (inputYValue == lookupTable[i][1]) { - if (inputXValue == lookupTable[i][0]) { - return new double[] {-lookupTable[i][2], lookupTable[i][3]}; - } else { - // if y matches up with a data pt, interpolate between adjacent (right for over 160) - // data pts' x's to determine pose - double xSlope = - (lookupTable[i + 1][2] - lookupTable[i][2]) - / (lookupTable[i + 1][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - return new double[] { - -(xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept), lookupTable[i][3] - }; - } - } - } - - // if outside the collected data pt range, - // then return unused values for now. I could make a handle to get the max one and then - // interpolate or get max for both if its outside and thats what the above 4 cases are - // supposed to do but the - // logic may need to be changed - return new double[] {-1.0, -1.0, 69.0}; - } - - // if the note center is greater than half the x_pixels (160) - if (inputXValue > 160) { - // Loop through the second row to the second to top most row drawing diagonal vectors. Cannot - // start at top/bottom because there cannot be diagonal vectors in both directions - for (int i = 7; i < lookupTable.length - 7; i++) { - // check to see if the found note center is equivalent to a collected data point - if (inputXValue == lookupTable[i][0]) { - if (inputYValue == lookupTable[i][1]) { - return new double[] {lookupTable[i][2], lookupTable[i][3]}; - } - } - - // bilinear interpolation approximation used diagnoal vectored pt (facing top right) - if (inputXValue > lookupTable[i][0] - && inputYValue < lookupTable[i][1] - && inputXValue < lookupTable[i + 8][0] - && inputYValue > lookupTable[i + 8][1]) { - // interpolate in both directions using vector - double xSlope = - (lookupTable[i + 8][2] - lookupTable[i][2]) - / (lookupTable[i + 8][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - double ySlope = - (lookupTable[i + 8][3] - lookupTable[i][3]) - / (lookupTable[i + 8][1] - lookupTable[i][1]); - double yYIntercept = lookupTable[i][3]; - - return new double[] { - xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept, - ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept - }; - } - - // bilinear interpolation approximation used diagnoal in opposite direction vectored pt - if (inputXValue > lookupTable[i][0] - && inputYValue > lookupTable[i][1] - && inputXValue < lookupTable[i - 6][0] - && inputYValue < lookupTable[i - 6][1]) { - // interpolate in both directions using vector - double xSlope = - (lookupTable[i - 6][2] - lookupTable[i][2]) - / (lookupTable[i - 6][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - double ySlope = - (lookupTable[i - 6][3] - lookupTable[i][3]) - / (lookupTable[i - 6][1] - lookupTable[i][1]); - double yYIntercept = lookupTable[i][3]; - - return new double[] { - xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept, - ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept - }; - } - } - - // check for first row for only x interpolation - for (int i = 0; i < 7; i++) { - if (inputYValue == lookupTable[i][1]) { - if (inputXValue == lookupTable[i][0]) { - return new double[] {lookupTable[i][2], lookupTable[i][3]}; - } else { - // if y matches up with a data pt, interpolate between adjacent (right for over 160) - // data pts' x's to determine pose - double xSlope = - (lookupTable[i + 1][2] - lookupTable[i][2]) - / (lookupTable[i + 1][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - return new double[] { - xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept, lookupTable[i][3] - }; - } - } - } - - // check for last row just to check for x only interpolation or point checking. (You can't do - // y_interpolation or diagonal) - for (int i = lookupTable.length - 7; i < lookupTable.length; i++) { - if (inputYValue == lookupTable[i][1]) { - if (inputXValue == lookupTable[i][0]) { - return new double[] {lookupTable[i][2], lookupTable[i][3]}; - } else { - // if y matches up with a data pt, interpolate between adjacent (right for over 160) - // data pts' x's to determine pose - double xSlope = - (lookupTable[i + 1][2] - lookupTable[i][2]) - / (lookupTable[i + 1][0] - lookupTable[i][0]); - double xYIntercept = lookupTable[i][2]; - return new double[] { - xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept, lookupTable[i][3] - }; - } - } - } - - // if outside the collected data pt range, - // then return unused values for now. I could make a handle to get the max one and then - // interpolate or get max for both if its outside and thats what the above 4 cases are - // supposed to do but the - // logic may need to be changed - return new double[] {-1.0, -1.0, 96.0}; - } - - // The third number in each double is just for debugging purposes to determine which case was - // selected - return new double[] {-1.0, -1.0, 77.0}; - } -} diff --git a/src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java b/src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java new file mode 100644 index 0000000..1559662 --- /dev/null +++ b/src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java @@ -0,0 +1,27 @@ +package frc.robot.extras.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; + +public final class AdvancedSwerveModuleState extends SwerveModuleState { + public double steerVelocityFF; + public double driveAccelerationFF; + + public AdvancedSwerveModuleState( + double speedMetersPerSecond, + Rotation2d angle, + double steerVelocityFF, + double driveAccelerationFF) { + super(speedMetersPerSecond, angle); + this.steerVelocityFF = steerVelocityFF; + this.driveAccelerationFF = driveAccelerationFF; + } + + // todo: implement custom struct + public static final Struct struct = SwerveModuleState.struct; + + public static AdvancedSwerveModuleState fromBase(SwerveModuleState base) { + return new AdvancedSwerveModuleState(base.speedMetersPerSecond, base.angle, 0.0, 0.0); + } +} diff --git a/src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java b/src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java new file mode 100644 index 0000000..a01f64d --- /dev/null +++ b/src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java @@ -0,0 +1,285 @@ +package frc.robot.extras.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.extras.util.ProceduralStructGenerator; +import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray; +import frc.robot.extras.util.ProceduralStructGenerator.IgnoreStructField; +import java.nio.ByteBuffer; +import java.util.Arrays; + +class SPGCalcs { + private static final double kEpsilon = 1E-8; + static final int NUM_MODULES = 4; + + static double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + static double findSteeringMaxS( + double prevVx, + double prevVy, + double prevHeading, + double desiredVx, + double desiredVy, + double desiredHeading, + double maxDeviation) { + desiredHeading = unwrapAngle(prevHeading, desiredHeading); + double diff = desiredHeading - prevHeading; + if (Math.abs(diff) <= maxDeviation) { + // Can go all the way to s=1. + return 1.0; + } + + double target = prevHeading + Math.copySign(maxDeviation, diff); + + // Rotate the velocity vectors such that the target angle becomes the +X + // axis. We only need find the Y components, h_0 and h_1, since they are + // proportional to the distances from the two points to the solution + // point (x_0 + (x_1 - x_0)s, y_0 + (y_1 - y_0)s). + double sin = Math.sin(-target); + double cos = Math.cos(-target); + double h_0 = sin * prevVx + cos * prevVy; + double h_1 = sin * desiredVx + cos * desiredVy; + + // Undo linear interpolation from h_0 to h_1: + // 0 = h_0 + (h_1 - h_0) * s + // -h_0 = (h_1 - h_0) * s + // -h_0 / (h_1 - h_0) = s + // h_0 / (h_0 - h_1) = s + // Guaranteed to not divide by zero, since if h_0 was equal to h_1, theta_0 + // would be equal to theta_1, which is caught by the difference check. + return h_0 / (h_0 - h_1); + } + + private static boolean isValidS(double s) { + return Double.isFinite(s) && s >= 0 && s <= 1; + } + + static double findDriveMaxS(double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // Derivation: + // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the + // length of P(s) is the target T. P(s) is linearly interpolated between the + // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). + // Then, + // T = sqrt(P(s).x^2 + P(s).y^2) + // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + // + (x_0^2 + y_0^2 - T^2). + // + // To simplify, we can factor out some common parts: + // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and + // p = x_0 * x_1 + y_0 * y_1. + // Then we have + // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + // with which we can solve for s using the quadratic formula. + + double l_0 = x_0 * x_0 + y_0 * y_0; + double l_1 = x_1 * x_1 + y_1 * y_1; + double sqrt_l_0 = Math.sqrt(l_0); + double diff = Math.sqrt(l_1) - sqrt_l_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + + double target = sqrt_l_0 + Math.copySign(max_vel_step, diff); + double p = x_0 * x_1 + y_0 * y_1; + + // Quadratic of s + double a = l_0 + l_1 - 2 * p; + double b = 2 * (p - l_0); + double c = l_0 - target * target; + double root = Math.sqrt(b * b - 4 * a * c); + + // Check if either of the solutions are valid + // Won't divide by zero because it is only possible for a to be zero if the + // target velocity is exactly the same or the reverse of the current + // velocity, which would be caught by the difference check. + double s_1 = (-b + root) / (2 * a); + if (isValidS(s_1)) { + return s_1; + } + double s_2 = (-b - root) / (2 * a); + if (isValidS(s_2)) { + return s_2; + } + + // Since we passed the initial max_vel_step check, a solution should exist, + // but if no solution was found anyway, just don't limit movement + return 1.0; + } + + static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { + return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) + && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) + && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); + } + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + static boolean flipHeading(double prevToGoal) { + return Math.abs(prevToGoal) > Math.PI / 2.0; + } + + private static final Struct structVectors; + private static final Struct structVars; + + static { + final var structVectorsProc = + ProceduralStructGenerator.genObject(LocalVectors.class, LocalVectors::new); + structVectors = + new Struct() { + @Override + public String getSchema() { + return "float64 vx;float64 vy;float64 cos;float64 sin;"; + } + + @Override + public int getSize() { + return 32; + } + + @Override + public Class getTypeClass() { + return LocalVectors.class; + } + + @Override + public String getTypeName() { + return "LocalVectors"; + } + + @Override + public void pack(ByteBuffer bb, LocalVectors value) { + bb.putDouble(value.vx); + bb.putDouble(value.vy); + bb.putDouble(value.cos); + bb.putDouble(value.sin); + } + + @Override + public LocalVectors unpack(ByteBuffer bb) { + return structVectorsProc.unpack(bb); + } + + @Override + public String toString() { + return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.getSchema() + "}"; + } + }; + structVars = ProceduralStructGenerator.genObject(LocalVars.class, LocalVars::new); + System.out.println(structVectors); + System.out.println(structVars); + } + + static final class LocalVectors implements StructSerializable { + public double vx, vy, cos, sin; + + public LocalVectors() {} + + public void reset() { + vx = vy = cos = sin = 0.0; + } + + public void applyModuleState(SwerveModuleState state) { + cos = state.angle.getCos(); + sin = state.angle.getSin(); + vx = cos * state.speedMetersPerSecond; + vy = sin * state.speedMetersPerSecond; + if (state.speedMetersPerSecond < 0.0) { + applyRotation(Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin()); + } + } + + public LocalVectors applyRotation(double otherCos, double otherSin) { + double newCos = cos * otherCos - sin * otherSin; + double newSin = cos * otherSin + sin * otherCos; + cos = newCos; + sin = newSin; + + return this; + } + + public double radians() { + return Math.atan2(sin, cos); + } + + public static final Struct struct = structVectors; + } + + static final class LocalVars implements StructSerializable { + @FixedSizeArray(size = NUM_MODULES) + public LocalVectors[] prev; + + @FixedSizeArray(size = NUM_MODULES) + public LocalVectors[] desired; + + public boolean needToSteer = true, allModulesShouldFlip = true; + public double minS, dt; + public double dx, dy, dtheta; + public ChassisSpeeds prevSpeeds, desiredSpeeds; + + @FixedSizeArray(size = NUM_MODULES) + public SwerveModuleState[] prevModuleStates; + + @FixedSizeArray(size = NUM_MODULES) + public SwerveModuleState[] desiredModuleStates; + + @IgnoreStructField public Rotation2d[] steeringOverride; + + public LocalVars() { + desiredSpeeds = prevSpeeds = new ChassisSpeeds(); + prev = new LocalVectors[NUM_MODULES]; + desired = new LocalVectors[NUM_MODULES]; + steeringOverride = new Rotation2d[NUM_MODULES]; + for (int i = 0; i < NUM_MODULES; i++) { + prev[i] = new LocalVectors(); + desired[i] = new LocalVectors(); + } + } + + public LocalVars reset() { + needToSteer = allModulesShouldFlip = true; + Arrays.fill(steeringOverride, null); + for (int i = 0; i < NUM_MODULES; i++) { + prev[i].reset(); + desired[i].reset(); + } + + return this; + } + + public static final Struct struct = structVars; + } +} diff --git a/src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java new file mode 100644 index 0000000..7bb5e7c --- /dev/null +++ b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java @@ -0,0 +1,30 @@ +package frc.robot.extras.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.extras.util.ProceduralStructGenerator; +import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray; + +public record SwerveSetpoint( + ChassisSpeeds chassisSpeeds, + @FixedSizeArray(size = 4) AdvancedSwerveModuleState[] moduleStates) // + implements StructSerializable { + public static SwerveSetpoint zeroed() { + return new SwerveSetpoint( + new ChassisSpeeds(), + new AdvancedSwerveModuleState[] { + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0) + }); + } + + public static final Struct struct; + + static { + struct = ProceduralStructGenerator.genRecord(SwerveSetpoint.class); + } +} diff --git a/src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java new file mode 100644 index 0000000..3f55029 --- /dev/null +++ b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java @@ -0,0 +1,363 @@ +package frc.robot.extras.setpointGen; + +import static frc.robot.extras.setpointGen.SPGCalcs.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; + +/** + * Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just + * know i didn't write most the logic in this code that credit goes to 254 and mjansen + * + *

Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the + * kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any + * forces acting on a module's wheel from exceeding the force of friction. + */ +public class SwerveSetpointGenerator { + private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); + + private static final int NUM_MODULES = SPGCalcs.NUM_MODULES; + + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; + private final DCMotor driveMotor; + private final double driveCurrentLimitAmps, + maxDriveVelocityMPS, + maxSteerVelocityRadsPerSec, + massKg, + moiKgMetersSquared, + wheelRadiusMeters, + wheelFrictionForce, + // maxTorqueFriction, + torqueLoss; + + public SwerveSetpointGenerator( + final Translation2d[] moduleLocations, + final DCMotor driveMotor, + final DCMotor angleMotor, + final double driveCurrentLimitAmps, + final double massKg, + final double moiKgMetersSquared, + final double wheelDiameterMeters, + final double wheelCoF, + final double torqueLoss) { + + if (moduleLocations.length != NUM_MODULES) { + throw new IllegalArgumentException("Module locations must have 4 elements"); + } + + this.driveMotor = driveMotor; + this.driveCurrentLimitAmps = driveCurrentLimitAmps; + this.maxSteerVelocityRadsPerSec = angleMotor.freeSpeedRadPerSec; + kinematics = new SwerveDriveKinematics(moduleLocations); + this.moduleLocations = moduleLocations; + this.massKg = massKg; + this.moiKgMetersSquared = moiKgMetersSquared; + this.wheelRadiusMeters = wheelDiameterMeters / 2; + this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters; + + wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81); + // maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters; + this.torqueLoss = torqueLoss; + } + + // alot of work was done to reduce allocations in this hot loop, + // migrating everything over to a vars object that gets reused + // was the best way to do this. + private static final LocalVars VARS_TEMPLATE = new LocalVars(); + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredRobotRelativeSpeeds The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + // https://github.com/wpilibsuite/allwpilib/issues/7332 + SwerveModuleState[] desiredModuleStates = + kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); + // Make sure desiredState respects velocity limits. + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS); + desiredRobotRelativeSpeeds = kinematics.toChassisSpeeds(desiredModuleStates); + + final LocalVars vars = VARS_TEMPLATE.reset(); + vars.dt = dt; + vars.prevSpeeds = prevSetpoint.chassisSpeeds(); + vars.desiredSpeeds = desiredRobotRelativeSpeeds; + vars.desiredModuleStates = desiredModuleStates; + vars.prevModuleStates = prevSetpoint.moduleStates(); + vars.dx = + desiredRobotRelativeSpeeds.vxMetersPerSecond + - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + vars.dy = + desiredRobotRelativeSpeeds.vyMetersPerSecond + - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + vars.dtheta = + desiredRobotRelativeSpeeds.omegaRadiansPerSecond + - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + vars.minS = 1.0; + + checkNeedToSteer(vars); + makeVectors(vars); + + // if (vars.allModulesShouldFlip + // && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) + // && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { + // // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // // of the desired angle, and accelerate again. + // return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); + // } + + solveSteering(vars); + + solveDriving(vars); + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, + vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, + vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); + retSpeeds.discretize(retSpeeds, dt); + + double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; + double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt; + double angularAccel = + (retSpeeds.omegaRadiansPerSecond - vars.prevSpeeds.omegaRadiansPerSecond) / dt; + + SwerveModuleState[] retStates = kinematics.toSwerveModuleStates(retSpeeds); + SwerveModuleState[] accelStates = + kinematics.toSwerveModuleStates( + new ChassisSpeeds(chassisAccelX, chassisAccelY, angularAccel)); + + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; + for (int m = 0; m < NUM_MODULES; m++) { + retStates[m].optimize(vars.prevModuleStates[m].angle); + double steerVelocity = + (vars.prevModuleStates[m].angle.getRadians() - retStates[m].angle.getRadians()) / dt; + outputStates[m] = + new AdvancedSwerveModuleState( + retStates[m].speedMetersPerSecond, + retStates[m].angle, + steerVelocity, + accelStates[m].speedMetersPerSecond); + } + + // log("output", + return new SwerveSetpoint(retSpeeds, outputStates); + } + + public SwerveSetpoint generateSimpleSetpoint( + final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; + SwerveModuleState[] desiredModuleStates = + kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS); + for (int m = 0; m < NUM_MODULES; m++) { + desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); + outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]); + } + + return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates); + } + + private static void checkNeedToSteer(LocalVars vars) { + if (epsilonEquals(vars.desiredSpeeds, ZERO_SPEEDS)) { + vars.needToSteer = false; + for (int m = 0; m < NUM_MODULES; m++) { + vars.desiredModuleStates[m].angle = vars.prevModuleStates[m].angle; + vars.desiredModuleStates[m].speedMetersPerSecond = 0.0; + } + } + } + + private static double rotateBy(double rad, double otherCos, double otherSin) { + return Math.atan2( + Math.sin(rad) * otherCos + Math.cos(rad) * otherSin, + Math.cos(rad) * otherCos - Math.sin(rad) * otherSin); + } + + private static double requiredRotation(double prevRadians, double desiredRads) { + // this looks messy without using Rotation2d methods. + // this is roughly equivalent to: + // + // double r = vars.prev[m].rot2d().unaryMinus() + // .rotateBy(vars.desired[m].rot2d()).getRadians(); + double unaryMinusPrevRads = -prevRadians; + return rotateBy(unaryMinusPrevRads, Math.cos(desiredRads), Math.sin(desiredRads)); + } + + private static void makeVectors(LocalVars vars) { + for (int m = 0; m < NUM_MODULES; m++) { + vars.prev[m].applyModuleState(vars.prevModuleStates[m]); + vars.desired[m].applyModuleState(vars.desiredModuleStates[m]); + if (vars.allModulesShouldFlip) { + double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians()); + if (flipHeading(requiredRots)) { + vars.allModulesShouldFlip = false; + } + } + } + } + + private void solveSteering(LocalVars vars) { + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since inverse kinematics doesn't care about angle, we can be opportunistically + // lazy). + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + for (int m = 0; m < NUM_MODULES; m++) { + if (!vars.needToSteer) { + vars.steeringOverride[m] = vars.prevModuleStates[m].angle; + continue; + } + + double maxThetaStep = vars.dt * maxSteerVelocityRadsPerSec; + + if (epsilonEquals(vars.prevModuleStates[m].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based purely on rotation in place. + if (epsilonEquals(vars.desiredModuleStates[m].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + vars.steeringOverride[m] = vars.prevModuleStates[m].angle; + continue; + } + + double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians()); + if (flipHeading(requiredRots)) { + requiredRots = + rotateBy(requiredRots, Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin()); + } + + // radians bounds to +/- Pi. + final double numStepsNeeded = Math.abs(requiredRots) / maxThetaStep; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + vars.steeringOverride[m] = vars.desiredModuleStates[m].angle; + } else { + // Adjust steering by max_theta_step. + // there really is no way to avoid this allocation. + Rotation2d adjusted = + vars.prevModuleStates[m].angle.rotateBy( + Rotation2d.fromRadians(Math.signum(requiredRots) * maxThetaStep)); + vars.steeringOverride[m] = adjusted; + vars.minS = 0.0; + } + continue; + } + if (vars.minS == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + double maxS = + findSteeringMaxS( + vars.prev[m].vx, + vars.prev[m].vy, + vars.prev[m].radians(), + vars.desired[m].vx, + vars.desired[m].vy, + vars.desired[m].radians(), + maxThetaStep); + vars.minS = Math.min(vars.minS, maxS); + } + } + + private void solveDriving(LocalVars vars) { + // Enforce drive wheel torque limits + Translation2d chassisForceVec = Translation2d.kZero; + double chassisTorque = 0.0; + for (int m = 0; m < NUM_MODULES; m++) { + double lastVelRadPerSec = vars.prevModuleStates[m].speedMetersPerSecond / wheelRadiusMeters; + // Use the current battery voltage since we won't be able to supply 12v if the + // battery is sagging down to 11v, which will affect the max torque output + double currentDraw = + driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage()); + currentDraw = Math.min(currentDraw, driveCurrentLimitAmps); + double moduleTorque = driveMotor.getTorque(currentDraw); + + double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond; + vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle); + double desiredSpeed = vars.desiredModuleStates[m].speedMetersPerSecond; + + int forceSign; + Rotation2d forceAngle = vars.prevModuleStates[m].angle; + if (epsilonEquals(prevSpeed, 0.0) + || (prevSpeed > 0 && desiredSpeed >= prevSpeed) + || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + // Torque loss will be fighting motor + moduleTorque -= torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } else { + // Torque loss will be helping the motor + moduleTorque += torqueLoss; + forceSign = -1; // Force will be applied in opposite direction of module + if (prevSpeed > 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = Math.min(moduleTorque, wheelFrictionForce * wheelRadiusMeters); + + double forceAtCarpet = moduleTorque / wheelRadiusMeters; + Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec.plus(moduleForceVec); + + // Calculate the torque this module will apply to the chassis + Rotation2d angleToModule = moduleLocations[m].getAngle(); + Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); + chassisTorque += forceAtCarpet * moduleLocations[m].getNorm() * theta.getSin(); + } + + Translation2d chassisAccelVec = chassisForceVec.div(massKg); + double chassisAngularAccel = chassisTorque / moiKgMetersSquared; + + // Use kinematics to convert chassis accelerations to module accelerations + ChassisSpeeds chassisAccel = + new ChassisSpeeds(chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); + var accelStates = kinematics.toSwerveModuleStates(chassisAccel); + + for (int m = 0; m < NUM_MODULES; m++) { + if (vars.minS == 0.0) { + // No need to carry on. + break; + } + + double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * vars.dt); + + double vxMinS; + double vyMinS; + if (vars.minS == 1.0) { + vxMinS = vars.desired[m].vx; + vyMinS = vars.desired[m].vy; + } else { + vxMinS = (vars.desired[m].vx - vars.prev[m].vx) * vars.minS + vars.prev[m].vx; + vyMinS = (vars.desired[m].vy - vars.prev[m].vy) * vars.minS + vars.prev[m].vy; + } + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster than that. + double s = findDriveMaxS(vars.prev[m].vx, vars.prev[m].vy, vxMinS, vyMinS, maxVelStep); + vars.minS = Math.min(vars.minS, s); + } + } +} diff --git a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java index d991fa8..e73bc04 100644 --- a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java +++ b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java @@ -3,12 +3,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.extras.simulation.gamePiece.CrescendoNoteSimulation; import frc.robot.extras.simulation.gamePiece.GamePieceSimulation; import frc.robot.extras.simulation.mechanismSim.IntakeSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation; +import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim; import frc.robot.extras.util.GeomUtil; +// import java.lang.ref.WeakReference; import java.util.*; import org.dyn4j.dynamics.Body; import org.dyn4j.dynamics.BodyFixture; @@ -125,6 +130,7 @@ public static void overrideSimulationTimings( protected final Set driveTrainSimulations; protected final Set gamePieces; protected final List simulationSubTickActions; + protected final List motors; private final List intakeSimulations; /** @@ -149,6 +155,7 @@ protected SimulatedField(FieldMap obstaclesMap) { simulationSubTickActions = new ArrayList<>(); this.gamePieces = new HashSet<>(); this.intakeSimulations = new ArrayList<>(); + motors = new ArrayList<>(); } /** @@ -276,6 +283,10 @@ public void simulationPeriodic() { "MapleArenaSimulation/Dyn4jEngineCPUTimeMS", (System.nanoTime() - t0) / 1000000.0); } + public void addMotor(BrushlessMotorSim motor) { + motors.add(motor); + } + /** * * @@ -293,6 +304,17 @@ public void simulationPeriodic() { * */ private void simulationSubTick() { + ArrayList motorCurrents = new ArrayList<>(); + for (var motor : motors) { + BrushlessMotorSim motorRef = motor; + if (motorRef != null) { + motorRef.update(); + } + } + double vin = + BatterySim.calculateLoadedBatteryVoltage( + 12.2, 0.015, motorCurrents.stream().mapToDouble(Double::doubleValue).toArray()); + RoboRioSim.setVInVoltage(vin); for (AbstractDriveTrainSimulation driveTrainSimulation : driveTrainSimulations) driveTrainSimulation.simulationSubTick(); diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java index 5ba3122..76f1e64 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java @@ -1,41 +1,41 @@ -// A drop-in replacement for WPILib's DCMotorSim to simulate modern brushless motors -// Copyright (C) 2024 Team-5516-"Iron Maple" -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// -// Original source: -// https://github.com/Shenzhen-Robotics-Alliance/maple-sim/blob/main/src/main/java/org/ironmaple/simulation/drivesims/BrushlessMotorSim.java - package frc.robot.extras.simulation.mechanismSim.swerve; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.AngularAccelerationUnit; +import edu.wpi.first.units.AngularVelocityUnit; +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.PerUnit; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Per; +import edu.wpi.first.units.measure.Torque; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.extras.simulation.field.SimulatedField; /** * * *

{@link DCMotorSim} with a bit of extra spice.

* - *

By Team 5516 "IRON MAPLE". - * *

This class extends the functionality of the original {@link DCMotorSim} and models the * following aspects in addition: * @@ -43,16 +43,47 @@ *

  • Friction force on the rotor. *
  • Smart current limiting. *
  • Brake and coast modes (only for simulating brushless motors). - *
  • Simulated encoder readings. * */ public class BrushlessMotorSim { + public enum OutputType { + VOLTAGE, + CURRENT + } + + public enum OutputMode { + VELOCITY, + POSITION, + OPEN_LOOP + } + + /** The Constants for the motor */ private final DCMotor motor; - private final double gearRatio, frictionTorque, loadInertiaJKgMetersSquared; - private double requestedVoltageOutput, currentLimitAmps; - private boolean breakModeEnabled; - private double angularPositionRad, angularVelocityRadPerSec, appliedVolts, currentDrawAmps; + /** The dynamics simulation for the motor */ + private final DCMotorSim sim; + + /** The gear ratio, value above 1.0 are a reduction */ + private final double gearing; + + /** The voltage required to overcome friction */ + private final Voltage frictionVoltage; + + private final PIDController poseVoltController = new PIDController(0, 0, 0); + private final PIDController veloVoltController = new PIDController(0, 0, 0); + private final PIDController poseCurrentController = new PIDController(0, 0, 0); + private final PIDController veloCurrentController = new PIDController(0, 0, 0); + + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0, 0); + + private Current currentLimit = Amps.of(300.0); + + private OutputType outputType = OutputType.VOLTAGE; + private OutputMode outputMode = OutputMode.OPEN_LOOP; + private double output = 0.0; + + private Angle forwardLimit = Radians.of(Double.POSITIVE_INFINITY); + private Angle reverseLimit = Radians.of(Double.NEGATIVE_INFINITY); /** * @@ -61,283 +92,413 @@ public class BrushlessMotorSim { * * @param motor the {@link DCMotor} model representing the motor(s) in the simulation * @param gearRatio the gear ratio of the mechanism; values greater than 1 indicate a reduction - * @param loadIntertiaJKgMetersSquared the rotational inertia of the mechanism, in kg·m² - * @param frictionVoltage the voltage required to overcome friction and make the mechanism move + * @param loadIntertia the rotational inertia of the mechanism + * @param frictionVoltage the voltage required to keep the motor moving at a constant velocity */ public BrushlessMotorSim( + SimulatedField arena, DCMotor motor, double gearRatio, - double loadIntertiaJKgMetersSquared, - double frictionVoltage) { + MomentOfInertia loadIntertia, + Voltage frictionVoltage) { + this.sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + motor, loadIntertia.in(KilogramSquareMeters), gearRatio), + motor); this.motor = motor; - this.gearRatio = gearRatio; - this.frictionTorque = motor.getTorque(motor.getCurrent(0, frictionVoltage)) * gearRatio; - - this.loadInertiaJKgMetersSquared = loadIntertiaJKgMetersSquared; - disableCurrentLimit(); - this.breakModeEnabled = true; + this.gearing = gearRatio; + this.frictionVoltage = frictionVoltage; - this.angularPositionRad = angularVelocityRadPerSec = appliedVolts = currentDrawAmps = 0; + arena.addMotor(this); } /** + * Configures the angle of the motor. * - * - *

    Requests an Output Voltage for the Motor.

    - * - *

    Note: The requested voltage may not always be fully applied due to current - * limits. For details on current limiting, refer to {@link #enableCurrentLimit(double)}. - * - * @param volts the requested output voltage for the motor + * @param angle the angle of the motor + * @return this instance for method chaining */ - public void requestVoltageOutput(double volts) { - this.requestedVoltageOutput = volts; + public BrushlessMotorSim withOverrideAngle(Angle angle) { + sim.setAngle(angle.in(Radians)); + return this; } /** + * Configures the angular velocity of the motor. * - * - *

    Configures a Current Limit for the Motor.

    - * - *

    If the motor's supply current exceeds the specified limit, the motor will reduce its output - * to prevent exceeding the limit. - * - * @param currentLimitAmps the maximum allowed current, in amperes + * @param angularVelocity the angular velocity of the motor + * @return this instance for method chaining */ - public void enableCurrentLimit(double currentLimitAmps) { - this.currentLimitAmps = currentLimitAmps; + public BrushlessMotorSim withOverrideAngularVelocity(AngularVelocity angularVelocity) { + sim.setAngularVelocity(angularVelocity.in(RadiansPerSecond)); + return this; + } + + public BrushlessMotorSim withFeedForward( + Voltage kS, + Per kV, + Per kA) { + var kVUnit = PerUnit.combine(Volts, RadiansPerSecond); + var kAUnit = PerUnit.combine(Volts, RadiansPerSecondPerSecond); + feedforward = + new SimpleMotorFeedforward( + kS.in(Volts), kV.in(kVUnit), kA.in(kAUnit), SimulatedField.getSimulationDt()); + return this; } /** + * Configures the PD controller for Positional Requests using {@link OutputType#VOLTAGE}. * + *

    This is unit safe and can be configure like so: * - *

    Disables the Current Limit of the Motor.

    + * 
    
+   * // Volts per Rotation of error is how CTRE handles PID when used with voltage requests
+   * sim.withPositionalVoltageController(
+   *   Volts.per(Rotation).ofNative(100.0),
+   *   Volts.per(RotationsPerSecond).ofNative(5.0)
+   * );
+   *
    * - *

    This method removes the current limit, allowing the motor to operate without any current - * restrictions. + * @param kP the proportional gain + * @param kD the derivative gain + * @return this instance for method chaining */ - public void disableCurrentLimit() { - this.currentLimitAmps = Double.POSITIVE_INFINITY; + public BrushlessMotorSim withPositionalVoltageController( + Per kP, Per kD) { + var kPUnit = PerUnit.combine(Volts, Radians); + var kDUnit = PerUnit.combine(Volts, RadiansPerSecond); + poseVoltController.setP(kP.in(kPUnit)); + poseVoltController.setD(kD.in(kDUnit)); + return this; } /** + * Configures the PD controller for Velocity Requests using {@link OutputType#VOLTAGE}. * + *

    This is unit safe and can be configure like so: * - *

    Configures the Zero Power Behavior of the Motor.

    - * - *

    This method sets the motor's zero power behavior, similar to the - * setZeroPowerBehavior() method for brushless motors. + * 

    
+   * // Volts per RPS of error is how CTRE handles PID when used with voltage requests
+   * sim.withVelocityVoltageController(
+   *   Volts.per(RotationsPerSecond).ofNative(0.4)
+   * );
+   *
    * - *

    Use this feature only when simulating brushless motors. - * - * @param enabled true to enable brake mode, false to enable coast mode + * @param kP the proportional gain + * @return this instance for method chaining */ - public void setMotorBrakeEnabled(boolean enabled) { - this.breakModeEnabled = enabled; + public BrushlessMotorSim withVelocityVoltageController(Per kP) { + var kPUnit = PerUnit.combine(Volts, Radians); + veloVoltController.setP(kP.in(kPUnit)); + return this; } /** + * Configures the PD controller for Positional Requests using {@link OutputType#CURRENT}. * + *

    This is unit safe and can be configure like so: * - *

    Sets the Current State of the Motor.

    - * - *

    This method instantly shifts the motor to a given state, setting its angular position and - * velocity. + * 

    
+   * // Amps per Rotation of error is how CTRE handles PID when used with current requests
+   * sim.withPositionalCurrentController(
+   *   Amps.per(Rotation).ofNative(100.0),
+   *   Amps.per(RotationsPerSecond).ofNative(5.0)
+   * );
+   *
    * - *

    Equivalent to the {@link DCMotorSim#setState(double, double)} method. - * - * @param angularPositionRad the angular position of the motor, in radians - * @param angularVelocityRadPerSec the angular velocity of the motor, in radians per second + * @param kP the proportional gain + * @param kD the derivative gain + * @return this instance for method chaining */ - public void setState(double angularPositionRad, double angularVelocityRadPerSec) { - this.angularPositionRad = angularPositionRad; - this.angularVelocityRadPerSec = angularVelocityRadPerSec; + public BrushlessMotorSim withPositionalCurrentController( + Per kP, Per kD) { + var kPUnit = PerUnit.combine(Amps, Radians); + var kDUnit = PerUnit.combine(Amps, RadiansPerSecond); + poseCurrentController.setP(kP.in(kPUnit)); + poseCurrentController.setD(kD.in(kDUnit)); + return this; } /** + * Configures the PD controller for Velocity Requests using {@link OutputType#CURRENT}. * + *

    This is unit safe and can be configure like so: * - *

    Updates the Simulation.

    - * - *

    This method steps the motor simulation forward by a given time interval (dt), - * recalculating and storing the states of the motor. + * 

    
+   * // Amps per RPS of error is how CTRE handles PID when used with current requests
+   * sim.withVelocityCurrentController(
+   *   Amps.per(RotationsPerSecond).ofNative(0.4)
+   * );
+   *
    * - *

    Equivalent to {@link DCMotorSim#update(double)}. - * - * @param dtSeconds the time step for the simulation, in seconds + * @param kP the proportional gain + * @return this instance for method chaining */ - public void update(double dtSeconds) { - appliedVolts = - constrainOutputVoltage( - motor, angularVelocityRadPerSec * gearRatio, currentLimitAmps, requestedVoltageOutput); - - double totalTorque = getMotorElectricTorque(); - - /* apply friction force */ - final boolean electricTorqueResultingInAcceleration = - totalTorque * angularVelocityRadPerSec > 0; - if (electricTorqueResultingInAcceleration) - totalTorque = MathUtil.applyDeadband(totalTorque, frictionTorque, Double.POSITIVE_INFINITY); - else totalTorque += getCurrentFrictionTorque(); - - this.angularVelocityRadPerSec += totalTorque / loadInertiaJKgMetersSquared * dtSeconds; - this.angularPositionRad += this.angularVelocityRadPerSec * dtSeconds; + public BrushlessMotorSim withVelocityCurrentController(Per kP) { + var kPUnit = PerUnit.combine(Amps, Radians); + veloCurrentController.setP(kP.in(kPUnit)); + return this; } /** + * Configures the positionaly controllers to use continuous wrap. * - * - *

    Calculates the Electric Torque on the Rotor.

    - * - * @return the torque applied to the mechanism by the motor's electrical output + * @param min the minimum angle + * @param max the maximum angle + * @return this instance for method chaining + * @see PIDController#enableContinuousInput(double, double) */ - private double getMotorElectricTorque() { - if (!breakModeEnabled && appliedVolts == 0) return currentDrawAmps = 0; - currentDrawAmps = motor.getCurrent(angularVelocityRadPerSec * gearRatio, appliedVolts); - return motor.getTorque(currentDrawAmps) * gearRatio; + public BrushlessMotorSim withControllerContinousInput(Angle min, Angle max) { + poseVoltController.enableContinuousInput(min.in(Radians), max.in(Radians)); + poseCurrentController.enableContinuousInput(min.in(Radians), max.in(Radians)); + return this; } /** + * Configures the current limit for the motor. * + *

    This is the total current limit for the sim * - *

    Calculates the Dynamic Friction Torque on the Mechanism.

    - * - *

    This method simulates the amount of dynamic friction acting on the rotor when the mechanism - * is rotating. - * - *

    In the real world, dynamic friction torque is typically constant. However, in the - * simulation, it is made proportional to the rotor speed when the speed is small to avoid - * oscillation. - * - * @return the amount of dynamic friction torque on the mechanism, in Newton-meters + * @param currentLimit the current limit for the motor + * @return */ - private double getCurrentFrictionTorque() { - final double kFriction = 3.0, - percentAngularVelocity = - Math.abs(angularVelocityRadPerSec) * gearRatio / motor.freeSpeedRadPerSec, - currentFrictionTorqueMagnitude = - Math.min(percentAngularVelocity * kFriction * frictionTorque, frictionTorque); - return Math.copySign(currentFrictionTorqueMagnitude, -angularVelocityRadPerSec); + public BrushlessMotorSim withStatorCurrentLimit(Current currentLimit) { + // this is a limit across the sum of all motors output, + // so it should be set to the total current limit of the mechanism + this.currentLimit = currentLimit; + return this; } /** + * Configures the hard limits for the motor. * - * - *

    Constrains the Output Voltage of the Motor.

    - * - *

    This method constrains the output voltage of the motor to ensure it operates within the - * current limit and does not exceed the available voltage from the robot's system. - * - *

    The output voltage is constrained such that the motor does not function outside of the - * specified current limit. Additionally, it ensures that the voltage does not exceed the robot's - * input voltage, which can be obtained from {@link RoboRioSim#getVInVoltage()}. - * - * @param motor the {@link DCMotor} that models the motor - * @param motorCurrentVelocityRadPerSec the current velocity of the motor's rotor (geared), in - * radians per second - * @param currentLimitAmps the configured current limit, in amperes. You can configure the current - * limit using {@link #enableCurrentLimit(double)}. - * @param requestedOutputVoltage the requested output voltage - * @return the constrained output voltage based on the current limit and system voltage + * @param forwardLimit the forward limit + * @param reverseLimit the reverse limit + * @return this instance for method chaining */ - public static double constrainOutputVoltage( - DCMotor motor, - double motorCurrentVelocityRadPerSec, - double currentLimitAmps, - double requestedOutputVoltage) { - final double currentAtRequestedVolts = - motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage); + public BrushlessMotorSim withHardLimits(Angle forwardLimit, Angle reverseLimit) { + this.forwardLimit = forwardLimit; + this.reverseLimit = reverseLimit; + return this; + } - /* normally, motor controller starts cutting the supply voltage when the current exceed 120% the current limit */ - final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > 1.2 * currentLimitAmps; - double limitedVoltage = requestedOutputVoltage; - if (currentTooHigh) { - final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts); - limitedVoltage = - motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); + public MomentOfInertia getMOI() { + return KilogramSquareMeters.of(sim.getJKgMetersSquared()); + } + + public Angle getPosition() { + return Radians.of(sim.getAngularPositionRad()); + } + + public AngularVelocity getVelocity() { + return RadiansPerSecond.of(sim.getAngularVelocityRadPerSec()); + } + + public AngularAcceleration getAcceleration() { + return RadiansPerSecondPerSecond.of(sim.getAngularAccelerationRadPerSecSq()); + } + + public Current getStatorCurrentDraw() { + return Amps.of(sim.getCurrentDrawAmps()); + } + + public Current getSupplyCurrent() { + // https://www.chiefdelphi.com/t/current-limiting-talonfx-values/374780/10 + return getStatorCurrentDraw() + .times(sim.getInputVoltage() / RobotController.getBatteryVoltage()); + } + + public Voltage getRotorVoltage() { + return Volts.of(sim.getInputVoltage()); + } + + public Voltage getSupplyVoltage() { + return Volts.of(RobotController.getBatteryVoltage()); + } + + public void setControl(OutputType outputType, AngularVelocity velo) { + this.outputType = outputType; + this.outputMode = OutputMode.VELOCITY; + this.output = velo.in(RadiansPerSecond); + } + + public void setControl(OutputType outputType, Angle pos) { + this.outputType = outputType; + this.outputMode = OutputMode.POSITION; + this.output = pos.in(Radians); + } + + public void setControl(Current amps) { + this.outputType = OutputType.CURRENT; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = amps.in(Amps); + } + + public void setControl(Voltage volts) { + this.outputType = OutputType.VOLTAGE; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = volts.in(Volts); + } + + public void setControl() { + this.outputType = OutputType.VOLTAGE; + this.outputMode = OutputMode.OPEN_LOOP; + this.output = 0.0; + } + + public void update() { + double dtSeconds = SimulatedField.getSimulationDt(); + switch (this.outputType) { + case VOLTAGE -> { + switch (this.outputMode) { + case OPEN_LOOP -> { + driveAtVoltage(Volts.of(output)); + } + case POSITION -> { + Voltage voltage = + Volts.of(poseVoltController.calculate(getPosition().in(Radians), output)); + Voltage feedforwardVoltage = + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + velocityForVolts(voltage).in(RadiansPerSecond))); + driveAtVoltage(feedforwardVoltage.plus(voltage)); + } + case VELOCITY -> { + Voltage voltage = + Volts.of(veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output)); + Voltage feedforwardVoltage = + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + RadiansPerSecond.of(output).in(RadiansPerSecond))); + driveAtVoltage(voltage.plus(feedforwardVoltage)); + } + } + } + case CURRENT -> { + switch (this.outputMode) { + case OPEN_LOOP -> { + sim.setInputVoltage(voltsForAmps(Amps.of(output), getVelocity()).in(Volts)); + } + case POSITION -> { + Current current = + Amps.of(poseCurrentController.calculate(getPosition().in(Radians), output)); + Voltage voltage = voltsForAmps(current, getVelocity()); + Voltage feedforwardVoltage = + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + velocityForVolts(voltage).in(RadiansPerSecond))); + driveAtVoltage(feedforwardVoltage.plus(voltage)); + } + case VELOCITY -> { + Current current = + Amps.of(veloCurrentController.calculate(getPosition().in(Radians), output)); + Voltage feedforwardVoltage = + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + RadiansPerSecond.of(output).in(RadiansPerSecond))); + Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage); + driveAtVoltage(voltage); + } + } + } } - if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) - limitedVoltage = requestedOutputVoltage; + sim.update(dtSeconds); - return MathUtil.clamp(limitedVoltage, -RoboRioSim.getVInVoltage(), RoboRioSim.getVInVoltage()); + if (getPosition().lte(reverseLimit)) { + sim.setState(reverseLimit.in(Radians), 0.0); + } else if (getPosition().gte(forwardLimit)) { + sim.setState(forwardLimit.in(Radians), 0.0); + } } - /** - * - * - *

    Obtains the Voltage Actually Applied to the Motor.

    - * - *

    This method returns the voltage that is actually applied to the motor after being - * constrained by {@link #constrainOutputVoltage(DCMotor, double, double, double)}. - * - *

    The voltage is constrained to ensure that the current limit is not exceeded and that it does - * not surpass the robot's battery voltage. - * - * @return the constrained voltage actually applied to the motor - */ - public double getAppliedVolts() { - return appliedVolts; + private void driveAtVoltage(Voltage voltage) { + // The voltage constrained to current limits and battery voltage + Voltage constrained = constrainOutputVoltage(voltage); + Voltage frictionVoltage = applyFriction(constrained); + + sim.setInputVoltage(frictionVoltage.in(Volts)); } - /** - * - * - *

    Obtains the Angular Position of the Mechanism.

    - * - *

    This method is equivalent to {@link DCMotorSim#getAngularPositionRad()}. - * - * @return the final angular position of the mechanism, in radians - */ - public double getAngularPositionRad() { - return angularPositionRad; + private Voltage applyFriction(Voltage voltage) { + // This function is responsible for slowing down acceleration + // and slowing down the velocity of the motor when at lowere output. + // This is not the same as the static friction, which is the force + // required to get the motor moving. + + // to apply friction we convert the motors output to torque then back to voltage + double current = + motor.getCurrent(sim.getAngularVelocityRadPerSec() * gearing, voltage.in(Volts)); + double currentVelo = getVelocity().in(RadiansPerSecond) * gearing; + double torque = motor.getTorque(current); + double friction = frictionTorque().in(NewtonMeters); + + boolean movingForward = currentVelo > 0; + + if (movingForward && currentVelo > motor.getSpeed(torque, sim.getInputVoltage())) { + // the motor is moving faster than it should based on the output voltage + // apply the friction to slow it down + torque -= friction; + } else if (!movingForward && currentVelo < motor.getSpeed(torque, sim.getInputVoltage())) { + // the motor is moving slower than it should based on the output voltage + // apply the friction to speed it up + torque += friction; + } + + return Volts.of(motor.getVoltage(torque, currentVelo)); } - /** - * - * - *

    Obtains the Angular Position of the Motor.

    - * - * @return the un-geared angular position of the motor (encoder reading), in radians - */ - public double getEncoderPositionRad() { - return angularPositionRad * gearRatio; + private Voltage voltsForAmps(Current current, AngularVelocity angularVelocity) { + // find what voltage is needed to get the current + return Volts.of( + motor.getVoltage(current.in(Amps), angularVelocity.in(RadiansPerSecond) * gearing)); } - /** - * - * - *

    Obtains the Angular Velocity of the Mechanism.

    - * - *

    This method is equivalent to {@link DCMotorSim#getAngularVelocityRadPerSec()}. - * - * @return the final angular velocity of the mechanism, in radians per second - */ - public double getAngularVelocityRadPerSec() { - return angularVelocityRadPerSec; + private AngularVelocity velocityForVolts(Voltage voltage) { + return RadiansPerSecond.of( + motor.getSpeed(motor.getTorque(getStatorCurrentDraw().in(Amps)), voltage.in(Volts))); } - /** - * - * - *

    Obtains the Angular Velocity of the Encoder.

    - * - * @return the un-geared angular velocity of the motor (encoder velocity), in radians per second - */ - public double getEncoderVelocityRadPerSec() { - return angularVelocityRadPerSec * gearRatio; + private Torque frictionTorque() { + return NewtonMeters.of( + motor.getTorque(motor.getCurrent(0.0, frictionVoltage.in(Volts))) * gearing); } - /** - * - * - *

    Obtains the Amount of Current Flowing into the Motor.

    - * - *

    This method is equivalent to {@link DCMotorSim#getCurrentDrawAmps()}. - * - * @return the amount of current flowing into the motor, in amperes - */ - public double getCurrentDrawAmps() { - return currentDrawAmps; + private Voltage constrainOutputVoltage(Voltage requestedOutput) { + final double kCurrentThreshold = 1.2; + + final double motorCurrentVelocityRadPerSec = getVelocity().in(RadiansPerSecond); + final double currentLimitAmps = currentLimit.in(Amps); + final double requestedOutputVoltage = requestedOutput.in(Volts); + final double currentAtRequestedVolts = + motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage); + + // Resource for current limiting: + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3) + final boolean currentTooHigh = + Math.abs(currentAtRequestedVolts) > (kCurrentThreshold * currentLimitAmps); + double limitedVoltage = requestedOutputVoltage; + if (currentTooHigh) { + final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts); + limitedVoltage = + motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); + } + + // ensure the current limit doesn't cause an increase to output voltage + if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) { + limitedVoltage = requestedOutputVoltage; + } + + // constrain the output voltage to the battery voltage + return Volts.of( + MathUtil.clamp( + limitedVoltage, + -RobotController.getBatteryVoltage(), + RobotController.getBatteryVoltage())); } } diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java index 74235ba..0702d5e 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java @@ -170,7 +170,7 @@ public static GyroSimulation createPigeon2() { * IMU */ public static GyroSimulation createNavX2() { - return new GyroSimulation(2, 0.04); + return new GyroSimulation(0.00208333333, 0.04); } /** diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index cd5fa62..5c2c25e 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -1,42 +1,85 @@ package frc.robot.extras.simulation.mechanismSim.swerve; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; -import static frc.robot.extras.simulation.field.SimulatedField.SIMULATION_DT; -import static frc.robot.extras.simulation.field.SimulatedField.SIMULATION_SUB_TICKS_IN_1_PERIOD; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; +import frc.robot.extras.simulation.field.SimulatedField; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import java.util.Queue; import java.util.concurrent.ConcurrentLinkedQueue; import java.util.function.Supplier; import org.dyn4j.geometry.Vector2; +/** + * + * + *

    Simulation for a Single Swerve Module.

    + * + *

    This class provides a simulation for a single swerve module in the {@link + * SwerveDriveSimulation}. + * + *

    1. Purpose

    + * + *

    This class serves as the bridge between your code and the physics engine. + * + *

    You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder + * readings in your code, just as how you deal with your physical motors. + * + *

    2. Perspectives

    + * + *
      + *
    • Simulates the steering mechanism using a custom brushless motor simulator. + *
    • Simulates the propelling force generated by the driving motor, with a current limit. + *
    • Simulates encoder readings, which can be used to simulate a {@link SwerveDriveOdometry}. + *
    + * + *

    3. Simulating Odometry

    + * + *
      + *
    • Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPositionRad()}} and + * {@link #getTurnAbsolutePosition()}. + *
    • Use {@link SwerveDriveOdometry} to estimate the pose of your robot. + *
    • 250Hz + * Odometry is supported. You can retrive cached encoder readings from every sub-tick + * through {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link + * #getCachedTurnAbsolutePositions()}. + *
    + * + *

    An example of how to simulate odometry using this class is the ModuleIOSim.java + * from the Advanced Swerve Drive with maple-sim example. + */ public class SwerveModuleSimulation { - private final DCMotor DRIVE_MOTOR; + public final DCMotor DRIVE_MOTOR; private final BrushlessMotorSim turnMotorSim; - private final double DRIVE_CURRENT_LIMIT, + public final double DRIVE_CURRENT_LIMIT, DRIVE_GEAR_RATIO, TURN_GEAR_RATIO, DRIVE_FRICTION_VOLTAGE, - TURN_FRICTION_VOLTAGE, WHEELS_COEFFICIENT_OF_FRICTION, WHEEL_RADIUS_METERS, DRIVE_WHEEL_INERTIA = 0.01; private double driveMotorRequestedVolts = 0.0, - turnMotorAppliedVolts = 0.0, driveMotorAppliedVolts = 0.0, driveMotorSupplyCurrentAmps = 0.0, - turnMotorSupplyCurrentAmps = 0.0, turnRelativeEncoderPositionRad = 0.0, turnRelativeEncoderSpeedRadPerSec = 0.0, turnAbsoluteEncoderSpeedRadPerSec = 0.0, driveEncoderUnGearedPositionRad = 0.0, driveEncoderUnGearedSpeedRadPerSec = 0.0; - private Rotation2d turnAbsoluteRotation = Rotation2d.fromRotations(Math.random()); + private Rotation2d turnAbsolutePosition = Rotation2d.fromRotations(Math.random()); private final double turnRelativeEncoderOffSet = (Math.random() - 0.5) * 30; @@ -44,6 +87,31 @@ public class SwerveModuleSimulation { cachedDriveEncoderUnGearedPositionsRad; private final Queue cachedTurnAbsolutePositions; + /** + * + * + *

    Constructs a Swerve Module Simulation.

    + * + *

    If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom + * timings, you must call the method before constructing any swerve module simulations using this + * constructor. + * + * @param driveMotor the model of the driving motor + * @param turnMotor the model of the steering motor + * @param driveCurrentLimit the current limit for the driving motor, in amperes + * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction + * @param turnGearRatio the gear ratio for the steering motor, >1 is reduction + * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving + * rotter, in volts + * @param turnFrictionVoltage the measured minimum amount of voltage that can turn the steering + * rotter, in volts + * @param tireCoefficientOfFriction the coefficient + * of friction of the tires, normally around 1.5 + * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using {@link + * Units#inchesToMeters(double)}. + * @param turnRotationalInertia the rotational inertia of the steering mechanism + */ public SwerveModuleSimulation( DCMotor driveMotor, DCMotor turnMotor, @@ -60,112 +128,309 @@ public SwerveModuleSimulation( DRIVE_GEAR_RATIO = driveGearRatio; TURN_GEAR_RATIO = turnGearRatio; DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; - TURN_FRICTION_VOLTAGE = turnFrictionVoltage; WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction; WHEEL_RADIUS_METERS = wheelsRadiusMeters; this.turnMotorSim = new BrushlessMotorSim( - turnMotor, TURN_GEAR_RATIO, turnRotationalInertia, TURN_FRICTION_VOLTAGE); + SimulatedField.getInstance(), + turnMotor, + turnGearRatio, + KilogramSquareMeters.of(turnRotationalInertia), + Volts.of(turnFrictionVoltage)); this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); this.cachedTurnRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); this.cachedTurnAbsolutePositions = new ConcurrentLinkedQueue<>(); - for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) - cachedTurnAbsolutePositions.offer(turnAbsoluteRotation); + for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++) + cachedTurnAbsolutePositions.offer(turnAbsolutePosition); this.turnRelativeEncoderPositionRad = - turnAbsoluteRotation.getRadians() + turnRelativeEncoderOffSet; - } - - public void requestDriveVoltageOut(double volts) { - this.driveMotorRequestedVolts = volts; - } - - public void requestTurnVoltageOut(double volts) { - this.turnMotorAppliedVolts = volts; - // this.turnMotorSim.setInputVoltage(MathUtil.applyDeadband(volts, turn_FRICTION_VOLTAGE, - // 12)); + turnAbsolutePosition.getRadians() + turnRelativeEncoderOffSet; } + /** + * + * + *

    Requests the Driving Motor to Run at a Specified Voltage Output.

    + * + *

    Think of it as the setVoltage() of your physical driving motor.

    + * + *

    This method sets the desired voltage output for the driving motor. The change will be + * applied in the next sub-tick of the simulation. + * + *

    Note: The requested voltage may not always be fully applied if the current + * is too high. The current limit may reduce the motor's output, similar to real motors. + * + *

    To check the actual voltage applied to the drivetrain, use {@link + * #getDriveMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the driving motor + */ public void requestDriveVoltageOut(Voltage volts) { this.driveMotorRequestedVolts = volts.in(Volts); } + /** + * + * + *

    Requests the Steering Motor to Run at a Specified Voltage Output.

    + * + *

    Think of it as the setVoltage() of your physical steering motor.

    + * + *

    This method sets the desired voltage output for the steering motor. The change will be + * applied in the next sub-tick of the simulation. + * + *

    Note: Similar to the drive motor, the requested voltage may not always be + * fully applied if the current exceeds the limit. The current limit will reduce the motor's + * output as needed, mimicking real motor behavior. + * + *

    To check the actual voltage applied to the steering motor, use {@link + * #getTurnMotorAppliedVolts()}. + * + * @param volts the voltage to be applied to the steering motor + */ public void requestTurnVoltageOut(Voltage volts) { - this.turnMotorAppliedVolts = volts.in(Volts); - // this.turnMotorSim.setInputVoltage(MathUtil.applyDeadband(volts, turn_FRICTION_VOLTAGE, - // 12)); + this.turnMotorSim.setControl(volts); } + /** + * + * + *

    Obtains the Actual Output Voltage of the Drive Motor.

    + * + *

    This method returns the actual voltage being applied to the drive motor. The actual applied + * voltage may differ from the value set by {@link #requestDriveVoltageOut(double)}. + * + *

    If the motor's supply current is too high, the motor will automatically reduce its output + * voltage to protect the system. + * + * @return the actual output voltage of the drive motor, in volts + */ public double getDriveMotorAppliedVolts() { return driveMotorAppliedVolts; } + /** + * + * + *

    Obtains the Actual Output Voltage of the Steering Motor.

    + * + *

    This method returns the actual voltage being applied to the steering motor. It wraps around + * the {@link BrushlessMotorSim#getAppliedVolts()} method. + * + *

    The actual applied voltage may differ from the value set by {@link + * #requestTurnVoltageOut(double)}. If the motor's supply current is too high, the motor will + * automatically reduce its output voltage to protect the system. + * + * @return the actual output voltage of the steering motor, in volts + */ public double getTurnMotorAppliedVolts() { - return turnMotorAppliedVolts; + return turnMotorSim.getRotorVoltage().in(Volts); } + /** + * + * + *

    Obtains the Amount of Current Supplied to the Drive Motor.

    + * + *

    Think of it as the getSupplyCurrent() of your physical drive motor.

    + * + * @return the current supplied to the drive motor, in amperes + */ public double getDriveMotorSupplyCurrentAmps() { return driveMotorSupplyCurrentAmps; } + /** + * + * + *

    Obtains the Amount of Current Supplied to the Steer Motor.

    + * + *

    Think of it as the getSupplyCurrent() of your physical steer motor.

    + * + *

    This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}. + * + * @return the current supplied to the steer motor, in amperes + */ public double getTurnMotorSupplyCurrentAmps() { - return turnMotorSupplyCurrentAmps; + return turnMotorSim.getSupplyCurrent().in(Amps); } + /** + * + * + *

    Obtains the Position of the Drive Encoder.

    + * + *

    Think of it as the getPosition() of your physical driving motor.

    + * + *

    This method is used to simulate your {@link SwerveDrivePoseEstimator}. + * + *

    This value represents the un-geared position of the encoder, i.e., the amount of radians the + * drive motor's encoder has rotated. + * + *

    To get the final wheel rotations, use {@link #getDriveEncoderFinalPositionRad()}. + * + * @return the position of the drive motor's encoder, in radians (un-geared) + */ public double getDriveEncoderUnGearedPositionRad() { return driveEncoderUnGearedPositionRad; } + /** + * + * + *

    Obtains the Final Position of the Drive Encoder (Wheel Rotations).

    + * + *

    This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final + * position of the drive encoder in terms of wheel rotations. + * + *

    The value is calculated by dividing the un-geared encoder position (obtained from {@link + * #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio. + * + * @return the final position of the drive encoder (wheel rotations), in radians + */ public double getDriveEncoderFinalPositionRad() { return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; } + /** + * + * + *

    Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.

    + * + *

    Think of it as the getVelocity() of your physical drive motor.

    + * + *

    This method returns the current speed of the drive encoder in radians per second, without + * accounting for the drive gear ratio. + * + * @return the un-geared speed of the drive encoder, in radians per second + */ public double getDriveEncoderUnGearedSpeedRadPerSec() { return driveEncoderUnGearedSpeedRadPerSec; } + /** + * + * + *

    Obtains the Final Speed of the Wheel, in Radians per Second.

    + * + * @return the final speed of the drive wheel, in radians per second + */ public double getDriveWheelFinalSpeedRadPerSec() { return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; } - /** geared */ + /** + * + * + *

    Obtains the Relative Position of the Steer Encoder, in Radians.

    + * + *

    Think of it as the getPosition() of your physical steer motor.

    + * + * @return the relative encoder position of the steer motor, in radians + */ public double getTurnRelativeEncoderPositionRad() { return turnRelativeEncoderPositionRad; } - /** geared */ + /** + * + * + *

    Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).

    + * + *

    Think of it as the getVelocity() of your physical steer motor.

    + * + * @return the speed of the steer relative encoder, in radians per second (geared) + */ public double getTurnRelativeEncoderSpeedRadPerSec() { return turnRelativeEncoderSpeedRadPerSec; } + /** + * + * + *

    Obtains the Absolute Facing of the Steer Mechanism.

    + * + *

    Think of it as the getAbsolutePosition() of your CanCoder.

    + * + * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} + */ public Rotation2d getTurnAbsolutePosition() { - return turnAbsoluteRotation; + return turnAbsolutePosition; } + /** + * + * + *

    Obtains the Absolute Rotational Velocity of the Steer Mechanism.

    + * + *

    Think of it as the getVelocity() of your CanCoder.

    + * + * @return the absolute angular velocity of the steer mechanism, in radians per second + */ public double getTurnAbsoluteEncoderSpeedRadPerSec() { return turnAbsoluteEncoderSpeedRadPerSec; } + /** + * + * + *

    Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

    + * + *

    The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and + * can be retrieved using this method. + * + * @return an array of cached drive encoder un-geared positions, in radians + */ public double[] getCachedDriveEncoderUnGearedPositionsRad() { return cachedDriveEncoderUnGearedPositionsRad.stream().mapToDouble(value -> value).toArray(); } + /** + * + * + *

    Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

    + * + *

    The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and + * are divided by the gear ratio to obtain the final wheel rotations. + * + * @return an array of cached drive encoder final positions (wheel rotations), in radians + */ public double[] getCachedDriveWheelFinalPositionsRad() { return cachedDriveEncoderUnGearedPositionsRad.stream() - .mapToDouble(value -> value / DRIVE_GEAR_RATIO) + .mapToDouble(value -> value * ModuleConstants.DRIVE_TO_METERS) .toArray(); } + /** + * + * + *

    Obtains the Cached Readings of the Steer Relative Encoder's Position.

    + * + *

    The values of {@link #getTurnRelativeEncoderPositionRad()} are cached at each sub-tick and + * can be retrieved using this method. + * + * @return an array of cached steer relative encoder positions, in radians + */ public double[] getCachedTurnRelativeEncoderPositions() { return cachedTurnRelativeEncoderPositionsRad.stream().mapToDouble(value -> value).toArray(); } + /** + * + * + *

    Obtains the Cached Readings of the Steer Absolute Positions.

    + * + *

    The values of {@link #getTurnAbsolutePosition()} are cached at each sub-tick and can be + * retrieved using this method. + * + * @return an array of cached absolute steer positions, as {@link Rotation2d} objects + */ public Rotation2d[] getCachedTurnAbsolutePositions() { return cachedTurnAbsolutePositions.toArray(Rotation2d[]::new); } @@ -175,50 +440,90 @@ protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { } /** - * updates the simulation sub-tick for this module, updating its inner status (sensor readings) - * and calculating a total force * - * @param moduleCurrentGroundVelocityWorldRelative - * @return the propelling force that the module generates + * + *

    Updates the Simulation for This Module.

    + * + *

    This method is called once during every sub-tick of the simulation. It performs the + * following actions: + * + *

      + *
    • Updates the simulation of the steering mechanism. + *
    • Simulates the propelling force generated by the module. + *
    • Updates and caches the encoder readings for odometry simulation. + *
    + * + *

    Note: Friction forces are not simulated in this method. + * + * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, + * relative to the world + * @param robotFacing the absolute facing of the robot, relative to the world + * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons + * @return the propelling force generated by the module, as a {@link Vector2} object */ public Vector2 updateSimulationSubTickGetModuleForce( Vector2 moduleCurrentGroundVelocityWorldRelative, - Rotation2d robotRotation, + Rotation2d robotFacing, double gravityForceOnModuleNewtons) { - updateturnSimulation(); + updateTurnSimulation(); /* the maximum gripping force that the wheel can generate */ final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons); - final Rotation2d moduleWorldRotation = this.turnAbsoluteRotation.plus(robotRotation); + final Rotation2d moduleWorldFacing = this.turnAbsolutePosition.plus(robotFacing); final Vector2 propellingForce = getPropellingForce( - grippingForceNewtons, moduleWorldRotation, moduleCurrentGroundVelocityWorldRelative); - updateEncoderTicks(); + grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); + updateDriveEncoders(); return propellingForce; } - /** */ - private void updateturnSimulation() { - turnMotorSim.update(SIMULATION_DT); - + /** + * + * + *

    updates the simulation for the steer mechanism.

    + * + *

    Updates the simulation for the steer mechanism and cache the encoder readings. + * + *

    The steer mechanism is modeled by a {@link BrushlessMotorSim}. + */ + private void updateTurnSimulation() { /* update the readings of the sensor */ - this.turnAbsoluteRotation = Rotation2d.fromRadians(turnMotorSim.getAngularPositionRad()); + this.turnAbsolutePosition = new Rotation2d(turnMotorSim.getPosition()); this.turnRelativeEncoderPositionRad = - turnMotorSim.getAngularPositionRad() + turnRelativeEncoderOffSet; - this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getAngularVelocityRadPerSec(); + turnMotorSim.getPosition().in(Radians) + turnRelativeEncoderOffSet; + this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getVelocity().in(RadiansPerSecond); this.turnRelativeEncoderSpeedRadPerSec = turnAbsoluteEncoderSpeedRadPerSec * TURN_GEAR_RATIO; /* cache sensor readings to queue for high-frequency odometry */ this.cachedTurnAbsolutePositions.poll(); - this.cachedTurnAbsolutePositions.offer(turnAbsoluteRotation); + this.cachedTurnAbsolutePositions.offer(turnAbsolutePosition); this.cachedTurnRelativeEncoderPositionsRad.poll(); this.cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad); } + /** + * + * + *

    Calculates the amount of propelling force that the module generates.

    + * + *

    The swerve module's drive motor will generate a propelling force. + * + *

    For most of the time, that propelling force is directly applied to the drivetrain. And the + * drive wheel runs as fast as the ground velocity + * + *

    However, if the propelling force exceeds the gripping, only the max gripping force is + * applied. The rest of the propelling force will cause the wheel to start skidding and make the + * odometry inaccurate. + * + * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons + * @param moduleWorldFacing the current world facing of the module + * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference + * @return a vector representing the propelling force that the module generates, world-reference + */ private Vector2 getPropellingForce( double grippingForceNewtons, - Rotation2d moduleWorldRotation, + Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { final double driveWheelTorque = getDriveWheelTorque(), theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS; @@ -233,39 +538,34 @@ private Vector2 getPropellingForce( moduleCurrentGroundVelocity.getMagnitude() * Math.cos( moduleCurrentGroundVelocity.getAngleBetween( - new Vector2(moduleWorldRotation.getRadians()))); - - if (skidding) { - /* if the chassis is skidding, part of the toque will cause the wheels to spin freely */ - final double torqueOnWheel = driveWheelTorque * 0.3; - this.driveEncoderUnGearedSpeedRadPerSec += - torqueOnWheel / DRIVE_WHEEL_INERTIA * SIMULATION_DT * DRIVE_GEAR_RATIO; - } else // if the chassis is tightly gripped on floor, the floor velocity is projected to the - // wheel + new Vector2(moduleWorldFacing.getRadians()))); + + // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel + this.driveEncoderUnGearedSpeedRadPerSec = + floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; + + // if the module is skidding + if (skidding) this.driveEncoderUnGearedSpeedRadPerSec = - floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO; + 0.5 * driveEncoderUnGearedSpeedRadPerSec + + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts); - return Vector2.create(propellingForceNewtons, moduleWorldRotation.getRadians()); + return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); } + /** + * + * + *

    Calculates the amount of torque that the drive motor can generate on the wheel.

    + * + *

    Before calculating the torque of the motor, the output voltage of the drive motor is + * constrained for the current limit through {@link + * BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}. + * + * @return the amount of torque on the wheel by the drive motor, in Newton * Meters + */ private double getDriveWheelTorque() { - final double currentAtRequestedVolts = - DRIVE_MOTOR.getCurrent(this.driveEncoderUnGearedSpeedRadPerSec, driveMotorRequestedVolts); - driveMotorAppliedVolts = driveMotorRequestedVolts; - /* normally, motor controller starts cutting the supply voltage when the current exceed 150% the current limit */ - final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > 1.2 * DRIVE_CURRENT_LIMIT, - driveMotorTryingToAccelerate = driveMotorRequestedVolts * driveMotorSupplyCurrentAmps > 0; - - if (currentTooHigh && driveMotorTryingToAccelerate) { - /* activate current limit, cut down the applied voltage to match current limit */ - final double currentWithLimits = Math.copySign(DRIVE_CURRENT_LIMIT, currentAtRequestedVolts); - driveMotorAppliedVolts = - DRIVE_MOTOR.getVoltage( - DRIVE_MOTOR.getTorque(currentWithLimits), this.driveEncoderUnGearedSpeedRadPerSec); - } - - driveMotorAppliedVolts = MathUtil.clamp(driveMotorAppliedVolts, -12, 12); /* calculate the actual supply current */ driveMotorSupplyCurrentAmps = @@ -274,21 +574,25 @@ private double getDriveWheelTorque() { MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12)); /* calculate the torque generated, */ - final double torqueOnRotor = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); - return torqueOnRotor * DRIVE_GEAR_RATIO; + final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps); + return torqueOnRotter * DRIVE_GEAR_RATIO; } /** * @return the current module state of this simulation module */ - protected SwerveModuleState getCurrentState() { + public SwerveModuleState getCurrentState() { return new SwerveModuleState( - getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsoluteRotation); + getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsolutePosition); } /** - * gets the state of the module, if it is allowed to spin freely for a long time under the current - * applied drive volts + * + * + *

    Obtains the "free spin" state of the module

    + * + *

    The "free spin" state of a simulated module refers to its state after spinning freely for a + * long time under the current input voltage * * @return the free spinning module state */ @@ -299,37 +603,88 @@ protected SwerveModuleState getFreeSpinState() { driveMotorAppliedVolts) / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS, - turnAbsoluteRotation); + turnAbsolutePosition); } - private void updateEncoderTicks() { - this.driveEncoderUnGearedPositionRad += this.driveEncoderUnGearedSpeedRadPerSec * SIMULATION_DT; + /** + * + * + *

    Cache the encoder values.

    + * + *

    An internal method to cache the encoder values to their queues. + */ + private void updateDriveEncoders() { + this.driveEncoderUnGearedPositionRad += + this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt(); this.cachedDriveEncoderUnGearedPositionsRad.poll(); this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad); } + /** + * + * + *

    Obtains the theoretical speed that the module can achieve.

    + * + * @return the theoretical maximum ground speed that the module can achieve, in m/s + */ public double getModuleTheoreticalSpeedMPS() { return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS; } - public double getTheoreticalPropellingForcePerModule(double robotMass, int modulesCount) { + /** + * + * + *

    Obtains the theoretical maximum propelling force of ONE module.

    + * + *

    Calculates the maximum propelling force with respect to the gripping force and the drive + * motor's torque under its current limit. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force + * equally + * @return the maximum propelling force of EACH module + */ + public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) { final double maxThrustNewtons = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, - maxGrippingNewtons = 9.8 * robotMass / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; + maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; return Math.min(maxThrustNewtons, maxGrippingNewtons); } - public double getModuleMaxAccelerationMPSsq(double robotMass, int modulesCount) { - return getTheoreticalPropellingForcePerModule(robotMass, modulesCount) + /** + * + * + *

    Obtains the theatrical linear acceleration that the robot can achieve.

    + * + *

    Calculates the maximum linear acceleration of a robot, with respect to its mass and {@link + * #getTheoreticalPropellingForcePerModule(double, int)}. + * + * @param robotMassKg the mass of the robot, is kilograms + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force + * equally + */ + public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) { + return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount) * modulesCount - / robotMass; + / robotMassKg; } - public enum DRIVE_WHEEL_TYPE { - RUBBER, - TIRE + /** + * + * + *

    Stores the coefficient of friction of some common used wheels.

    + */ + public enum WHEEL_GRIP { + RUBBER_WHEEL(1.25), + TIRE_WHEEL(1.2); + + public final double cof; + + WHEEL_GRIP(double cof) { + this.cof = cof; + } } /** @@ -340,7 +695,7 @@ public static Supplier getModule( DCMotor driveMotor, DCMotor turnMotor, double driveCurrentLimitAmps, - DRIVE_WHEEL_TYPE driveWheelType, + WHEEL_GRIP driveWheelType, double driveGearRatio) { return () -> new SwerveModuleSimulation( @@ -351,10 +706,7 @@ public static Supplier getModule( 11.3142, 0.25, 0.05, - switch (driveWheelType) { - case RUBBER -> 1.55; - case TIRE -> 1.45; - }, + driveWheelType.cof, Units.inchesToMeters(2), 0.05); } diff --git a/src/main/java/frc/robot/extras/util/AllianceFlipper.java b/src/main/java/frc/robot/extras/util/AllianceFlipper.java index a1fe9e4..92ffe9c 100644 --- a/src/main/java/frc/robot/extras/util/AllianceFlipper.java +++ b/src/main/java/frc/robot/extras/util/AllianceFlipper.java @@ -12,10 +12,9 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Constants.FieldConstants; public class AllianceFlipper { - private static final double FIELD_LENGTH = 16.58112; - public static boolean isBlue() { return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue).equals(Alliance.Blue); } @@ -29,7 +28,8 @@ public static boolean isRed() { * @return Translation2d object with its x coordinate flipped over the y-axis */ public static final Translation2d flipTranslation(Translation2d translation) { - return new Translation2d(FIELD_LENGTH - translation.getX(), translation.getY()); + return new Translation2d( + FieldConstants.FIELD_LENGTH_METERS - translation.getX(), translation.getY()); } /** @@ -38,7 +38,9 @@ public static final Translation2d flipTranslation(Translation2d translation) { */ public static final Translation3d flipTranslation(Translation3d translation) { return new Translation3d( - FIELD_LENGTH - translation.getX(), translation.getY(), translation.getZ()); + FieldConstants.FIELD_LENGTH_METERS - translation.getX(), + translation.getY(), + translation.getZ()); } /** diff --git a/src/main/java/frc/robot/extras/util/GeomUtil.java b/src/main/java/frc/robot/extras/util/GeomUtil.java index 3b52c44..017c98e 100644 --- a/src/main/java/frc/robot/extras/util/GeomUtil.java +++ b/src/main/java/frc/robot/extras/util/GeomUtil.java @@ -50,7 +50,38 @@ public static ChassisSpeeds toWpilibChassisSpeeds( dyn4jLinearVelocity.x, dyn4jLinearVelocity.y, angularVelocityRadPerSec); } + /** + * Gets the x and y velocities of a ChassisSpeeds + * + * @param chassisSpeeds the ChassisSpeeds to retrieve velocities from + * @return a Translation2d containing the velocities in the x and y direction in meters per second + */ public static Translation2d getChassisSpeedsTranslationalComponent(ChassisSpeeds chassisSpeeds) { return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); } + + /** + * Checks if two translations are within a certain threshold in meters + * + * @param t1 Translation 1 as a Translation2d + * @param t2 Translation 2 as a Translation2d + * @param thresholdMeters the threshold between the two translations in meters + * @return true if the two translations are within thresholdMeters + */ + public static boolean isTranslationWithinThreshold( + Translation2d t1, Translation2d t2, double thresholdMeters) { + return t1.getDistance(t2) <= thresholdMeters; + } + + /** + * Checks if two rotations are within a certain threshold in degrees + * + * @param r1 Rotations 1 as a Rotation2d + * @param r2 Rotation 2 as a Rotation2d + * @param thresholdMeters the threshold between the two rotations in degrees + * @return true if the two rotations are within thresholdDegrees + */ + public static boolean isRotationWithinThreshold(double r1, double r2, double thresholdDegrees) { + return Math.abs(r1 - r2) <= thresholdDegrees; + } } diff --git a/src/main/java/frc/robot/extras/util/JoystickUtil.java b/src/main/java/frc/robot/extras/util/JoystickUtil.java new file mode 100644 index 0000000..1ffc4bb --- /dev/null +++ b/src/main/java/frc/robot/extras/util/JoystickUtil.java @@ -0,0 +1,102 @@ +package frc.robot.extras.util; + +import frc.robot.Constants.JoystickConstants; +import java.util.function.DoubleSupplier; + +public class JoystickUtil { + + /** + * Deadbands a value to 0 + * + * @param value Value to input + * @param deadband If the the input is within this value, the function will return 0. + * @return 0.0 if the value is within specified deadband range + */ + private static double deadband(double value, double deadband) { + if (Math.abs(value) > deadband) { + if (value > 0.0) { + return (value - deadband) / (1.0 - deadband); + } else { + return (value + deadband) / (1.0 - deadband); + } + } else { + return 0.0; + } + } + + /** + * Function to modify a singular joystick axis. + * + * @param supplierValue Supplies a double, usually by a joystick. + * @param exponent Exponent to raise the supplied value to. + * @return Returns the modified value. + */ + public static double modifyAxis(DoubleSupplier supplierValue, int exponent) { + double value = supplierValue.getAsDouble(); + + // Deadband + value = deadband(value, JoystickConstants.DEADBAND_VALUE); + + // Raise value to specified exponent + value = Math.copySign(Math.pow(value, exponent), value); + + return value; + } + + public static double modifyAxisCubed(DoubleSupplier supplierValue) { + double value = supplierValue.getAsDouble(); + + // Deadband + value = JoystickUtil.deadband(value, JoystickConstants.DEADBAND_VALUE); + + // Cube the axis + value = Math.copySign(value * value * value, value); + + return value; + } + + /** + * Converts the two axis coordinates to polar to get both the angle and radius, so they can work + * in a double[] list. + * + * @param xJoystick The supplied input of the xJoystick. + * @param yJoystick The supplied input of the yJoystick. + * @param exponent The exponent to raise the supplied value to. + * @return The modified axises of both joysticks in polar form. + */ + public static double[] modifyAxisPolar( + DoubleSupplier xJoystick, DoubleSupplier yJoystick, int exponent) { + double xInput = deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); + double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); + if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) { + double theta = Math.atan(xInput / yInput); + double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput); + double raisedHypotenuse = Math.pow(hypotenuse, exponent); + xInput = Math.copySign(Math.sin(theta) * raisedHypotenuse, xInput); + yInput = Math.copySign(Math.cos(theta) * raisedHypotenuse, yInput); + return new double[] {xInput, yInput}; + } + return new double[] { + Math.copySign(Math.pow(xInput, exponent), xInput), + Math.copySign(Math.pow(yInput, exponent), yInput) + }; + } + + public static double[] modifyAxisCubedPolar(DoubleSupplier xJoystick, DoubleSupplier yJoystick) { + double xInput = + JoystickUtil.deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); + double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); + if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) { + double theta = Math.atan(xInput / yInput); + double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput); + double cubedHypotenuse = Math.pow(hypotenuse, 3); + xInput = Math.copySign(Math.sin(theta) * cubedHypotenuse, xInput); + yInput = Math.copySign(Math.cos(theta) * cubedHypotenuse, yInput); + return new double[] {xInput, yInput}; + } + return new double[] { + Math.copySign(xInput * xInput * xInput, xInput), + Math.copySign(yInput * yInput * yInput, yInput) + }; + } +} diff --git a/src/main/java/frc/robot/extras/util/MathUtil.java b/src/main/java/frc/robot/extras/util/MathUtil.java index 65acfdf..f67401a 100644 --- a/src/main/java/frc/robot/extras/util/MathUtil.java +++ b/src/main/java/frc/robot/extras/util/MathUtil.java @@ -1,7 +1,6 @@ package frc.robot.extras.util; import edu.wpi.first.math.geometry.Rotation3d; -import frc.robot.BuildConstants; import java.util.Random; public class MathUtil { @@ -9,7 +8,7 @@ public class MathUtil { * random object that generates random variables the seed is the hash of GIT_SHA this way when you * do log-replay even the generated random numbers are the same */ - private static final Random random = new Random(BuildConstants.GIT_SHA.hashCode()); + private static final Random random = new Random((long) Math.random()); public static double linearInterpretationWithBounding( double x1, double y1, double x2, double y2, double x) { diff --git a/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java new file mode 100644 index 0000000..7a47a72 --- /dev/null +++ b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java @@ -0,0 +1,857 @@ +package frc.robot.extras.util; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; +import java.lang.reflect.AnnotatedElement; +import java.lang.reflect.Field; +import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Modifier; +import java.lang.reflect.RecordComponent; +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Iterator; +import java.util.List; +import java.util.Optional; +import java.util.OptionalInt; +import java.util.Set; +import java.util.function.Supplier; +import java.util.stream.BaseStream; + +/** A utility class for procedurally generating {@link Struct}s from records and enums. */ +public final class ProceduralStructGenerator { + private ProceduralStructGenerator() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * A functional interface representing a method that retrives a value from a {@link ByteBuffer}. + */ + @FunctionalInterface + private interface Unpacker { + T unpack(ByteBuffer buffer); + } + + /** A functional interface representing a method that packs a value into a {@link ByteBuffer}. */ + @FunctionalInterface + private interface Packer { + ByteBuffer pack(ByteBuffer buffer, T value); + + static Packer fromStruct(Struct struct) { + return (buffer, value) -> { + struct.pack(buffer, value); + return buffer; + }; + } + } + + private record PrimType(String name, int size, Unpacker unpacker, Packer packer) {} + + /** A map of primitive types to their schema types. */ + private static final HashMap, PrimType> primitiveTypeMap = new HashMap<>(); + + private static void addPrimType( + Class boxedClass, + Class primitiveClass, + String name, + int size, + Unpacker unpacker, + Packer packer) { + PrimType primType = new PrimType<>(name, size, unpacker, packer); + primitiveTypeMap.put(boxedClass, primType); + primitiveTypeMap.put(primitiveClass, primType); + } + + // Add primitive types to the map + static { + addPrimType( + Integer.class, int.class, "int32", Integer.BYTES, ByteBuffer::getInt, ByteBuffer::putInt); + addPrimType( + Double.class, + double.class, + "float64", + Double.BYTES, + ByteBuffer::getDouble, + ByteBuffer::putDouble); + addPrimType( + Float.class, + float.class, + "float32", + Float.BYTES, + ByteBuffer::getFloat, + ByteBuffer::putFloat); + addPrimType( + Boolean.class, + boolean.class, + "bool", + Byte.BYTES, + buffer -> buffer.get() != 0, + (buffer, value) -> buffer.put((byte) (value ? 1 : 0))); + addPrimType( + Character.class, + char.class, + "char", + Character.BYTES, + ByteBuffer::getChar, + ByteBuffer::putChar); + addPrimType(Byte.class, byte.class, "uint8", Byte.BYTES, ByteBuffer::get, ByteBuffer::put); + addPrimType( + Short.class, short.class, "int16", Short.BYTES, ByteBuffer::getShort, ByteBuffer::putShort); + addPrimType( + Long.class, long.class, "int64", Long.BYTES, ByteBuffer::getLong, ByteBuffer::putLong); + } + + /** + * A map of types to their custom struct schemas. + * + *

    This allows adding custom struct implementations for types that are not supported by + * default. Think of vendor-specific. + */ + private static final HashMap, Struct> customStructTypeMap = new HashMap<>(); + + /** + * Add a custom struct to the structifier. + * + * @param The type the struct is for. + * @param clazz The class of the type. + * @param struct The struct to add. + * @param override Whether to override an existing struct. An existing struct could mean the type + * already has a {@code struct} field and implemnts {@link StructSerializable} or that the + * type is already in the custom struct map. + */ + public static void addCustomStruct(Class clazz, Struct struct, boolean override) { + if (override) { + customStructTypeMap.put(clazz, struct); + } else if (!StructSerializable.class.isAssignableFrom(clazz)) { + customStructTypeMap.putIfAbsent(clazz, struct); + } + } + + /** + * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the + * non-contractual nature of the marker this can fail. If the {@code struct} field could not be + * accessed for any reason, an empty {@link Optional} is returned. + * + * @param The type of the class. + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> extractClassStruct( + Class clazz) { + try { + var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct")); + return possibleField.flatMap( + field -> { + field.setAccessible(true); + if (Struct.class.isAssignableFrom(field.getType())) { + try { + return Optional.ofNullable((Struct) field.get(null)); + } catch (IllegalAccessException e) { + return Optional.empty(); + } + } else { + return Optional.empty(); + } + }); + } catch (NoSuchFieldException e) { + return Optional.empty(); + } + } + + /** + * Returns a {@link Struct} for the given class. This does not do compile time checking that the + * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link + * #extractClassStruct(Class)}. + * + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> extractClassStructDynamic(Class clazz) { + if (StructSerializable.class.isAssignableFrom(clazz)) { + return extractClassStruct((Class) clazz).map(struct -> struct); + } else { + return Optional.empty(); + } + } + + @Retention(RetentionPolicy.RUNTIME) + @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) + @Documented + public @interface IgnoreStructField {} + + @Retention(RetentionPolicy.RUNTIME) + @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) + @Documented + public @interface FixedSizeArray { + int size(); + } + + private static OptionalInt arraySize(AnnotatedElement field) { + return Optional.ofNullable(field.getAnnotation(FixedSizeArray.class)) + .map(FixedSizeArray::size) + .map(OptionalInt::of) + .orElse(OptionalInt.empty()); + } + + private static boolean shouldIgnore(AnnotatedElement field) { + return field.isAnnotationPresent(IgnoreStructField.class); + } + + private record StructField( + String name, + String type, + int size, + boolean immutable, + Set> structsToLoad, + Unpacker unpacker, + Packer packer) { + + public static StructField fromField(Field field) { + return StructField.fromNameAndClass( + field.getName(), + field.getType(), + arraySize(field), + Modifier.isFinal(field.getModifiers())); + } + + public static StructField fromRecordComponent(RecordComponent component) { + return StructField.fromNameAndClass( + component.getName(), component.getType(), arraySize(component), true); + } + + @SuppressWarnings("unchecked") + public static StructField fromNameAndClass( + String name, Class clazz, OptionalInt arraySize, boolean isFinal) { + if (!isFixedSize(clazz, arraySize)) { + return null; + } + if (clazz.isArray() && arraySize.isPresent()) { + final Class componentType = clazz.getComponentType(); + final int size = arraySize.getAsInt(); + final StructField componentField = + fromNameAndClass( + componentType.getSimpleName(), componentType, OptionalInt.empty(), false); + return new StructField( + name + "[" + size + "]", + componentField.type, + componentField.size * size, + isFinal, + componentField.structsToLoad, + buffer -> { + Object[] array = new Object[size]; + for (int i = 0; i < size; i++) { + array[i] = componentField.unpacker.unpack(buffer); + } + return array; + }, + (buffer, value) -> { + for (Object obj : (Object[]) value) { + ((Packer) componentField.packer).pack(buffer, obj); + } + return buffer; + }); + } else if (Measure.class.isAssignableFrom(clazz)) { + return new StructField( + name, + "float64", + Double.BYTES, + isFinal, + Set.of(), + buffer -> { + throw new UnsupportedOperationException("Cannot unpack Measure"); + }, + (buffer, value) -> buffer.putDouble(((Measure) value).baseUnitMagnitude())); + } else if (primitiveTypeMap.containsKey(clazz)) { + PrimType primType = primitiveTypeMap.get(clazz); + return new StructField( + name, + primType.name, + primType.size, + isFinal, + Set.of(), + primType.unpacker, + primType.packer); + } else { + Struct struct = null; + if (customStructTypeMap.containsKey(clazz)) { + struct = customStructTypeMap.get(clazz); + } else if (StructSerializable.class.isAssignableFrom(clazz)) { + struct = extractClassStructDynamic(clazz).orElse(null); + } + if (struct == null) { + return null; + } + Set> structsToLoad = new HashSet<>(Set.of(struct.getNested())); + structsToLoad.add(struct); + return new StructField( + name, + struct.getTypeName(), + struct.getSize(), + struct.isImmutable() && isFinal, + structsToLoad, + struct::unpack, + Packer.fromStruct(struct)); + } + } + } + + /** + * Introspects a class to determine if it's a fixed size. + * + *

    Fixed size means no collections, no strings, no arrays, etc. + * + * @param clazz The class to introspect. + * @return Whether the class is fixed size. + */ + public static boolean isFixedSize(Class clazz, OptionalInt arraySize) { + if (clazz.isArray()) { + if (arraySize.isEmpty()) { + return false; + } else { + Class componentType = clazz.getComponentType(); + return isFixedSize(componentType, OptionalInt.empty()); + } + } else if (clazz.isRecord()) { + for (RecordComponent component : clazz.getRecordComponents()) { + if (!isFixedSize(component.getType(), arraySize(component))) { + return false; + } + } + } else { + for (Field field : clazz.getDeclaredFields()) { + Class fieldClass = field.getType(); + if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { + continue; + } + if (Collection.class.isAssignableFrom(fieldClass) + || Iterator.class.isAssignableFrom(fieldClass) + || Iterable.class.isAssignableFrom(fieldClass) + || BaseStream.class.isAssignableFrom(fieldClass) + || fieldClass.isArray() + || fieldClass == String.class + || fieldClass == Optional.class) { + return false; + } + if (!primitiveTypeMap.containsKey(fieldClass) + && !isFixedSize(fieldClass, arraySize(field))) { + return false; + } + } + } + return true; + } + + /** + * Introspects a class to determine if it's interiorly mutable. + * + *

    Interior mutability means that the class has fields that are mutable. + * + * @param clazz The class to introspect. + * @return Whether the class is interiorly mutable. + */ + public static boolean isInteriorlyMutable(Class clazz) { + if (clazz.isArray()) { + return true; + } else if (clazz.isRecord()) { + for (RecordComponent component : clazz.getRecordComponents()) { + if (isInteriorlyMutable(component.getType())) { + return true; + } + } + } else { + for (Field field : clazz.getDeclaredFields()) { + if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { + continue; + } + if (!Modifier.isFinal(field.getModifiers())) { + return true; + } + if (!primitiveTypeMap.containsKey(field.getType()) + && isInteriorlyMutable(field.getType())) { + return true; + } + } + } + return false; + } + + /** A utility for building schema syntax in a procedural manner. */ + @SuppressWarnings("PMD.AvoidStringBufferField") + public static class SchemaBuilder { + /** A utility for building enum fields in a procedural manner. */ + public static final class EnumFieldBuilder { + private final StringBuilder m_builder = new StringBuilder(); + private final String m_fieldName; + private boolean m_firstVariant = true; + + /** + * Creates a new enum field builder. + * + * @param fieldName The name of the field. + */ + public EnumFieldBuilder(String fieldName) { + this.m_fieldName = fieldName; + m_builder.append("enum {"); + } + + /** + * Adds a variant to the enum field. + * + * @param name The name of the variant. + * @param value The value of the variant. + * @return The builder for chaining. + */ + public EnumFieldBuilder addVariant(String name, int value) { + if (!m_firstVariant) { + m_builder.append(','); + } + m_firstVariant = false; + m_builder.append(name).append('=').append(value); + return this; + } + + /** + * Builds the enum field. If this object is being used with {@link SchemaBuilder#addEnumField} + * then {@link #build()} does not have to be called by the user. + * + * @return The built enum field. + */ + public String build() { + m_builder.append("} int8 ").append(m_fieldName).append(';'); + return m_builder.toString(); + } + } + + /** Creates a new schema builder. */ + public SchemaBuilder() {} + + private final StringBuilder m_builder = new StringBuilder(); + + /** + * Adds a field to the schema. + * + * @param name The name of the field. + * @param type The type of the field. + * @return The builder for chaining. + */ + public SchemaBuilder addField(StructField field) { + m_builder.append(field.type).append(' ').append(field.name).append(';'); + return this; + } + + /** + * Adds an inline enum field to the schema. + * + * @param enumFieldBuilder The builder for the enum field. + * @return The builder for chaining. + */ + public SchemaBuilder addEnumField(EnumFieldBuilder enumFieldBuilder) { + m_builder.append(enumFieldBuilder.build()); + return this; + } + + /** + * Builds the schema. + * + * @return The built schema. + */ + public String build() { + return m_builder.toString(); + } + } + + public static Struct noopStruct(Class cls) { + return new Struct<>() { + @Override + public Class getTypeClass() { + return cls; + } + + @Override + public String getTypeName() { + return cls.getSimpleName(); + } + + @Override + public String getSchema() { + return ""; + } + + @Override + public int getSize() { + return 0; + } + + @Override + public void pack(ByteBuffer buffer, T value) {} + + @Override + public T unpack(ByteBuffer buffer) { + return null; + } + }; + } + + private abstract static class ProcStruct implements Struct { + protected final Class typeClass; + protected final List fields; + private final String schema; + + // stored values so we never recompute them + private final int size; + private final boolean isImmutable; + private final Struct[] nested; + + public ProcStruct(Class typeClass, List fields, String schema) { + this.typeClass = typeClass; + this.fields = fields; + this.schema = schema; + + this.size = fields.stream().mapToInt(StructField::size).sum(); + this.isImmutable = fields.stream().allMatch(StructField::immutable); + this.nested = + fields.stream() + .map(StructField::structsToLoad) + .flatMap(Collection::stream) + .toArray(Struct[]::new); + + ProceduralStructGenerator.customStructTypeMap.put(typeClass, this); + } + + @Override + public Class getTypeClass() { + return typeClass; + } + + @Override + public String getTypeName() { + return typeClass.getSimpleName(); + } + + @Override + public String getSchema() { + return schema; + } + + @Override + public int getSize() { + return size; + } + + @Override + public boolean isCloneable() { + return Cloneable.class.isAssignableFrom(typeClass); + } + + @Override + @SuppressWarnings("unchecked") + public T clone(T obj) throws CloneNotSupportedException { + if (isCloneable()) { + try { + return (T) typeClass.getMethod("clone").invoke(obj); + } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) { + throw new CloneNotSupportedException(); + } + } else { + throw new CloneNotSupportedException(); + } + } + + @Override + public boolean isImmutable() { + return isImmutable; + } + + @Override + public Struct[] getNested() { + return nested; + } + + @Override + public String toString() { + return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.schema + "}"; + } + } + + /** + * Generates a {@link Struct} for the given {@link Record} class. If a {@link Struct} cannot be + * generated from the {@link Record}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the record. + * @param recordClass The class of the record. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static Struct genRecord(final Class recordClass) { + final RecordComponent[] components = recordClass.getRecordComponents(); + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final ArrayList fields = new ArrayList<>(); + + for (final RecordComponent component : components) { + if (shouldIgnore(component)) { + continue; + } + component.getAccessor().setAccessible(true); + fields.add(StructField.fromRecordComponent(component)); + } + + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(recordClass); + } + fields.forEach(schemaBuilder::addField); + + return new ProcStruct<>(recordClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, R value) { + boolean failed = false; + int startingPosition = buffer.position(); + for (int i = 0; i < components.length; i++) { + Packer packer = (Packer) fields.get(i).packer(); + try { + Object componentValue = components[i].getAccessor().invoke(value); + if (componentValue == null) { + throw new IllegalArgumentException("Component is null"); + } + packer.pack(buffer, componentValue); + } catch (IllegalAccessException + | IllegalArgumentException + | InvocationTargetException e) { + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + @Override + public R unpack(ByteBuffer buffer) { + try { + Object[] args = new Object[components.length]; + Class[] argTypes = new Class[components.length]; + for (int i = 0; i < components.length; i++) { + args[i] = fields.get(i).unpacker().unpack(buffer); + argTypes[i] = components[i].getType(); + } + return recordClass.getConstructor(argTypes).newInstance(args); + } catch (InstantiationException + | IllegalAccessException + | InvocationTargetException + | NoSuchMethodException + | SecurityException e) { + System.err.println( + "Could not unpack record: " + + recordClass.getSimpleName() + + "\n " + + e.getMessage()); + return null; + } + } + }; + } + + /** + * Generates a {@link Struct} for the given {@link Enum} class. If a {@link Struct} cannot be + * generated from the {@link Enum}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the enum. + * @param enumClass The class of the enum. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static > Struct genEnum(Class enumClass) { + final E[] enumVariants = enumClass.getEnumConstants(); + final Field[] allEnumFields = enumClass.getDeclaredFields(); + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final SchemaBuilder.EnumFieldBuilder enumFieldBuilder = + new SchemaBuilder.EnumFieldBuilder("variant"); + final HashMap enumMap = new HashMap<>(); + final ArrayList fields = new ArrayList<>(); + + if (enumVariants == null || enumVariants.length == 0) { + return noopStruct(enumClass); + } + + for (final E constant : enumVariants) { + final String name = constant.name(); + final int ordinal = constant.ordinal(); + + enumFieldBuilder.addVariant(name, ordinal); + enumMap.put(ordinal, constant); + } + schemaBuilder.addEnumField(enumFieldBuilder); + fields.add( + new StructField( + "variant", + "int8", + 1, + true, + Set.of(), + ByteBuffer::get, + (buffer, value) -> buffer.put((byte) ((Enum) value).ordinal()))); + + final List enumFields = + List.of(allEnumFields).stream() + .filter( + f -> + !f.isEnumConstant() && !Modifier.isStatic(f.getModifiers()) && !shouldIgnore(f)) + .toList(); + + for (final Field field : enumFields) { + field.setAccessible(true); + fields.add(StructField.fromField(field)); + } + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(enumClass); + } + for (int i = 1; i < fields.size(); i++) { + // do this to skip the variant field + schemaBuilder.addField(fields.get(i)); + } + + return new ProcStruct<>(enumClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, E value) { + boolean failed = false; + int startingPosition = buffer.position(); + buffer.put((byte) value.ordinal()); + for (int i = 0; i < enumFields.size(); i++) { + Packer packer = (Packer) fields.get(i + 1).packer(); + Field field = enumFields.get(i); + try { + Object fieldValue = field.get(value); + if (fieldValue == null) { + throw new IllegalArgumentException("Field is null"); + } + packer.pack(buffer, fieldValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not pack enum field: " + + enumClass.getSimpleName() + + "#" + + field.getName() + + "\n " + + e.getMessage()); + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + final byte[] m_spongeBuffer = new byte[this.getSize() - 1]; + + @Override + public E unpack(ByteBuffer buffer) { + int ordinal = buffer.get(); + buffer.get(m_spongeBuffer); + return enumMap.getOrDefault(ordinal, null); + } + + public boolean isCloneable() { + return true; + } + ; + + public E clone(E obj) throws CloneNotSupportedException { + return obj; + } + ; + }; + } + + /** + * Generates a {@link Struct} for the given {@link Object} class. If a {@link Struct} cannot be + * generated from the {@link Object}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the object. + * @param objectClass The class of the object. + * @param objectSupplier A supplier for the object. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static Struct genObject(Class objectClass, Supplier objectSupplier) { + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final Field[] allFields = + List.of(objectClass.getDeclaredFields()).stream() + .filter(f -> !shouldIgnore(f) && !Modifier.isStatic(f.getModifiers())) + .toArray(Field[]::new); + final ArrayList fields = new ArrayList<>(allFields.length); + + for (final Field field : allFields) { + field.setAccessible(true); + fields.add(StructField.fromField(field)); + } + + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(objectClass); + } + fields.forEach(schemaBuilder::addField); + + return new ProcStruct<>(objectClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, O value) { + boolean failed = false; + int startingPosition = buffer.position(); + for (int i = 0; i < allFields.length; i++) { + Packer packer = (Packer) fields.get(i).packer(); + try { + Object fieldValue = allFields[i].get(value); + if (fieldValue == null) { + throw new IllegalArgumentException("Field is null"); + } + packer.pack(buffer, fieldValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not pack object field: " + + objectClass.getSimpleName() + + "#" + + allFields[i].getName() + + "\n " + + e.getMessage()); + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + @Override + public O unpack(ByteBuffer buffer) { + try { + O obj = objectSupplier.get(); + for (int i = 0; i < allFields.length; i++) { + Object fieldValue = fields.get(i).unpacker().unpack(buffer); + allFields[i].set(obj, fieldValue); + } + return obj; + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not unpack object: " + + objectClass.getSimpleName() + + "\n " + + e.getMessage()); + return null; + } + } + }; + } +} diff --git a/src/main/java/frc/robot/extras/util/TimeUtil.java b/src/main/java/frc/robot/extras/util/TimeUtil.java index 754968a..d59cd95 100644 --- a/src/main/java/frc/robot/extras/util/TimeUtil.java +++ b/src/main/java/frc/robot/extras/util/TimeUtil.java @@ -3,28 +3,16 @@ import org.littletonrobotics.junction.Logger; public class TimeUtil { - public static void delay(double seconds) { - try { - // Convert seconds to total milliseconds - long totalMillis = (long) (seconds * 1000); - // Calculate the remaining nanoseconds - int nanoPart = (int) ((seconds * 1000 - totalMillis) * 1000000); - - // Pause the thread for the specified duration in milliseconds and nanoseconds - Thread.sleep(totalMillis, nanoPart); - } catch (InterruptedException e) { - // Restore the interrupted status - Thread.currentThread().interrupt(); - - // Optionally, handle the interruption, e.g. logging or throwing a runtime exception - System.err.println("The sleep was interrupted"); - } - } - + /** + * @return Gets the deterministic timestamp in seconds. + */ public static double getLogTimeSeconds() { return Logger.getTimestamp() / 1_000_000.0; } + /** + * @return Gets the non-deterministic timestamp in seconds. + */ public static double getRealTimeSeconds() { return Logger.getRealTimestamp() / 1_000_000.0; } diff --git a/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java new file mode 100644 index 0000000..04a7d7a --- /dev/null +++ b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java @@ -0,0 +1,99 @@ +package frc.robot.extras.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; + +public class MegatagPoseEstimate implements StructSerializable { + public static class MegatagPoseEstimateStruct implements Struct { + public Pose2d fieldToCamera = Pose2d.kZero; + public double timestampSeconds; + public double latency; + public double avgTagArea; + public double avgTagDist; + public int tagCount; + + @Override + public Class getTypeClass() { + return MegatagPoseEstimate.class; + } + + @Override + public String getTypeString() { + return "struct:MegatagPoseEstimate"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "Pose2d fieldToCamera;double timestampSeconds;double latency;double avgTagArea; int tagCount; double avgTagDist"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct}; + } + + @Override + public MegatagPoseEstimate unpack(ByteBuffer bb) { + MegatagPoseEstimate rv = new MegatagPoseEstimate(); + rv.fieldToCamera = Pose2d.struct.unpack(bb); + rv.timestampSeconds = bb.getDouble(); + rv.latency = bb.getDouble(); + rv.avgTagArea = bb.getDouble(); + rv.fiducialIds = new int[0]; + rv.avgTagDist = bb.getDouble(); + rv.timestampSeconds = bb.getInt(); + return rv; + } + + @Override + public void pack(ByteBuffer bb, MegatagPoseEstimate value) { + Pose2d.struct.pack(bb, value.fieldToCamera); + bb.putDouble(value.timestampSeconds); + bb.putDouble(value.latency); + bb.putDouble(value.avgTagArea); + bb.putDouble(value.avgTagDist); + } + + @Override + public String getTypeName() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getTypeName'"); + } + } + + public Pose2d fieldToCamera = Pose2d.kZero; + public double timestampSeconds; + public double latency; + public double avgTagArea; + public int[] fiducialIds; + public int tagCount; + public double avgTagDist; + + public MegatagPoseEstimate() {} + + public static MegatagPoseEstimate fromLimelight(LimelightHelpers.PoseEstimate poseEstimate) { + MegatagPoseEstimate rv = new MegatagPoseEstimate(); + rv.fieldToCamera = poseEstimate.pose; + if (rv.fieldToCamera == null) rv.fieldToCamera = Pose2d.kZero; + rv.timestampSeconds = poseEstimate.timestampSeconds; + rv.latency = poseEstimate.latency; + rv.avgTagArea = poseEstimate.avgTagArea; + rv.avgTagDist = poseEstimate.avgTagDist; + rv.tagCount = poseEstimate.tagCount; + rv.fiducialIds = new int[poseEstimate.rawFiducials.length]; + for (int i = 0; i < rv.fiducialIds.length; ++i) { + rv.fiducialIds[i] = poseEstimate.rawFiducials[i].id; + } + + return rv; + } + + public static final MegatagPoseEstimateStruct struct = new MegatagPoseEstimateStruct(); +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index d9c4fe1..ed08692 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -1,17 +1,14 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems.swerve; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; -/** Add your docs here. */ +/** Swerve Constants */ public class SwerveConstants { public static final class DriveConstants { @@ -22,9 +19,9 @@ public static final class DriveConstants { // Wheel base and track width are measured by the center of the swerve modules, not the frame of // the robot // Distance between centers of right and left wheels on robot - public static final double TRACK_WIDTH = Units.inchesToMeters(22); + public static final double TRACK_WIDTH = Units.inchesToMeters(21.25); // Distance between front and back wheels on robot - public static final double WHEEL_BASE = Units.inchesToMeters(22); + public static final double WHEEL_BASE = Units.inchesToMeters(21.25); public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] { @@ -37,25 +34,25 @@ public static final class DriveConstants { public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics(MODULE_TRANSLATIONS); - public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 0 - 9; - public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 0 - 9; - public static final int REAR_LEFT_DRIVE_MOTOR_ID = 0 - 9; - public static final int REAR_RIGHT_DRIVE_MOTOR_ID = 0 - 9; + public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 22; + public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 24; + public static final int REAR_LEFT_DRIVE_MOTOR_ID = 23; + public static final int REAR_RIGHT_DRIVE_MOTOR_ID = 21; - public static final int FRONT_LEFT_TURN_MOTOR_ID = 0 - 9; - public static final int FRONT_RIGHT_TURN_MOTOR_ID = 0 - 9; - public static final int REAR_LEFT_TURN_MOTOR_ID = 0 - 9; - public static final int REAR_RIGHT_TURN_MOTOR_ID = 0 - 9; + public static final int FRONT_LEFT_TURN_MOTOR_ID = 5; + public static final int FRONT_RIGHT_TURN_MOTOR_ID = 6; + public static final int REAR_LEFT_TURN_MOTOR_ID = 8; + public static final int REAR_RIGHT_TURN_MOTOR_ID = 7; - public static final int FRONT_LEFT_CANCODER_ID = 0 - 9; - public static final int FRONT_RIGHT_CANCODER_ID = 0 - 9; - public static final int REAR_LEFT_CANCODER_ID = 0 - 9; - public static final int REAR_RIGHT_CANCODER_ID = 0 - 9; + public static final int FRONT_LEFT_CANCODER_ID = 14; + public static final int FRONT_RIGHT_CANCODER_ID = 12; + public static final int REAR_LEFT_CANCODER_ID = 11; + public static final int REAR_RIGHT_CANCODER_ID = 13; - public static final double FRONT_LEFT_ZERO_ANGLE = 0; - public static final double FRONT_RIGHT_ZERO_ANGLE = 0; - public static final double REAR_LEFT_ZERO_ANGLE = 0; - public static final double REAR_RIGHT_ZERO_ANGLE = 0; + public static final double FRONT_LEFT_ZERO_ANGLE = 0.137939453125; + public static final double FRONT_RIGHT_ZERO_ANGLE = -0.420654296875; + public static final double REAR_LEFT_ZERO_ANGLE = -0.475341796875; + public static final double REAR_RIGHT_ZERO_ANGLE = -0.05078125; public static final SensorDirectionValue FRONT_LEFT_CANCODER_REVERSED = SensorDirectionValue.CounterClockwise_Positive; @@ -67,34 +64,36 @@ public static final class DriveConstants { SensorDirectionValue.CounterClockwise_Positive; public static final InvertedValue FRONT_LEFT_TURN_MOTOR_REVERSED = - InvertedValue.Clockwise_Positive; + InvertedValue.CounterClockwise_Positive; public static final InvertedValue FRONT_RIGHT_TURN_MOTOR_REVERSED = - InvertedValue.Clockwise_Positive; + InvertedValue.CounterClockwise_Positive; public static final InvertedValue REAR_LEFT_TURN_MOTOR_REVERSED = InvertedValue.Clockwise_Positive; public static final InvertedValue REAR_RIGHT_TURN_MOTOR_REVERSED = InvertedValue.Clockwise_Positive; public static final InvertedValue FRONT_LEFT_DRIVE_ENCODER_REVERSED = - InvertedValue.Clockwise_Positive; - public static final InvertedValue FRONT_RIGHT_DRIVE_ENCODER_REVERSED = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue FRONT_RIGHT_DRIVE_ENCODER_REVERSED = + InvertedValue.Clockwise_Positive; public static final InvertedValue REAR_LEFT_DRIVE_ENCODER_REVERSED = InvertedValue.Clockwise_Positive; public static final InvertedValue REAR_RIGHT_DRIVE_ENCODER_REVERSED = InvertedValue.CounterClockwise_Positive; - public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 0 - 9; - public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 0 - 9; + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 20; + public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 5; + + public static final double MAX_SPEED_METERS_PER_SECOND = 6.95; + public static final double MAX_SHOOT_SPEED_METERS_PER_SECOND = 3; - public static final double MAX_SPEED_METERS_PER_SECOND = 0 - 9; + public static final double HEADING_ACCEPTABLE_ERROR_RADIANS = Units.degreesToRadians(2.5); + public static final double HEADING_ACCEPTABLE_ERROR_MOVING_RADIANS = Units.degreesToRadians(4); } public class ModuleConstants { - public static final double DRIVE_GEAR_RATIO = 0 - 9; - public static final double TURN_GEAR_RATIO = 0 - 9; - // Use the wheel diamer characterization periodically to find this - public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(4); + public static final double DRIVE_GEAR_RATIO = 4.59; + public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(3.774788522800778); public static final double WHEEL_CIRCUMFERENCE_METERS = WHEEL_DIAMETER_METERS * Math.PI; public static final double DRIVE_TO_METERS = WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; @@ -104,116 +103,71 @@ public class ModuleConstants { public static final double DRIVE_SUPPLY_LIMIT = 45.0; public static final double DRIVE_STATOR_LIMIT = 50.0; - public static final double TURN_P = 0 - 9; - public static final double TURN_I = 0 - 9; - public static final double TURN_D = 0 - 9; + public static final double TURN_P = 116; + public static final double TURN_I = 0.0; + public static final double TURN_D = 0.64; - public static final double TURN_S = 0 - 9; - public static final double TURN_V = 0 - 9; - public static final double TURN_A = 0 - 9; + public static final double TURN_S = 0.0; + public static final double TURN_V = 0.0; + public static final double TURN_A = 0.0; - public static final double MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND = 0 - 9; - public static final double MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED = 0 - 9; + public static final double MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND = 30; + public static final double MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED = 24; - public static final double DRIVE_P = 0 - 9; - public static final double DRIVE_I = 0 - 9; - public static final double DRIVE_D = 0 - 9; - // Use recalc to find the feedfoward values + public static final double DRIVE_P = 0.417; + public static final double DRIVE_I = 0.0; + public static final double DRIVE_D = 0.0; + + public static final double DRIVE_S = 0.16; + // These values were gotten using recalc, then converted to the correct units & were confirmed + // through testing and characterization // https://www.reca.lc/drive?appliedVoltageRamp=%7B%22s%22%3A1200%2C%22u%22%3A%22V%2Fs%22%7D&batteryAmpHours=%7B%22s%22%3A18%2C%22u%22%3A%22A%2Ah%22%7D&batteryResistance=%7B%22s%22%3A0.018%2C%22u%22%3A%22Ohm%22%7D&batteryVoltageAtRest=%7B%22s%22%3A12.6%2C%22u%22%3A%22V%22%7D&efficiency=97&filtering=1&gearRatioMax=%7B%22magnitude%22%3A15%2C%22ratioType%22%3A%22Reduction%22%7D&gearRatioMin=%7B%22magnitude%22%3A3%2C%22ratioType%22%3A%22Reduction%22%7D&maxSimulationTime=%7B%22s%22%3A4%2C%22u%22%3A%22s%22%7D&maxSpeedAccelerationThreshold=%7B%22s%22%3A0.15%2C%22u%22%3A%22ft%2Fs2%22%7D&motor=%7B%22quantity%22%3A4%2C%22name%22%3A%22Kraken%20X60%2A%22%7D&motorCurrentLimit=%7B%22s%22%3A60%2C%22u%22%3A%22A%22%7D&numCyclesPerMatch=24&peakBatteryDischarge=20&ratio=%7B%22magnitude%22%3A4.59%2C%22ratioType%22%3A%22Reduction%22%7D&sprintDistance=%7B%22s%22%3A25%2C%22u%22%3A%22ft%22%7D&swerve=1&targetTimeToGoal=%7B%22s%22%3A2%2C%22u%22%3A%22s%22%7D&throttleResponseMax=0.99&throttleResponseMin=0.5&weightAuxilliary=%7B%22s%22%3A24%2C%22u%22%3A%22lbs%22%7D&weightDistributionFrontBack=0.5&weightDistributionLeftRight=0.5&weightInspected=%7B%22s%22%3A125%2C%22u%22%3A%22lbs%22%7D&wheelBaseLength=%7B%22s%22%3A27%2C%22u%22%3A%22in%22%7D&wheelBaseWidth=%7B%22s%22%3A20%2C%22u%22%3A%22in%22%7D&wheelCOFDynamic=0.9&wheelCOFLateral=1.1&wheelCOFStatic=1.1&wheelDiameter=%7B%22s%22%3A4%2C%22u%22%3A%22in%22%7D - public static final double DRIVE_S = 0 - 9; - public static final double DRIVE_V = 0 - 9; - public static final double DRIVE_A = 0 - 9; + public static final double DRIVE_V = + 1.73 * WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; // = 0.1203 V*s/m + public static final double DRIVE_A = + 0.32 * WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; // = 0.02225 V*s^2/m } public static final class TrajectoryConstants { public static final double DRIVE_BASE_DIAMETER = Math.sqrt(Math.pow(DriveConstants.TRACK_WIDTH, 2) + Math.pow(DriveConstants.WHEEL_BASE, 2)); - public static final double MAX_SPEED = 0 - 9; - public static final double MAX_ACCELERATION = 0 - 9; - public static final double AUTO_TRANSLATION_P = 0 - 9; - public static final double AUTO_TRANSLATION_D = 0 - 9; - public static final double AUTO_THETA_P = 0 - 9; - public static final double AUTO_THETA_D = 0 - 9; - } + public static final double MAX_SPEED = 5.0; + public static final double MAX_ACCELERATION = 3; + + public static final double AUTO_TRANSLATION_P = 1.5; // 1.7 + public static final double AUTO_TRANSLATION_D = 0.2; + public static final double AUTO_THETA_P = 4.5; // 5 + public static final double AUTO_THETA_D = 0.4; + + public static final double AUTO_SHOOT_HEADING_OFFSET = 2; + + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 2; + public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = 2; + + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints THETA_CONTROLLER_CONSTRAINTS = + new TrapezoidProfile.Constraints( + MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED); + + public static final double X_TOLERANCE = 0.02; + public static final double Y_TOLERANCE = 0.02; + public static final double THETA_TOLERANCE = 1.25; + + // Note Detection Driving Constants + public static final double AUTO_ALIGN_TRANSLATIONAL_P = 3; + public static final double AUTO_ALIGN_TRANSLATIONAL_I = 0; + public static final double AUTO_ALIGN_TRANSLATIONAL_D = 0; + + public static Constraints AUTO_ALIGN_TRANSLATION_CONSTRAINTS = new Constraints(5, 2); + + public static final double AUTO_ALIGN_ROTATIONAL_P = 3; + public static final double AUTO_ALIGN_ROTATIONAL_I = 0; + public static final double AUTO_ALIGN_ROTATIONAL_D = 0; - /** - * stores the constants and PID configs for chassis because we want an all-real simulation for the - * chassis, the numbers are required to be considerably precise - */ - public class SimulationConstants { - /** - * numbers that needs to be changed to fit each robot TODO: change these numbers to match your - * robot - */ - public static final double WHEEL_COEFFICIENT_OF_FRICTION = 0.95, - ROBOT_MASS_KG = 40; // with bumpers - - /** TODO: change motor type to match your robot */ - public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60(1), - STEER_MOTOR = DCMotor.getFalcon500(1); - - public static final double WHEEL_RADIUS_METERS = ModuleConstants.WHEEL_DIAMETER_METERS / 2, - DRIVE_GEAR_RATIO = ModuleConstants.DRIVE_GEAR_RATIO, - STEER_GEAR_RATIO = 16.0, - TIME_ROBOT_STOP_ROTATING_SECONDS = 0.06, - STEER_FRICTION_VOLTAGE = 0.12, - DRIVE_FRICTION_VOLTAGE = ModuleConstants.DRIVE_S, - DRIVE_INERTIA = 0.01, - STEER_INERTIA = 0.01; - - /* adjust current limit */ - public static final double DRIVE_CURRENT_LIMIT = ModuleConstants.DRIVE_STATOR_LIMIT; - // public static final double STEER_CURRENT_LIMIT = ModuleConstants.ST; - - /* equations that calculates some constants for the simulator (don't modify) */ - private static final double GRAVITY_CONSTANT = 9.81; - public static final double DRIVE_BASE_RADIUS = DriveConstants.MODULE_TRANSLATIONS[0].getNorm(), - /* friction_force = normal_force * coefficient_of_friction */ - MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION, - MAX_FRICTION_FORCE_PER_MODULE = - MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG / DriveConstants.MODULE_TRANSLATIONS.length, - /* force = torque / distance */ - MAX_PROPELLING_FORCE_NEWTONS = - DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, - /* floor_speed = wheel_angular_velocity * wheel_radius */ - CHASSIS_MAX_VELOCITY = - DRIVE_MOTOR.freeSpeedRadPerSec - / DRIVE_GEAR_RATIO - * WHEEL_RADIUS_METERS, // calculate using choreo - CHASSIS_MAX_ACCELERATION_MPS_SQ = - Math.min( - MAX_FRICTION_ACCELERATION, // cannot exceed max friction - MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG), - CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS, - CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = - CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS, - CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = - CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC / TIME_ROBOT_STOP_ROTATING_SECONDS; - - /* for collision detection in simulation */ - public static final double BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5), - BUMPER_LENGTH_METERS = Units.inchesToMeters(36), - /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */ - BUMPER_COEFFICIENT_OF_FRICTION = 0.75, - /* https://simple.wikipedia.org/wiki/Coefficient_of_restitution */ - BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08; - - /* Gyro Sim */ - public static final double GYRO_ANGULAR_ACCELERATION_THRESHOLD_SKIDDING_RAD_PER_SEC_SQ = 100; - public static final double SKIDDING_AMOUNT_AT_THRESHOLD_RAD = Math.toRadians(1.2); - /* - * https://store.ctr-electronics.com/pigeon-2/ - * for a well-installed one with vibration reduction, only 0.4 degree - * but most teams just install it directly on the rigid chassis frame (including my team :D) - * so at least 1.2 degrees of drifting in 1 minutes for an average angular velocity of 60 degrees/second - * which is the average velocity during normal swerve-circular-offense - * */ - public static final double NORMAL_GYRO_DRIFT_IN_1_MIN_Std_Dev_RAD = Math.toRadians(1.2); - public static final double AVERAGE_VELOCITY_RAD_PER_SEC_DURING_TEST = Math.toRadians(60); - - public static final int SIMULATION_TICKS_IN_1_PERIOD = 5; + public static Constraints AUTO_ALIGN_ROTATIONAL_CONSTRAINTS = + new Constraints(DriveConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 2); } public static final ModuleConfig[] moduleConfigs = diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 740bb8b..c66b0d9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -8,19 +8,22 @@ 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.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.extras.setpointGen.SwerveSetpoint; +import frc.robot.extras.setpointGen.SwerveSetpointGenerator; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; -import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; +import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged; +import frc.robot.subsystems.swerve.gyro.GyroInterface; +import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; import frc.robot.subsystems.vision.VisionConstants; @@ -38,12 +41,25 @@ public class SwerveDrive extends SubsystemBase { private final SwerveModulePosition[] lastModulePositions; private final SwerveDrivePoseEstimator poseEstimator; + private final SwerveSetpointGenerator setpointGenerator = + new SwerveSetpointGenerator( + DriveConstants.MODULE_TRANSLATIONS, + DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO), + DCMotor.getFalcon500(1).withReduction(1), + 60, + 58, + 7, + ModuleConstants.WHEEL_DIAMETER_METERS, + WHEEL_GRIP.TIRE_WHEEL.cof, + 0.0); + private SwerveSetpoint setpoint = SwerveSetpoint.zeroed(); + private final OdometryThread odometryThread; + private Optional alliance; + private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.kError); - private SwerveDriveKinematics kinematics; - private boolean isTest; public SwerveDrive( GyroInterface gyroIO, @@ -55,7 +71,6 @@ public SwerveDrive( this.gyroInputs = new GyroInputsAutoLogged(); this.rawGyroRotation = new Rotation2d(); - setKinematics(DriveConstants.DRIVE_KINEMATICS); swerveModules = new SwerveModule[] { new SwerveModule(frontLeftModuleIO, "FrontLeft"), @@ -71,9 +86,9 @@ public SwerveDrive( new SwerveModulePosition(), new SwerveModulePosition() }; - poseEstimator = + this.poseEstimator = new SwerveDrivePoseEstimator( - getKinematics(), + DriveConstants.DRIVE_KINEMATICS, rawGyroRotation, lastModulePositions, new Pose2d(), @@ -91,30 +106,6 @@ public SwerveDrive( gyroDisconnectedAlert.set(false); } - public SwerveDriveKinematics getKinematics() { - return kinematics; - } - - public void setKinematics(SwerveDriveKinematics newKinematics) { - kinematics = newKinematics; - } - - /** - * Gets the current velocity of the gyro's yaw - * - * @return the yaw velocity - */ - public double getGyroRate() { - return gyroInputs.yawVelocity; - } - - /** Updates the pose estimator with the pose calculated from the swerve modules. */ - public void addPoseEstimatorSwerveMeasurement() { - for (int timestampIndex = 0; - timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); - } - /* * Updates the pose estimator with the pose calculated from the april tags. How much it * contributes to the pose estimation is set by setPoseEstimatorVisionConfidence. @@ -142,6 +133,7 @@ public void setPoseEstimatorVisionConfidence( VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation)); } + @Override public void periodic() { final double t0 = TimeUtil.getRealTimeSeconds(); fetchOdometryInputs(); @@ -171,7 +163,7 @@ public double getCharacterizationVelocity() { } /** Processes odometry inputs */ - void fetchOdometryInputs() { + private void fetchOdometryInputs() { odometryThread.lockOdometry(); odometryThread.updateInputs(odometryThreadInputs); Logger.processInputs("Drive/OdometryThread", odometryThreadInputs); @@ -185,6 +177,7 @@ void fetchOdometryInputs() { odometryThread.unlockOdometry(); } + /** Runs the SwerveModules periodic methods */ private void modulesPeriodic() { for (SwerveModule module : swerveModules) module.periodic(); } @@ -198,28 +191,82 @@ private void modulesPeriodic() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { - SwerveModuleState[] swerveModuleStates = - getKinematics() - .toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotationSpeed, getPose().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); - - setModuleStates(swerveModuleStates); - Logger.recordOutput("SwerveStates/SwerveModuleStates", swerveModuleStates); + ChassisSpeeds desiredSpeeds = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + + setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02); + + setModuleStates(setpoint.moduleStates()); + Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates()); + } + + /** + * Returns the heading of the robot in degrees from 0 to 360. + * + * @return Value is Counter-clockwise positive. + */ + public double getHeading() { + return gyroInputs.yawDegrees; + } + + /** + * Gets the rate of rotation of the NavX. + * + * @return The current rate in degrees per second. + */ + public double getGyroRate() { + return gyroInputs.yawVelocity; + } + + /** Returns a Rotation2d for the heading of the robot. */ + public Rotation2d getGyroRotation2d() { + return Rotation2d.fromDegrees(getHeading()); + } + + /** Returns a Rotation2d for the heading of the robot. */ + public Rotation2d getGyroFieldRelativeRotation2d() { + return Rotation2d.fromDegrees(getHeading() + getAllianceAngleOffset()); } /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */ public double getAllianceAngleOffset() { - Optional alliance = DriverStation.getAlliance(); + alliance = DriverStation.getAlliance(); double offset = alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red ? 180.0 : 0.0; return offset; } + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + gyroIO.reset(); + } + + /** + * Returns the estimated field-relative pose of the robot. Positive x being forward, positive y + * being left. + */ + @AutoLogOutput(key = "Odometry/Odometry") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns a Rotation2d for the heading of the robot */ + public Rotation2d getOdometryRotation2d() { + return getPose().getRotation(); + } + + /** + * Returns a Rotation2d for the heading of the robot relative to the field from the driver's + * perspective. This method is needed so that the drive command and poseEstimator don't fight each + * other. It uses odometry rotation. + */ + public Rotation2d getOdometryAllianceRelativeRotation2d() { + return getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset())); + } + /** * Sets the modules to the specified states. * @@ -228,7 +275,7 @@ public double getAllianceAngleOffset() { */ public void setModuleStates(SwerveModuleState[] desiredStates) { for (int i = 0; i < 4; i++) { - swerveModules[i].runSetpoint(desiredStates[i]); + swerveModules[i].runSetPoint((desiredStates[i])); } } @@ -237,38 +284,27 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ - private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex), + public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex + final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); if (gyroInputs.isConnected) { - rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + rawGyroRotation = getGyroRotation2d(); } else { - Twist2d twist = getKinematics().toTwist2d(moduleDeltas); + Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } poseEstimator.updateWithTime( - odometryThreadInputs.measurementTimeStamps[timestampIndex], - rawGyroRotation, - modulePositions); + // odometryThreadInputs.measurementTimeStamps[timestampIndex], + Logger.getTimestamp(), rawGyroRotation, modulePositions); } /** - * Gets the modules positions, sampled at the indexed timestamp. - * - * @param timestampIndex the timestamp index to sample. - * @return a list of SwerveModulePosition, containing relative drive position and absolute turn - * rotation at the sampled timestamp. + * @param freshModulesPosition Latest module positions + * @return The change of the module positions between the current and last update */ - private SwerveModulePosition[] getModulesPosition(int timestampIndex) { - SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) - swerveModulePositions[moduleIndex] = - swerveModules[moduleIndex].getOdometryPositions()[timestampIndex]; - return swerveModulePositions; - } - private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) { SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { @@ -297,17 +333,6 @@ private SwerveModulePosition[] getModulePositions() { return positions; } - /** Gets the fused pose from the pose estimator. */ - @AutoLogOutput(key = "Odometry/RobotPosition") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** Gets the current gyro yaw */ - public Rotation2d getRawGyroYaw() { - return gyroInputs.yawDegreesRotation2d; - } - /** * Sets the pose * diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 399ab4c..342ea77 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -10,8 +10,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; -import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; +import frc.robot.subsystems.swerve.module.ModuleInputsAutoLogged; +import frc.robot.subsystems.swerve.module.ModuleInterface; import org.littletonrobotics.junction.Logger; public class SwerveModule extends SubsystemBase { @@ -19,8 +19,6 @@ public class SwerveModule extends SubsystemBase { private final String name; private final ModuleInputsAutoLogged inputs = new ModuleInputsAutoLogged(); - SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - private final Alert hardwareFaultAlert; public SwerveModule(ModuleInterface io, String name) { @@ -34,6 +32,7 @@ public SwerveModule(ModuleInterface io, String name) { CommandScheduler.getInstance().unregisterSubsystem(this); } + /** Updates the module's odometry inputs. */ public void updateOdometryInputs() { io.updateInputs(inputs); Logger.processInputs("Drive/Module-" + name, inputs); @@ -43,22 +42,30 @@ public void updateOdometryInputs() { @Override public void periodic() {} + /** Sets the drive voltage of the module. */ public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); io.setTurnVoltage(Volts.zero()); } + /** Gets the drive voltage of the module. */ public double getDriveVoltage() { return inputs.driveAppliedVolts; } + /** Sets the drive velocity of the module. */ public double getCharacterizationVelocity() { return inputs.driveVelocity; } - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public void runSetpoint(SwerveModuleState state) { + /** Runs the module with the specified setpoint state. Returns optimized setpoint */ + public void runSetPoint(SwerveModuleState state) { + state.optimize(getTurnRotation()); + if (state.speedMetersPerSecond < 0.01) { + io.stopModule(); + } io.setDesiredState(state); + Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations()); } /** Returns the current turn angle of the module. */ @@ -72,12 +79,12 @@ public double getTurnVelocity() { /** Returns the current drive position of the module in meters. */ public double getDrivePositionMeters() { - return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.drivePosition; } /** Returns the current drive velocity of the module in meters per second. */ public double getDriveVelocityMetersPerSec() { - return ModuleConstants.DRIVE_TO_METERS_PER_SECOND * inputs.driveVelocity; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.driveVelocity; } /** Returns the module state (turn angle and drive velocity). */ @@ -85,11 +92,6 @@ public SwerveModuleState getMeasuredState() { return new SwerveModuleState(getDriveVelocityMetersPerSec(), getTurnRotation()); } - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - /** * Gets the module position consisting of the distance it has traveled and the angle it is * rotated. @@ -97,8 +99,6 @@ public SwerveModulePosition[] getOdometryPositions() { * @return a SwerveModulePosition object containing position and rotation */ public SwerveModulePosition getPosition() { - double position = ModuleConstants.DRIVE_TO_METERS * getDrivePositionMeters(); - Rotation2d rotation = getTurnRotation(); - return new SwerveModulePosition(position, rotation); + return new SwerveModulePosition(getDrivePositionMeters(), getTurnRotation()); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java similarity index 86% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index b2fa566..2d45260 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; @@ -22,4 +22,7 @@ public static class GyroInputs { * @param inputs inputs to update */ default void updateInputs(GyroInputs inputs) {} + + /** Resets the gyro yaw */ + default void reset() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java similarity index 71% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java index fd107a0..0dfc028 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java @@ -1,28 +1,29 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import static edu.wpi.first.units.Units.*; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import com.studica.frc.AHRS.NavXUpdateRate; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.SPI; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import java.util.Queue; public class PhysicalGyro implements GyroInterface { - private final AHRS gyro = new AHRS(SPI.Port.kMXP, (byte) 250); + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI, NavXUpdateRate.k200Hz); private final Queue yawPositionInput; public PhysicalGyro() { - yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(-gyro.getAngle())); + yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(gyro.getAngle())); } @Override public void updateInputs(GyroInputs inputs) { inputs.isConnected = gyro.isConnected(); inputs.yawDegreesRotation2d = gyro.getRotation2d(); - inputs.yawVelocity = -gyro.getRate(); - inputs.yawDegrees = -gyro.getAngle(); + inputs.yawVelocity = gyro.getRate(); + inputs.yawDegrees = gyro.getAngle(); // Handle odometry yaw positions if (!yawPositionInput.isEmpty()) { @@ -35,4 +36,9 @@ public void updateInputs(GyroInputs inputs) { yawPositionInput.clear(); } } + + @Override + public void reset() { + gyro.reset(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java similarity index 78% rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java rename to src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index cea6b99..4197e45 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java @@ -1,8 +1,9 @@ -package frc.robot.subsystems.swerve.gyroIO; +package frc.robot.subsystems.swerve.gyro; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; @@ -19,8 +20,14 @@ public void updateInputs(GyroInputs inputs) { inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); inputs.odometryYawTimestamps = OdometryTimestampsSim.getTimestamps(); inputs.yawDegreesRotation2d = gyroSimulation.getGyroReading(); + inputs.yawDegrees = gyroSimulation.getGyroReading().getDegrees(); inputs.yawVelocity = RadiansPerSecond.of(gyroSimulation.getMeasuredAngularVelocityRadPerSec()) .in(DegreesPerSecond); } + + @Override + public void reset() { + gyroSimulation.setRotation(Rotation2d.kZero); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java similarity index 87% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java rename to src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java index 25b0bf9..7fdbbcd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -23,7 +23,8 @@ class ModuleInputs { public double[] odometryTimestamps = new double[] {}; - public double turnPosition = 0.0; + public double[] odometryDriveWheelRevolutions = new double[] {}; + public Rotation2d[] odometrySteerPositions = new Rotation2d[] {}; } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java similarity index 88% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java rename to src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java index 48da6ab..f89a9bf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.*; @@ -11,7 +11,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,15 +48,13 @@ public class PhysicalModule implements ModuleInterface { private final BaseStatusSignal[] periodicallyRefreshedSignals; public PhysicalModule(ModuleConfig moduleConfig) { - driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.RIO.name); - turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.RIO.name); - turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.RIO.name); + driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.CANIVORE.name); + turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.CANIVORE.name); + turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.CANIVORE.name); CANcoderConfiguration turnEncoderConfig = new CANcoderConfiguration(); turnEncoderConfig.MagnetSensor.MagnetOffset = -moduleConfig.angleZero(); turnEncoderConfig.MagnetSensor.SensorDirection = moduleConfig.encoderReversed(); - turnEncoderConfig.MagnetSensor.AbsoluteSensorRange = - AbsoluteSensorRangeValue.Signed_PlusMinusHalf; turnEncoder.getConfigurator().apply(turnEncoderConfig, HardwareConstants.TIMEOUT_S); TalonFXConfiguration driveConfig = new TalonFXConfiguration(); @@ -153,13 +150,9 @@ public void updateInputs(ModuleInputs inputs) { turnEncoderAbsolutePosition.clear(); } - inputs.turnPosition = turnMotor.getPosition().getValueAsDouble(); - inputs.driveAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble(); inputs.driveCurrentAmps = driveMotorCurrent.getValueAsDouble(); - inputs.turnPosition = turnMotor.getPosition().getValueAsDouble(); - inputs.turnVelocity = turnEncoderVelocity.getValueAsDouble(); inputs.turnAppliedVolts = turnMotorAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnMotorCurrent.getValueAsDouble(); @@ -177,29 +170,15 @@ public void setTurnVoltage(Voltage volts) { @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRotations = getTurnRotations(); - // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState setpoint = - new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - - setpoint.optimize(Rotation2d.fromRotations(turnRotations)); - setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - - if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { - driveMotor.set(0); - turnMotor.set(0); - return; - } - // Converts meters per second to rotations per second double desiredDriveRPS = - setpoint.speedMetersPerSecond + desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; driveMotor.setControl(velocityRequest.withVelocity(RotationsPerSecond.of(desiredDriveRPS))); turnMotor.setControl( - mmPositionRequest.withPosition(Rotations.of(setpoint.angle.getRotations()))); + mmPositionRequest.withPosition(Rotations.of(desiredState.angle.getRotations()))); } public double getTurnRotations() { diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java similarity index 58% rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java rename to src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index ff474ca..44094b5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -1,14 +1,12 @@ -package frc.robot.subsystems.swerve.moduleIO; +package frc.robot.subsystems.swerve.module; import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; @@ -18,14 +16,20 @@ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(5, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(5, 0, 0); - private final Constraints turnConstraints = new Constraints(5, 0); - private final ProfiledPIDController turnPID = new ProfiledPIDController(5, 0, 0, turnConstraints); - private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(5, 0, 0); + private final PIDController drivePID = new PIDController(.27, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(1, 1.5); + + private final Constraints turnConstraints = + new Constraints( + ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND, + ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED); + private final ProfiledPIDController turnPID = + new ProfiledPIDController(18, 0, 0, turnConstraints); + private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0, 0, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; + turnPID.enableContinuousInput(-Math.PI, Math.PI); } @Override @@ -36,18 +40,19 @@ public void updateInputs(ModuleInputs inputs) { RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) .in(RotationsPerSecond); inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); - inputs.driveCurrentAmps = Math.abs(moduleSimulation.getDriveMotorSupplyCurrentAmps()); + inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps(); - // inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); - inputs.turnPosition = - Radians.of(moduleSimulation.getTurnRelativeEncoderPositionRad()).in(Rotations); + inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); inputs.turnVelocity = - RadiansPerSecond.of(moduleSimulation.getTurnRelativeEncoderSpeedRadPerSec()) - .in(RotationsPerSecond); + Radians.of(moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec()).in(Rotations); inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts(); - inputs.turnCurrentAmps = Math.abs(moduleSimulation.getTurnMotorSupplyCurrentAmps()); + inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps(); + + inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps(); + + inputs.isConnected = true; } @Override @@ -60,48 +65,38 @@ public void setTurnVoltage(Voltage volts) { moduleSimulation.requestTurnVoltageOut(volts); } + @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRotations = getTurnRotations(); - // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState setpoint = - new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - - setpoint.optimize(Rotation2d.fromRotations(turnRotations)); - setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - - if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { - moduleSimulation.requestDriveVoltageOut(0); - moduleSimulation.requestTurnVoltageOut(0); - return; - } - // Converts meters per second to rotations per second double desiredDriveRPS = - setpoint.speedMetersPerSecond + desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; moduleSimulation.requestDriveVoltageOut( Volts.of( drivePID.calculate( - Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()), + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) + .in(RotationsPerSecond) + * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS, desiredDriveRPS)) - .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS)))); + .plus(Volts.of(driveFF.calculate(desiredDriveRPS)))); moduleSimulation.requestTurnVoltageOut( Volts.of( turnPID.calculate( moduleSimulation.getTurnAbsolutePosition().getRotations(), desiredState.angle.getRotations())) - .plus(turnFF.calculate(RotationsPerSecond.of(turnPID.getSetpoint().velocity)))); + .plus(Volts.of(turnFF.calculate(turnPID.getSetpoint().velocity)))); } + @Override public double getTurnRotations() { return moduleSimulation.getTurnAbsolutePosition().getRotations(); } @Override public void stopModule() { - moduleSimulation.requestDriveVoltageOut(Volts.zero()); - moduleSimulation.requestTurnVoltageOut(Volts.zero()); + moduleSimulation.requestDriveVoltageOut(Volts.of(0)); + moduleSimulation.requestTurnVoltageOut(Volts.of(0)); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java index b65ad20..ebc9216 100644 --- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java @@ -3,11 +3,12 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; +import frc.robot.Constants; import frc.robot.Constants.HardwareConstants; +import frc.robot.Constants.SimulationConstants; import frc.robot.Robot; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; -import frc.robot.subsystems.swerve.SwerveConstants.SimulationConstants; import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -47,7 +48,7 @@ static Queue registerInput(Supplier supplier) { } static OdometryThread createInstance(DeviceCANBus canBus) { - return switch (Robot.CURRENT_ROBOT_MODE) { + return switch (Constants.CURRENT_MODE) { case REAL -> new OdometryThreadReal( canBus, diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java index 1b8f17c..879f431 100644 --- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java @@ -55,7 +55,8 @@ private void odometryPeriodic() { private void refreshSignalsAndBlockThread() { switch (canBus) { case RIO -> { - TimeUtil.delay(1.0 / HardwareConstants.SIGNAL_FREQUENCY); + BaseStatusSignal.setUpdateFrequencyForAll( + 1.0 / HardwareConstants.SIGNAL_FREQUENCY, statusSignals); BaseStatusSignal.refreshAll(); } case CANIVORE -> BaseStatusSignal.waitForAll(HardwareConstants.TIMEOUT_S, statusSignals); diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java new file mode 100644 index 0000000..2c07913 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -0,0 +1,415 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants.FieldConstants; +import frc.robot.extras.util.GeomUtil; +import frc.robot.extras.vision.LimelightHelpers; +import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; +import frc.robot.extras.vision.MegatagPoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +// import frc.robot.subsystems.vision.VisionInterface.VisionInputs; +import java.util.concurrent.ConcurrentHashMap; +// import java.util.concurrent.ExecutorService; +// import java.util.concurrent.Executors; +import java.util.concurrent.atomic.AtomicReference; + +public class PhysicalVision implements VisionInterface { + + private Pose2d lastSeenPose = new Pose2d(); + private double headingDegrees = 0; + private double headingRateDegreesPerSecond = 0; + private final ConcurrentHashMap> limelightThreads = + new ConcurrentHashMap<>(); + // private final ExecutorService executorService = Executors.newFixedThreadPool(3); + private final AtomicReference latestInputs = + new AtomicReference<>(new VisionInputs()); + private final ThreadManager threadManager = new ThreadManager(Limelight.values().length); + + /** + * The pose estimates from the limelights in the following order {shooterLimelight, + * frontLeftLimelight, frontRightLimelight} + */ + private MegatagPoseEstimate[] limelightEstimates = + new MegatagPoseEstimate[] { + new MegatagPoseEstimate(), new MegatagPoseEstimate(), new MegatagPoseEstimate() + }; + + public PhysicalVision() { + for (Limelight limelight : Limelight.values()) { + limelightThreads.put(limelight, new AtomicReference<>(latestInputs.get())); + + // Start a vision input task for each Limelight + threadManager.startVisionInputTask( + limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get())); + } + } + + @Override + public void updateInputs(VisionInputs inputs) { + // Combine inputs into the main inputs object + synchronized (inputs) { + for (Limelight limelight : Limelight.values()) { + inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight); + inputs.limelightLatencies[limelight.getId()] = getLatencySeconds(limelight); + inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight); + inputs.limelightMegatagPoses[limelight.getId()] = enabledPoseUpdate(limelight); + inputs.limelightAprilTagDistances[limelight.getId()] = + getLimelightAprilTagDistance(limelight); + inputs.limelightCalculatedPoses[limelight.getId()] = getPoseFromAprilTags(limelight); + inputs.limelightTimestamps[limelight.getId()] = getTimeStampSeconds(limelight); + inputs.limelightLastSeenPose = getLastSeenPose(); + inputs.limelightAprilTagDistances[limelight.getId()] = + getLimelightAprilTagDistance(limelight); + + latestInputs.set(inputs); + limelightThreads.get(limelight).set(latestInputs.get()); + } + } + } + + /** + * Checks if the specified limelight can fully see one or more April Tag. + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return true if the limelight can fully see one or more April Tag + */ + @Override + public boolean canSeeAprilTags(Limelight limelight) { + // First checks if it can see an april tag, then checks if it is fully in frame + // Different Limelights have different FOVs + if (getNumberOfAprilTags(limelight) > VisionConstants.MIN_APRIL_TAG_ID + && getNumberOfAprilTags(limelight) <= VisionConstants.MAX_APRIL_TAG_ID) { + if (limelight.getName().equals(Limelight.SHOOTER.getName())) { + return Math.abs(LimelightHelpers.getTX(limelight.getName())) + <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; + } else { + // return false; + return Math.abs(LimelightHelpers.getTX(limelight.getName())) + <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; + } + } + // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false; + // return LimelightHelpers.getTV(limelight.getName()); + return false; + // latestInputs.get().limelightSeesAprilTags[limelight.getId()] = + } + + /** + * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which + * is then placed into its corresponding spot in the limelightEstimates array. + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + */ + public MegatagPoseEstimate enabledPoseUpdate(Limelight limelight) { + PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight); + PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight); + + if (canSeeAprilTags(limelight) + && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) { + if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate) + && getLimelightAprilTagDistance(limelight) + < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { + LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0); + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight)); + } else { + return limelightEstimates[limelight.getId()] = + MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + } + } + return limelightEstimates[limelight.getId()] = new MegatagPoseEstimate(); + } + + /** + * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link + * #enabledPoseUpdate(int)} + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + */ + public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) { + synchronized (inputs) { + limelightEstimates[limelight.getId()] = + DriverStation.isEnabled() + ? enabledPoseUpdate(limelight) + : MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight)); + } + } + + /** + * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates. + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return true if the discrepancy is larger than the defined threshold, false otherwise + */ + public boolean isLargeDiscrepancyBetweenMegaTag1And2( + Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + return !GeomUtil.isTranslationWithinThreshold( + mt1.pose.getTranslation(), + mt2.pose.getTranslation(), + VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD) + || !GeomUtil.isRotationWithinThreshold( + mt1.pose.getRotation().getDegrees(), + mt2.pose.getRotation().getDegrees(), + VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD); + // return true; + } + + /** + * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it + * sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will + * return 0 for x, y, and theta + */ + public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + } + + /** + * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it + * sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will + * return 0 for x, y, and theta + */ + public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) { + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + } + + /** + * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return true if the pose estimate exists within the field and the pose estimate is not null + */ + public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) { + return LimelightHelpers.isValidPoseEstimate(mt1) + && LimelightHelpers.isValidPoseEstimate(mt2) + && isWithinFieldBounds(mt1, mt2); + } + + /** + * Checks whether the pose estimate for MT1 and MT2 is within the field + * + * @param megaTag1Estimate the MT1 pose estimate to check + * @param megaTag2Estimate the MT2 pose estimate to check + */ + private boolean isWithinFieldBounds( + PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { + return (megaTag1Estimate.pose.getX() > 0 + && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag1Estimate.pose.getY() > 0 + && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getX() > 0 + && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getY() > 0 + && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); + } + + /** + * Gets the pose of the robot calculated by specified limelight via any April Tags it sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for + * x, y, and theta + */ + @Override + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return limelightEstimates[limelight.getId()].fieldToCamera; + } + + // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) { + // return LimelightHelpers.getTargetPose_CameraSpace(limelight.getName()); + // } + + // public Pose2d getAprilTagPositionToRobot(Limelight limelight) { + // return latestInputs.get().limelightRobotToTargetPose[limelight.getId()] + // =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName()); + // } + + /** Returns how many april tags the limelight that is being used for pose estimation can see. */ + @Override + public int getNumberOfAprilTags(Limelight limelight) { + return limelightEstimates[limelight.getId()].tagCount; + // latestInputs.get().limelightMegatagPose[limelight.getId()].fiducialIds.length; + // return limelightEstimates[limelight.getId()].tagCount; + } + + /** + * Returns the timestamp for the vision measurement from the limelight that is being used for pose + * estimation. + */ + @Override + public double getTimeStampSeconds(Limelight limelight) { + return limelightEstimates[limelight.getId()].timestampSeconds / 1000.0; + } + + /** + * Returns the latency in seconds of when the limelight that is being used for pose estimation + * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing + * latency. + */ + @Override + public double getLatencySeconds(Limelight limelight) { + return (limelightEstimates[limelight.getId()].latency) / 1000.0; + } + + /** Gets the pose calculated the last time a limelight saw an April Tag */ + @Override + public Pose2d getLastSeenPose() { + return lastSeenPose; + } + + /** + * Gets the average distance between the specified limelight and the April Tags it sees + * + * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT). + * @return the average distance between the robot and the April Tag(s) in meters + */ + @Override + public double getLimelightAprilTagDistance(Limelight limelight) { + if (canSeeAprilTags(limelight)) { + return limelightEstimates[limelight.getId()].avgTagDist; + } + // To be safe returns a big distance from the april tags if it can't see any + return Double.MAX_VALUE; + } + + /** + * Sets the heading and heading rate of the robot, this is used for deciding between MegaTag 1 and + * 2 for pose estimation. + * + * @param headingDegrees the angle the robot is facing in degrees (0 degrees facing the red + * alliance) + * @param headingRateDegrees the rate the robot is rotating, CCW positive + */ + @Override + public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { + this.headingDegrees = headingDegrees; + this.headingRateDegreesPerSecond = headingRateDegrees; + } + + /** + * Checks if the specified limelight is connected + * + * @param limelight The limelight to check + * @return True if the limelight network table contains the key "tv" + */ + public boolean isLimelightConnected(Limelight limelight) { + NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName()); + return limelightTable.containsKey("tv"); + } + + /** + * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate + * + * @param limelight the limelight number + */ + public void checkAndUpdatePose(Limelight limelight, VisionInputs inputs) { + double last_TX = 0; + double last_TY = 0; + + // Syncronization block to ensure thread safety during the critical section where pose + // information is read and compared. + // This helps prevent race conditions, where one limelight may be updating an object that + // another limelight is reading. + // A race condition could cause unpredictable things to happen. Such as causing a limelight to + // be unable to reference an + // object, as its reference was modified earlier. + synchronized (this) { + try { + double current_TX = LimelightHelpers.getTX(limelight.getName()); + double current_TY = LimelightHelpers.getTY(limelight.getName()); + + // This checks if the limelight reading is new. The reasoning being that if the TX and TY + // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, + // because to get the timestamp of the reading, you need to parse the JSON dump which can be + // very demanding whereas this only has to get the Network Table entries for TX and TY. + if (current_TX != last_TX || current_TY != last_TY) { + + updatePoseEstimate(limelight, inputs); + latestInputs.set(inputs); + + limelightThreads.computeIfPresent(limelight, (key, value) -> latestInputs); + // // Handle threading for Limelight (start or stop threads if needed) + // Check if this Limelight thread exists in limelightThreads + if (limelightThreads.get(limelight) != null) { + // Update thread inputs or restart the thread if needed + limelightThreads.get(limelight).set(latestInputs.get()); + } + + // This is to keep track of the last valid pose calculated by the limelights + // it is used when the driver resets the robot odometry to the limelight calculated + // position + if (canSeeAprilTags(limelight)) { + lastSeenPose = getMegaTag1PoseEstimate(limelight).pose; + } + } else { + // // Retrieve the AtomicReference for the given limelight number + AtomicReference isThreadRunning = + limelightThreads.getOrDefault(limelight, latestInputs); + // Only stop the thread if it's currently running + if (isThreadRunning.get() != null) { + // stop the thread for the specified limelight + stopLimelightThread(limelight); + } + } + last_TX = current_TX; + last_TY = current_TY; + } catch (Exception e) { + System.err.println( + "Error communicating with the: " + limelight.getName() + ": " + e.getMessage()); + } + } + } + + /** + * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. + * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the + * JSON dump for each limelight sequentially within a single scheduler loop. + * + *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage + * the lifecycle of these threads. + * + *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the + * corresponding limelight's thread is marked as "running". This ensures that pose estimates are + * updated in real-time, leveraging the parallel processing capabilities of the executor service. + * + * @param limelight the limelight number + */ + public void visionThreadTask(VisionInputs inputs) { // Limelight limelight + try { + synchronized (inputs) { + for (Limelight limelight : Limelight.values()) { + checkAndUpdatePose(limelight, inputs); + } + } + + } catch (Exception e) { + e.printStackTrace(); + } + } + + /** + * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread + * for the specified limelight. + * + * @param limelight the limelight number + */ + public void stopLimelightThread(Limelight limelight) { + threadManager.stopThread(limelight.getName()); + } + + /** Shuts down all the threads. */ + public void endAllThreads() { + threadManager.shutdownAllThreads(); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java new file mode 100644 index 0000000..3a82e3b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -0,0 +1,213 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.networktables.NetworkTable; +import frc.robot.extras.vision.LimelightHelpers; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; + +// Simulate the vision system. +// Please see the following link for example code +// https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java +public class SimulatedVision extends PhysicalVision { + PhotonCameraSim shooterCameraSim; + PhotonCameraSim frontLeftCameraSim; + PhotonCameraSim frontRightCameraSim; + private final VisionSystemSim visionSim; + private final Supplier robotSimulationPose; + private Pose2d lastSeenPose = new Pose2d(); + + private final int kResWidth = 1280; + private final int kResHeight = 800; + + private int[] tagCount = new int[Limelight.values().length]; + private double[] apriltagDist = new double[Limelight.values().length]; + + public SimulatedVision(Supplier robotSimulationPose) { + super(); + this.robotSimulationPose = robotSimulationPose; + // 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(VisionConstants.FIELD_LAYOUT); + // visionSim.addVisionTargets(TargetModel.kAprilTag36h11); + + // Create simulated camera properties. These can be set to mimic your actual + // camera. + var cameraProperties = new SimCameraProperties(); + cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7)); + cameraProperties.setCalibError(0.35, 0.10); + cameraProperties.setFPS(15); + cameraProperties.setAvgLatencyMs(20); + cameraProperties.setLatencyStdDevMs(5); + + // Create a PhotonCameraSim which will update the linked PhotonCamera's values + // with visible + // targets. + // Instance variables + shooterCameraSim = + new PhotonCameraSim(getSimulationCamera(Limelight.SHOOTER), cameraProperties); + frontLeftCameraSim = + new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties); + frontRightCameraSim = + new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties); + + visionSim.addCamera( + shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM); // check inverse things + visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM); + visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM); + + // Enable the raw and processed streams. (http://localhost:1181 / 1182) + // shooterCameraSim.enableRawStream(true); + // shooterCameraSim.enableProcessedStream(true); + // frontLeftCameraSim.enableRawStream(true); + // frontLeftCameraSim.enableProcessedStream(true); + // frontRightCameraSim.enableRawStream(true); + // frontRightCameraSim.enableProcessedStream(true); + + // // Enable drawing a wireframe visualization of the field to the camera streams. + // // This is extremely resource-intensive and is disabled by default. + // shooterCameraSim.enableDrawWireframe(true); + // frontLeftCameraSim.enableDrawWireframe(true); + // frontRightCameraSim.enableDrawWireframe(true); + } + + @Override + public void updateInputs(VisionInputs inputs) { + // Abuse the updateInputs periodic call to update the sim + + // Move the vision sim robot on the field + if (robotSimulationPose.get() != null) { + visionSim.update(robotSimulationPose.get()); + Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get()); + } + super.updateInputs(inputs); + + for (Limelight limelight : Limelight.values()) { + writeToTable( + getSimulationCamera(limelight).getAllUnreadResults(), + getLimelightTable(limelight), + limelight); + // inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight); + // inputs.limelightAprilTagDistance[limelight.getId()] = + // getLimelightAprilTagDistance(limelight); + } + } + + /** + * Writes photonvision results to the limelight network table + * + * @param results The simulated photonvision pose estimation results + * @param table The Limelight table to write to + * @param limelight The Limelight to write for + */ + private void writeToTable( + List results, NetworkTable table, Limelight limelight) { + // write to ll table + for (PhotonPipelineResult result : results) { + if (result.getMultiTagResult().isPresent()) { + Transform3d best = result.getMultiTagResult().get().estimatedPose.best; + List pose_data = + new ArrayList<>( + Arrays.asList( + best.getX(), // 0: X + best.getY(), // 1: Y + best.getZ(), // 2: Z, + best.getRotation().getX(), // 3: roll + best.getRotation().getY(), // 4: pitch + best.getRotation().getZ(), // 5: yaw + result.metadata.getLatencyMillis(), // 6: latency ms, + (double) + result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count + 0.0, // 8: tag span + 0.0, // 9: tag dist + result.getBestTarget().getArea() // 10: tag area + )); + // Add RawFiducials + // This is super inefficient but it's sim only, who cares. + for (var target : result.targets) { + pose_data.add((double) target.getFiducialId()); // 0: id + pose_data.add(target.getYaw()); // 1: txnc + pose_data.add(target.getPitch()); // 2: tync + pose_data.add(0.0); // 3: ta + pose_data.add(0.0); // 4: distToCamera + pose_data.add(0.0); // 5: distToRobot + pose_data.add(0.5); // 6: ambiguity + } + + table + .getEntry("botpose_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + table + .getEntry("botpose_orb_wpiblue") + .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray()); + + table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + table.getEntry("cl").setDouble(result.metadata.getLatencyMillis()); + lastSeenPose = + new Pose2d( + result.getMultiTagResult().get().estimatedPose.best.getTranslation().getX(), + result.getMultiTagResult().get().estimatedPose.best.getY(), + new Rotation2d( + result.getMultiTagResult().get().estimatedPose.best.getRotation().getAngle())); + } + } + } + + /** + * Gets the PhotonCamera for the given Limelight + * + * @param limelight The Limelight to get the camera for + * @return A PhotonCamera object for the given Limelight + */ + private PhotonCamera getSimulationCamera(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> VisionConstants.SHOOTER_CAMERA; + case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA; + case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_CAMERA; + default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight); + }; + } + + /** + * Gets the Limelight network table + * + * @param limelight The Limelight to get the table for + * @return The network table of the Limelight + */ + private NetworkTable getLimelightTable(Limelight limelight) { + return switch (limelight) { + case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); + case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); + case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName()); + default -> throw new IllegalArgumentException("Invalid limelight " + limelight); + }; + } + + // @Override + // public boolean canSeeAprilTags(Limelight limelight) { + // table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0); + + // } + @Override + public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { + super.setHeadingInfo(headingDegrees, headingRateDegrees); + } + + @Override + public Pose2d getLastSeenPose() { + return lastSeenPose; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java new file mode 100644 index 0000000..f0113e8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.vision; + +import frc.robot.subsystems.vision.VisionInterface.VisionInputs; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; + +public class ThreadManager { + private final ExecutorService executorService; + private final Map> threadFutures = new ConcurrentHashMap<>(); + + public ThreadManager(int maxThreads) { + executorService = Executors.newFixedThreadPool(maxThreads); + } + + // Start a thread for a specific task + public void startThread(String threadName, Runnable task) { + if (threadFutures.containsKey(threadName)) { + System.err.println("Thread " + threadName + " is already running."); + return; + } + + Future future = + executorService.submit( + () -> { + try { + task.run(); + } catch (Exception e) { + System.err.println("Error in thread " + threadName + ": " + e.getMessage()); + } + }); + + threadFutures.put(threadName, future); + } + + // Stop a specific thread + public void stopThread(String threadName) { + Future future = threadFutures.get(threadName); + if (future != null) { + future.cancel(true); + threadFutures.remove(threadName); + } else { + System.err.println("Thread " + threadName + " is not running."); + } + } + + // Submit a vision input task + public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) { + startThread( + threadName, + () -> { + try { + while (!Thread.currentThread().isInterrupted()) { + visionTask.run(); + Thread.sleep(VisionConstants.THREAD_SLEEP_MS); // Sleep to avoid excessive updates + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + }); + } + + // Shut down all threads + public void shutdownAllThreads() { + threadFutures.values().forEach(future -> future.cancel(true)); + threadFutures.clear(); + executorService.shutdown(); + try { + if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { + executorService.shutdownNow(); + } + } catch (InterruptedException e) { + executorService.shutdownNow(); + Thread.currentThread().interrupt(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index 72a4df2..0000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Vision extends SubsystemBase { - private final VisionIO visionIO; - private final VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); - - public Vision(VisionIO visionIO) { - // Initializing Fields - this.visionIO = visionIO; - } - - @Override - public void periodic() { - // Updates limelight inputs - visionIO.updateInputs(inputs); - Logger.processInputs(visionIO.getLimelightName(0), inputs); - } - - // Add methods to support DriveCommand - public int getNumberOfAprilTags(int limelightNumber) { - return visionIO.getNumberOfAprilTags(limelightNumber); - } - - public double getLimelightAprilTagDistance(int limelightNumber) { - return visionIO.getLimelightAprilTagDistance(limelightNumber); - } - - public double getTimeStampSeconds(int limelightNumber) { - return visionIO.getTimeStampSeconds(limelightNumber); - } - - public double getLatencySeconds(int limelightNumber) { - return visionIO.getLatencySeconds(limelightNumber); - } - - public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { - visionIO.setHeadingInfo(headingDegrees, headingRateDegrees); - } - - @AutoLogOutput(key = "Vision/Has Targets") - public boolean canSeeAprilTags(int limelightNumber) { - return visionIO.canSeeAprilTags( - limelightNumber); // Assuming we're checking the shooter limelight - } - - public Pose2d getPoseFromAprilTags(int limelightNumber) { - return visionIO.getPoseFromAprilTags(limelightNumber); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index ac54ce9..b4ceef6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,9 +1,66 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import org.photonvision.PhotonCamera; public final class VisionConstants { + public enum Limelight { + SHOOTER(0, VisionConstants.SHOOTER_LIMELIGHT_NAME), + FRONT_LEFT(1, VisionConstants.FRONT_LEFT_LIMELIGHT_NAME), + FRONT_RIGHT(2, VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME); + private final int id; + private final String name; + + Limelight(int id, String name) { + this.id = id; + this.name = name; + } + + public int getId() { + return id; + } + + public String getName() { + return name; + } + + public static Limelight fromId(int id) { + return switch (id) { + case 0 -> SHOOTER; + case 1 -> FRONT_LEFT; + case 2 -> FRONT_RIGHT; + default -> throw new IllegalArgumentException("Invalid Limelight ID: " + id); + }; + } + } + + public static final Transform3d SHOOTER_TRANSFORM = + new Transform3d( + new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, 35, 180.0)); + public static final Transform3d FRONT_LEFT_TRANSFORM = + new Transform3d( + new Translation3d(0.2749477356, -0.269958439, 0.2318054546), + new Rotation3d(0.0, 25, -35)); + public static final Transform3d FRONT_RIGHT_TRANSFORM = + new Transform3d( + new Translation3d(0.2816630892, 0.2724405524, 0.232156), new Rotation3d(0.0, 25, 35)); + + public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName()); + public static final PhotonCamera FRONT_LEFT_CAMERA = + new PhotonCamera(Limelight.FRONT_LEFT.getName()); + public static final PhotonCamera FRONT_RIGHT_CAMERA = + new PhotonCamera(Limelight.FRONT_RIGHT.getName()); + + public static final int THREAD_SLEEP_MS = 20; + + public static final AprilTagFieldLayout FIELD_LAYOUT = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); public static final double VISION_X_POS_TRUST = 0.5; // meters public static final double VISION_Y_POS_TRUST = 0.5; // meters public static final double VISION_ANGLE_TRUST = Units.degreesToRadians(50); // radians @@ -17,6 +74,9 @@ public final class VisionConstants { public static final double MEGA_TAG_2_DISTANCE_THRESHOLD = 5; // TODO: Tune + public static final double MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD = 0.5; // TODO: tune + public static final double MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD = 45; + public static final String SHOOTER_LIMELIGHT_NAME = "limelight-shooter"; public static final int SHOOTER_LIMELIGHT_NUMBER = 0; public static final String FRONT_LEFT_LIMELIGHT_NAME = "limelight-left"; @@ -24,53 +84,8 @@ public final class VisionConstants { public static final String FRONT_RIGHT_LIMELIGHT_NAME = "limelight-right"; public static final int FRONT_RIGHT_LIMELIGHT_NUMBER = 2; - public static final double[][] APRIL_TAG_POSITIONS = { - // {x, y, z, rotation (degrees)} - { - Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38), 120 - }, // 1 - { - Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38), 120 - }, // 2 - { - Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13), 180 - }, // 3 - { - Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13), 180 - }, // 4 - { - Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38), 270 - }, // 5 - { - Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38), 270 - }, // 6 - {-Units.inchesToMeters(1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13), 0}, // 7 - {-Units.inchesToMeters(1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13), 0}, // 8 - { - Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38), 60 - }, // 9 - { - Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38), 60 - }, // 10 - { - Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0), 300 - }, // 11 - { - Units.inchesToMeters(468.69), Units.inchesToMeters(177.1), Units.inchesToMeters(52.0), 60 - }, // 12 - { - Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0), 180 - }, // 13 - { - Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0), 0 - }, // 14 - { - Units.inchesToMeters(182.73), Units.inchesToMeters(177.1), Units.inchesToMeters(52.0), 120 - }, // 15 - { - Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0), 240 - }, // 16 - }; + public static final int MIN_APRIL_TAG_ID = 1; + public static final int MAX_APRIL_TAG_ID = 16; public static final double[][] ONE_APRIL_TAG_LOOKUP_TABLE = { // {distance in meters, x std deviation, y std deviation, r (in degrees) std deviation} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java deleted file mode 100644 index ad9c125..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import org.littletonrobotics.junction.AutoLog; - -public interface VisionIO { - @AutoLog - class VisionIOInputs { - public boolean cameraConnected = false; - public double latency = 0.0; - public double fiducialMarksID = 0.0; - - public int camerasAmount = 0; - public int targetsCount = 0; - } - - void updateInputs(VisionIOInputs inputs); - - String getLimelightName(int limelightNumber); - - double getLatencySeconds(int limelightNumber); - - double getTimeStampSeconds(int limelightNumber); - - boolean canSeeAprilTags(int limelightNumber); - - double getLimelightAprilTagDistance(int limelightNumber); - - int getNumberOfAprilTags(int limelightNumber); - - Pose2d getPoseFromAprilTags(int limelightNumber); - - void setHeadingInfo(double headingDegrees, double headingRateDegrees); -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java deleted file mode 100644 index aa4405d..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ /dev/null @@ -1,439 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.FieldConstants; -import frc.robot.extras.vision.LimelightHelpers; -import frc.robot.extras.vision.LimelightHelpers.PoseEstimate; -import java.util.Map; -import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.ExecutorService; -import java.util.concurrent.Executors; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicBoolean; - -public class VisionIOReal implements VisionIO { - - private Pose2d lastSeenPose = new Pose2d(); - private double headingDegrees = 0; - private double headingRateDegreesPerSecond = 0; - private final Map limelightThreads = new ConcurrentHashMap<>(); - private final ExecutorService executorService = Executors.newFixedThreadPool(3); - - /** - * The pose estimates from the limelights in the following order {shooterLimelight, - * frontLeftLimelight, frontRightLimelight} - */ - private PoseEstimate[] limelightEstimates; - - public VisionIOReal() { - limelightEstimates = new PoseEstimate[3]; - for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - limelightThreads.put(limelightNumber, new AtomicBoolean(true)); - limelightEstimates[limelightNumber] = new PoseEstimate(); - } - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - inputs.camerasAmount = limelightEstimates.length; - inputs.cameraConnected = true; - - for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { - // Update camera connection status - inputs.cameraConnected = true; - - // Add number of April tags seen by this limelight - inputs.targetsCount += getNumberOfAprilTags(limelightNumber); - - // Add fiducial mark ID - inputs.fiducialMarksID = LimelightHelpers.getFiducialID(getLimelightName(limelightNumber)); - - // Add latency for this limelight - inputs.latency += getLatencySeconds(limelightNumber) / 1000.0; - } - - // Calculate average latency - inputs.latency /= limelightEstimates.length; - - periodic(); - } - - /** - * Checks if the specified limelight can fully see one or more April Tag. - * - * @param limelightNumber the number of the limelight - * @return true if the limelight can fully see one or more April Tag - */ - @Override - public boolean canSeeAprilTags(int limelightNumber) { - // First checks if it can see an april tag, then checks if it is fully in frame - // Different Limelights have different FOVs - if (getNumberOfAprilTags(limelightNumber) > 0 - && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.length) { - if (getLimelightName(limelightNumber).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber))) - <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR; - } else { - return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber))) - <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR; - } - } - return false; - } - - /** - * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which - * is then placed into its corresponding spot in the limelightEstimates array. - * - * @param limelightNumber the number of the limelight - */ - public PoseEstimate enabledPoseUpdate(int limelightNumber) { - if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) { - if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber) - && getLimelightAprilTagDistance(limelightNumber) - < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); - } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { - LimelightHelpers.SetRobotOrientation( - getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0); - return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber); - } else { - return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); - } - } - return limelightEstimates[limelightNumber] = new PoseEstimate(); - } - - /** - * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link - * #enabledPoseUpdate(int)} - * - * @param limelightNumber the number of the limelight - */ - public void updatePoseEstimate(int limelightNumber) { - limelightEstimates[limelightNumber] = - DriverStation.isEnabled() - ? enabledPoseUpdate(limelightNumber) - : getMegaTag1PoseEstimate(limelightNumber); - } - - /** - * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates. - * - * @param limelightNumber the number of the limelight - * @return true if the discrepancy is larger than the defined threshold, false otherwise - */ - public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); - - // Extract the positions of the two poses - Translation2d megaTag1TranslationMeters = megaTag1Estimate.pose.getTranslation(); - Translation2d megaTag2TranslationMeters = megaTag2Estimate.pose.getTranslation(); - - double megaTag1RotationDegrees = megaTag1Estimate.pose.getRotation().getDegrees(); - double megaTag2RotationDegrees = megaTag2Estimate.pose.getRotation().getDegrees(); - - // Calculate the discrepancy between the two MegaTag translations in meters - double megaTagTranslationDiscrepancyMeters = - megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters); - double megaTagRotationDiscrepancyDegrees = - Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees); - - // Define a threshold (meters) for what constitutes a "large" discrepancy - // This value should be determined based on your testing - double thresholdMeters = 0.5; - double thresholdDegrees = 45; - - // Check if the discrepancy is larger than the threshold (meters) - return megaTagTranslationDiscrepancyMeters > thresholdMeters - || megaTagRotationDiscrepancyDegrees > thresholdDegrees; - } - - /** - * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it - * sees - * - * @param limelightNumber the number of the limelight - * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will - * return 0 for x, y, and theta - */ - public PoseEstimate getMegaTag1PoseEstimate(int limelightNumber) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue(getLimelightName(limelightNumber)); - } - - /** - * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it - * sees - * - * @param limelightNumber the number of the limelight - * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will - * return 0 for x, y, and theta - */ - public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) { - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelightNumber)); - } - - /** - * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field - * - * @param limelightNumber the number of the limelight - * @return true if the pose estimate exists within the field and the pose estimate is not null - */ - public boolean isValidPoseEstimate(int limelightNumber) { - PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); - - return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate) - && LimelightHelpers.isValidPoseEstimate(megaTag2Estimate) - && isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate); - } - - /** - * Checks whether the pose estimate for MT1 and MT2 is within the field - * - * @param megaTag1Estimate the MT1 pose estimate to check - * @param megaTag2Estimate the MT2 pose estimate to check - */ - private boolean isWithinFieldBounds( - PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { - return (megaTag1Estimate.pose.getX() > 0 - && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag1Estimate.pose.getY() > 0 - && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.pose.getX() > 0 - && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag2Estimate.pose.getY() > 0 - && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); - } - - /** - * Gets the pose of the robot calculated by specified limelight via any April Tags it sees - * - * @param limelightNumber the number of the limelight - * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for - * x, y, and theta - */ - @Override - public Pose2d getPoseFromAprilTags(int limelightNumber) { - return limelightEstimates[limelightNumber].pose; - } - - /** Returns how many april tags the limelight that is being used for pose estimation can see. */ - @Override - public int getNumberOfAprilTags(int limelightNumber) { - return limelightEstimates[limelightNumber].tagCount; - } - - /** - * Returns the timestamp for the vision measurement from the limelight that is being used for pose - * estimation. - */ - @Override - public double getTimeStampSeconds(int limelightNumber) { - return limelightEstimates[limelightNumber].timestampSeconds / 1000.0; - } - - /** - * Returns the latency in seconds of when the limelight that is being used for pose estimation - * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing - * latency. - */ - @Override - public double getLatencySeconds(int limelightNumber) { - return (limelightEstimates[limelightNumber].latency) / 1000.0; - } - - /** Gets the pose calculated the last time a limelight saw an April Tag */ - public Pose2d getLastSeenPose() { - return lastSeenPose; - } - - /** - * Gets the average distance between the specified limelight and the April Tags it sees - * - * @param limelightNumber the number of the limelight - * @return the average distance between the robot and the April Tag(s) in meters - */ - public double getLimelightAprilTagDistance(int limelightNumber) { - if (canSeeAprilTags(limelightNumber)) { - return limelightEstimates[limelightNumber].avgTagDist; - } - // To be safe returns a big distance from the april tags if it can't see any - return Double.MAX_VALUE; - } - - /** - * Sets the heading and heading rate of the robot, this is used for deciding between MegaTag 1 and - * 2 for pose estimation. - * - * @param headingDegrees the angle the robot is facing in degrees (0 degrees facing the red - * alliance) - * @param headingRateDegrees the rate the robot is rotating, CCW positive - */ - public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { - this.headingDegrees = headingDegrees; - this.headingRateDegreesPerSecond = headingRateDegrees; - } - - /** - * Gets the limelight name associated with the specified limelight number/index - * - * @param limelightNumber the limelight number - * @return 0 = limelight-shooter, 1 = limelight-left, 2 = limelight-right - */ - public String getLimelightName(int limelightNumber) { - return switch (limelightNumber) { - case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME; - case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; - case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; - default -> - throw new IllegalArgumentException("You entered a number for a non-existent limelight"); - }; - } - - /** - * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate - * - * @param limelightNumber the limelight number - */ - public void checkAndUpdatePose(int limelightNumber) { - double last_TX = 0; - double last_TY = 0; - - // Syncronization block to ensure thread safety during the critical section where pose - // information is read and compared. - // This helps prevent race conditions, where one limelight may be updating an object that - // another limelight is reading. - // A race condition could cause unpredictable things to happen. Such as causing a limelight to - // be unable to reference an - // object, as its reference was modified earlier. - synchronized (this) { - try { - double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber)); - double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber)); - - // This checks if the limelight reading is new. The reasoning being that if the TX and TY - // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, - // because to get the timestamp of the reading, you need to parse the JSON dump which can be - // very demanding whereas this only has to get the Network Table entries for TX and TY. - if (current_TX != last_TX || current_TY != last_TY) { - updatePoseEstimate(limelightNumber); - limelightThreads.computeIfPresent( - limelightNumber, (key, value) -> new AtomicBoolean(true)); - // This is to keep track of the last valid pose calculated by the limelights - // it is used when the driver resets the robot odometry to the limelight calculated - // position - if (canSeeAprilTags(limelightNumber)) { - lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose; - } - } else { - // Retrieve the AtomicBoolean for the given limelight number - AtomicBoolean isThreadRunning = - limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean()); - // Only stop the thread if it's currently running - if (isThreadRunning.get()) { - // stop the thread for the specified limelight - stopThread(limelightNumber); - } - } - last_TX = current_TX; - last_TY = current_TY; - } catch (Exception e) { - System.err.println( - "Error communicating with the: " - + getLimelightName(limelightNumber) - + ": " - + e.getMessage()); - } - } - } - - /** - * Starts a separate thread dedicated to updating the pose estimate for a specified limelight. - * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the - * JSON dump for each limelight sequentially within a single scheduler loop. - * - *

    To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage - * the lifecycle of these threads. - * - *

    Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the - * corresponding limelight's thread is marked as "running". This ensures that pose estimates are - * updated in real-time, leveraging the parallel processing capabilities of the executor service. - * - * @param limelightNumber the limelight number - */ - public void visionThread(int limelightNumber) { - - executorService.submit( - () -> { - try { - // while (limelightThreads.get(limelightNumber).get()) { - checkAndUpdatePose(limelightNumber); - // } - } catch (Exception e) { - System.err.println( - "Error executing task for the: " - + getLimelightName(limelightNumber) - + ": " - + e.getMessage()); - } - }); - } - - /** - * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread - * for the specified limelight. - * - * @param limelightNumber the limelight number - */ - public void stopThread(int limelightNumber) { - try { - // Since we can't see an April Tag, set the estimate for the specified limelight to an empty - // PoseEstimate() - limelightEstimates[limelightNumber] = new PoseEstimate(); - limelightThreads.get(limelightNumber).set(false); - } catch (Exception e) { - System.err.println( - "Error stopping thread for the: " - + getLimelightName(limelightNumber) - + ": " - + e.getMessage()); - } - } - - /** Shuts down all the threads. */ - public void endAllThreads() { - // Properly shut down the executor service when the subsystem ends - executorService.shutdown(); // Prevents new tasks from being submitted - try { - // Wait for existing tasks to finish - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) { - executorService.shutdownNow(); - // Wait a bit longer for tasks to respond to being cancelled - if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) - System.err.println("ExecutorService did not terminate"); - } - } catch (InterruptedException e) { - // (Re-)Cancel if current thread also interrupted - executorService.shutdownNow(); - // Preserve interrupt status - Thread.currentThread().interrupt(); - } - } - - // Override periodic method to start the vision threads at the beginning of each subsystem tick - public void periodic() { - visionThread(VisionConstants.SHOOTER_LIMELIGHT_NUMBER); - visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); - visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); - SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0)); - SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString()); - SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString()); - - SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString()); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java deleted file mode 100644 index 343d4b4..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ /dev/null @@ -1,44 +0,0 @@ -// package frc.robot.subsystems.vision; - -// import edu.wpi.first.apriltag.AprilTagFieldLayout; -// import edu.wpi.first.math.geometry.Pose2d; -// import org.photonvision.simulation.PhotonCameraSim; -// import org.photonvision.simulation.VisionSystemSim; - -// import java.util.List; -// import java.util.function.Supplier; - -// public class VisionIOSim extends VisionIOPhoton { -// private final VisionSystemSim visionSystemSim; -// private final PhotonCameraSim[] camerasSim; -// private final Supplier robotActualPoseInSimulationSupplier; -// public VisionIOSim(List cameraProperties, AprilTagFieldLayout -// aprilTagFieldLayout, Supplier robotActualPoseInSimulationSupplier) { -// super(cameraProperties); - -// this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier; -// this.visionSystemSim = new VisionSystemSim("main"); -// visionSystemSim.addAprilTags(aprilTagFieldLayout); -// camerasSim = new PhotonCameraSim[cameraProperties.size()]; - -// for (int i = 0; i < cameraProperties.size(); i++) { -// final PhotonCameraSim cameraSim = new PhotonCameraSim( -// super.cameras[i], -// cameraProperties.get(i).getSimulationProperties() -// ); -// cameraSim.enableRawStream(true); -// cameraSim.enableProcessedStream(true); -// cameraSim.enableDrawWireframe(true); -// visionSystemSim.addCamera( -// camerasSim[i] = cameraSim, -// cameraProperties.get(i).robotToCamera -// ); -// } -// } - -// @Override -// public void updateInputs(VisionIOInputs inputs) { -// visionSystemSim.update(robotActualPoseInSimulationSupplier.get()); -// super.updateInputs(inputs); -// } -// } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java new file mode 100644 index 0000000..08c77cb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -0,0 +1,95 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.extras.vision.MegatagPoseEstimate; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionInterface { + @AutoLog + class VisionInputs { + public boolean[] isLimelightConnected = new boolean[Limelight.values().length]; + + public MegatagPoseEstimate[] limelightMegatagPoses = + new MegatagPoseEstimate[Limelight.values().length]; + public double[] limelightLatencies = new double[Limelight.values().length]; + public int[] limelightTargets = new int[Limelight.values().length]; + public boolean[] limelightSeesAprilTags = new boolean[Limelight.values().length]; + + public Pose2d[] limelightCalculatedPoses = new Pose2d[Limelight.values().length]; + public Pose2d limelightLastSeenPose = new Pose2d(); + public double[] limelightAprilTagDistances = new double[Limelight.values().length]; + + public double[] limelightTimestamps = new double[Limelight.values().length]; + } + + /** + * Updates the inputs for the vision subsystem using VisionInputs + * + * @param inputs the inputs to update + */ + default void updateInputs(VisionInputs inputs) {} + + /** + * @param limelight The Limelight to retrieve the latency data from + * @return The current latency of the Limelight + */ + default double getLatencySeconds(Limelight limelight) { + return 0.0; + } + + /** + * @param limelight The Limelight to retrieve the timestamp data from + * @return The current timestamp of the Limelight + */ + default double getTimeStampSeconds(Limelight limelight) { + return 0.0; + } + + /** + * @param limelight The Limelight to retrieve the target data from + * @return Whether the Limelight can see any targets, and if it is within its field of view + */ + default boolean canSeeAprilTags(Limelight limelight) { + return false; + } + + /** + * @param limelight The Limelight to retrieve the distance data from + * @return The current April Tag distance from the Limelight + */ + default double getLimelightAprilTagDistance(Limelight limelight) { + return 0.0; + } + + /** + * @param limelight The Limelight to retrieve the data from + * @return The current number of April Tags of the Limelight + */ + default int getNumberOfAprilTags(Limelight limelight) { + return 0; + } + + /** + * @param limelight The Limelight to retrieve the pose data from + * @return The current pose of the Limelight + */ + default Pose2d getPoseFromAprilTags(Limelight limelight) { + return null; + } + + /** + * Sets the heading information of the robot, used with MT2 + * + * @param headingDegrees The heading of the robot in degrees + * @param headingRateDegrees The rate of change of the heading of the robot in degrees + */ + default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} + + /** + * @return The last seen pose of any Limelight that most recently saw a target + */ + default Pose2d getLastSeenPose() { + return null; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java new file mode 100644 index 0000000..28b95d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,60 @@ +// All praise 254 lib + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionConstants.Limelight; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class VisionSubsystem extends SubsystemBase { + private final VisionInterface visionInterface; + private final VisionInputsAutoLogged inputs = new VisionInputsAutoLogged(); + + public VisionSubsystem(VisionInterface visionInterface) { + // Initializing Fields + this.visionInterface = visionInterface; + } + + @Override + public void periodic() { + // Updates limelight inputs + visionInterface.updateInputs(inputs); + Logger.processInputs("Vision/", inputs); + } + + // Add methods to support DriveCommand + public int getNumberOfAprilTags(Limelight limelight) { + return inputs.limelightTargets[limelight.getId()]; + } + + public double getLimelightAprilTagDistance(Limelight limelight) { + return inputs.limelightAprilTagDistances[limelight.getId()]; + } + + public double getTimeStampSeconds(Limelight limelight) { + return inputs.limelightTimestamps[limelight.getId()]; + } + + public double getLatencySeconds(Limelight limelight) { + return inputs.limelightLatencies[limelight.getId()]; + } + + public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { + visionInterface.setHeadingInfo(headingDegrees, headingRateDegrees); + } + + @AutoLogOutput(key = "Vision/Has Targets") + public boolean canSeeAprilTags(Limelight limelight) { + return inputs.limelightSeesAprilTags[limelight.getId()]; + } + + public Pose2d getPoseFromAprilTags(Limelight limelight) { + return inputs.limelightCalculatedPoses[limelight.getId()]; + } + + public Pose2d getLastSeenPose() { + return visionInterface.getLastSeenPose(); + } +} diff --git a/src/test/java/SwerveDriveTest.java b/src/test/java/SwerveDriveTest.java deleted file mode 100644 index 718fa7f..0000000 --- a/src/test/java/SwerveDriveTest.java +++ /dev/null @@ -1,83 +0,0 @@ -import static org.junit.jupiter.api.Assertions.*; -import static org.mockito.ArgumentMatchers.any; -import static org.mockito.ArgumentMatchers.eq; -import static org.mockito.Mockito.*; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; -import frc.robot.subsystems.swerve.odometryThread.OdometryThread; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import org.mockito.Mock; -import org.mockito.MockitoAnnotations; - -class SwerveDriveTest { - - @Mock private GyroInterface gyroIO; - - @Mock - private ModuleInterface frontLeftModuleIO, - frontRightModuleIO, - backLeftModuleIO, - backRightModuleIO; - @Mock private SwerveModule frontLeftModule, frontRightModule, backLeftModule, backRightModule; - - @Mock private OdometryThread mockOdometryThread; - - @Mock private SwerveDriveKinematics swerveDriveKinematics; - - private SwerveDrive swerveDrive; - - @BeforeEach - void setUp() { - // Initialize the mocks - MockitoAnnotations.openMocks(this); - - swerveDrive = - spy( - new SwerveDrive( - gyroIO, - frontLeftModuleIO, - frontRightModuleIO, - backLeftModuleIO, - backRightModuleIO)); - - swerveDrive.setKinematics(swerveDriveKinematics); - } - - @Test - void testDrive() { - // Prepare mock module states - SwerveModuleState[] mockStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - mockStates[i] = mock(SwerveModuleState.class); // Mock each SwerveModuleState - } - - // Mock the toSwerveModuleStates method to return mock states - when(swerveDriveKinematics.toSwerveModuleStates(any(ChassisSpeeds.class))) - .thenReturn(mockStates); - - // Now when the drive method calls toSwerveModuleStates, it will return mockStates - swerveDrive.drive(1.0, 1.0, 0.5, true); - - // Verify that setModuleStates was called with mockStates - verify(swerveDrive).setModuleStates(mockStates); - verify(swerveDrive.getKinematics()).toSwerveModuleStates(any(ChassisSpeeds.class)); - } - - @Test - void testSetPose() { - // Mock pose setter - Pose2d newPose = new Pose2d(3.0, 4.0, Rotation2d.fromDegrees(90)); - swerveDrive.setPose(newPose); - - // Verify that the setPose method was called with the new pose - verify(swerveDrive).setPose(eq(newPose)); - } -} diff --git a/src/test/java/SwerveModuleTest.java b/src/test/java/SwerveModuleTest.java deleted file mode 100644 index 08535c9..0000000 --- a/src/test/java/SwerveModuleTest.java +++ /dev/null @@ -1,39 +0,0 @@ -import static edu.wpi.first.units.Units.*; -import static org.junit.jupiter.api.Assertions.*; -import static org.mockito.Mockito.*; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import org.mockito.Mock; -import org.mockito.MockitoAnnotations; - -class SwerveModuleTest { - - private SwerveModule swerveModule; - @Mock private ModuleInterface mockModuleInterface; - @Mock private ModuleInputsAutoLogged mockInputs; - - @BeforeEach - void setUp() { - MockitoAnnotations.openMocks(this); - - // Initialize SwerveModule with the mocked interface - swerveModule = spy(new SwerveModule(mockModuleInterface, "Mocked Module")); - mockInputs = spy(mockInputs); - } - - @Test - void testRunSetpoint() { - // Prepare a mock state for the module - SwerveModuleState mockState = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45)); - - // Call runSetPoint and verify that the setDesiredState method is called on the module interface - swerveModule.runSetpoint(mockState); - verify(mockModuleInterface).setDesiredState(mockState); - } -} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 498e436..69538ea 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,42 +1,32 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "4.0.0-alpha-1", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2025", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "4.0.0-alpha-1" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "4.0.0-alpha-1" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "4.0.0-alpha-1" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "4.0.0-alpha-1", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [] -} \ No newline at end of file + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.0.0-beta-1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.0.0-beta-1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.0.0-beta-1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} diff --git a/vendordeps/ChoreoLib2025Beta.json b/vendordeps/ChoreoLib2025Beta.json index 1f6b2b3..a912da9 100644 --- a/vendordeps/ChoreoLib2025Beta.json +++ b/vendordeps/ChoreoLib2025Beta.json @@ -1,44 +1,44 @@ { - "fileName": "ChoreoLib2025Beta.json", - "name": "ChoreoLib", - "version": "2025.0.0-beta-6", - "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2025", - "mavenUrls": [ - "https://SleipnirGroup.github.io/ChoreoLib/dep", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json", - "javaDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-java", - "version": "2025.0.0-beta-6" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.11.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-cpp", - "version": "2025.0.0-beta-6", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file + "fileName": "ChoreoLib2025Beta.json", + "name": "ChoreoLib", + "version": "2025.0.0-beta-6", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.0-beta-6" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.0-beta-6", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/Dyn4j.json b/vendordeps/Dyn4j.json index 7bbdce5..b537d76 100644 --- a/vendordeps/Dyn4j.json +++ b/vendordeps/Dyn4j.json @@ -1,23 +1,23 @@ { - "fileName": "Dyn4j.json", - "name": "MapleSim", - "version": "0.0.1", - "uuid": "123e4567-e89b-12d3-a456-426614174000", - "frcYear": "2025", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "junit", - "artifactId": "junit", - "version": "4.13.1" - }, - { - "groupId": "org.dyn4j", - "artifactId": "dyn4j", - "version": "5.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] - } \ No newline at end of file + "fileName": "Dyn4j.json", + "name": "MapleSim", + "version": "0.0.1", + "uuid": "123e4567-e89b-12d3-a456-426614174000", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "junit", + "artifactId": "junit", + "version": "4.13.1" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/vendordeps/NavX-2025.1.1-beta-1.json b/vendordeps/NavX-2025.1.1-beta-1.json deleted file mode 100644 index 1bb5610..0000000 --- a/vendordeps/NavX-2025.1.1-beta-1.json +++ /dev/null @@ -1,39 +0,0 @@ -{ - "fileName": "NavX-2025.1.1-beta-1.json", - "name": "navx_frc", - "version": "2025.1.1-beta-1", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2025", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2025/" - ], - "jsonUrl": "https://dev.studica.com/releases/2025/NavX-2025.1.1-beta-1.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx_frc-java", - "version": "2025.1.1-beta-1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx_frc-cpp", - "version": "2025.1.1-beta-1", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-25.0.0-beta-4.json b/vendordeps/Phoenix6-25.0.0-beta-4.json new file mode 100644 index 0000000..5a588aa --- /dev/null +++ b/vendordeps/Phoenix6-25.0.0-beta-4.json @@ -0,0 +1,389 @@ +{ + "fileName": "Phoenix6-25.0.0-beta-4.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.0.0-beta-4", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.0.0-beta-4" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.0.0-beta-4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.0.0-beta-4", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.0.0-beta-4", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.0.0-beta-4", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.0.0-beta-4", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.0.0-beta-4", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/Phoenix6-replay-25.0.0-beta-1.json b/vendordeps/Phoenix6-replay-25.0.0-beta-1.json deleted file mode 100644 index e29d5c4..0000000 --- a/vendordeps/Phoenix6-replay-25.0.0-beta-1.json +++ /dev/null @@ -1,386 +0,0 @@ -{ - "fileName": "Phoenix6-replay-25.0.0-beta-1.json", - "name": "CTRE-Phoenix (v6) Replay", - "version": "25.0.0-beta-1", - "frcYear": "2025", - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2025-beta-latest.json", - "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - }, - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-frc2025-beta-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.0.0-beta-1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "api-cpp-replay", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "tools-replay", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.0.0-beta-1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-1", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.0.0-beta-1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "wpiapi-cpp-replay", - "version": "25.0.0-beta-1", - "libName": "CTRE_Phoenix6_WPIReplay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "tools-replay", - "version": "25.0.0-beta-1", - "libName": "CTRE_PhoenixTools_Replay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "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": "25.0.0-beta-1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/Studica-2025.1.1-beta-4.json b/vendordeps/Studica-2025.1.1-beta-4.json new file mode 100644 index 0000000..facfc72 --- /dev/null +++ b/vendordeps/Studica-2025.1.1-beta-4.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.1.1-beta-4.json", + "name": "Studica", + "version": "2025.1.1-beta-4", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-4.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.1.1-beta-4" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.1.1-beta-4" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.1.1-beta-4" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.1.1-beta-4" + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..2e8e6b0 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-5", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "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": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-5", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-5", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-5" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-5" + } + ] +}