diff --git a/.gitignore b/.gitignore index 99ac30a..375359c 100644 --- a/.gitignore +++ b/.gitignore @@ -168,13 +168,17 @@ out/ # Fleet .fleet - # Simulation GUI and other tools window save file -*-window.json - networktables.json -*.wpilog simgui.json -simgui-ds.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ -ctre_sim +# clangd +/.cache +compile_commands.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 410b0cd..466f7dd 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024beta", + "projectYear": "2025beta", "teamNumber": 5990 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec..645e542 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 351fbfc..2b4461c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1" } java { @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.2' + implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.13' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" @@ -117,4 +117,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} +} \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 7f93135..a4b76b9 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 1058752..fbacf71 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a4..f5feea6 100644 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59..9d21a21 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail diff --git a/settings.gradle b/settings.gradle index d94f73c..969c7b0 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2fe531b..7273d93 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -7,8 +7,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj2.command.Command; -import frc.trigon.robot.commands.Commands; -import frc.trigon.robot.constants.CommandConstants; +import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -17,7 +17,7 @@ public class RobotContainer { public static final Swerve SWERVE = new Swerve(); public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator(); - private LoggedDashboardChooser autoChooser; + static LoggedDashboardChooser autoChooser; public RobotContainer() { configureBindings(); @@ -42,9 +42,10 @@ private void bindDefaultCommands() { private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); + OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); - OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand()); - OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(Commands.getToggleBrakeCommand()); + OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); + OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); } private void buildAutoChooser() { diff --git a/src/main/java/frc/trigon/robot/constants/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java similarity index 73% rename from src/main/java/frc/trigon/robot/constants/CommandConstants.java rename to src/main/java/frc/trigon/robot/commands/CommandConstants.java index 3125620..a758e13 100644 --- a/src/main/java/frc/trigon/robot/constants/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -1,10 +1,11 @@ -package frc.trigon.robot.constants; +package frc.trigon.robot.commands; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -21,16 +22,17 @@ public class CommandConstants { public static final Command FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopFieldRelativeDriveCommand( - () -> DRIVER_CONTROLLER.getLeftY() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> DRIVER_CONTROLLER.getLeftX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> DRIVER_CONTROLLER.getRightX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER) + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( - () -> DRIVER_CONTROLLER.getLeftY() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> DRIVER_CONTROLLER.getLeftX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> DRIVER_CONTROLLER.getRightX() / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER) + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), false), new Rotation2d()).get())), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), @@ -38,6 +40,14 @@ public class CommandConstants { ), STATIC_WHITE_LED_COLOR_COMMAND = LEDStripCommands.getStaticColorCommand(Color.white, LEDStripConstants.LED_STRIPS); + public static double calculateDriveStickAxisValue(double axisValue) { + return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER); + } + + public static double calculateRotationStickAxisValue(double axisValue) { + return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER); + } + /** * The shift mode is a mode of the robot that slows down the robot relative to how much the right trigger axis is pressed. * This method will take the given power, and slow it down relative to how much the right trigger is pressed. diff --git a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java index bb8d8b6..32e25b7 100644 --- a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java @@ -1,6 +1,7 @@ package frc.trigon.robot.commands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -8,10 +9,11 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; +import org.json.simple.parser.ParseException; import org.trigon.utilities.mirrorable.MirrorablePose2d; import java.awt.*; -import java.util.function.Supplier; +import java.io.IOException; /** * A command that sets the LED strips to a color based on the robot's position and orientation relative to the starting @@ -22,17 +24,9 @@ public class LEDAutoSetupCommand extends SequentialCommandGroup { private static final double TOLERANCE_METERS = 0.1, TOLERANCE_DEGREES = 2; - private final Supplier autoName; private Pose2d autoStartPose; - /** - * Constructs a new LEDAutoSetupCommand. - * - * @param autoName a supplier that returns the name of the selected autonomous path - */ - public LEDAutoSetupCommand(Supplier autoName) { - this.autoName = autoName; - + public LEDAutoSetupCommand() { addCommands( getUpdateAutoStartPoseCommand(), LEDStripCommands.getThreeSectionColorCommand( @@ -58,7 +52,14 @@ public boolean runsWhenDisabled() { private Command getUpdateAutoStartPoseCommand() { return new InstantCommand( - () -> this.autoStartPose = new MirrorablePose2d(PathPlannerAuto.getStaringPoseFromAutoFile(autoName.get()), true).get() + () -> { + try { + final Pose2d nonMirroredAutoStartPose = PathPlannerPath.fromPathFile(PathPlannerAuto.currentPathName).getStartingHolonomicPose().get(); + this.autoStartPose = new MirrorablePose2d(nonMirroredAutoStartPose, true).get(); + } catch (IOException | ParseException e) { + throw new RuntimeException(e); + } + } ).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/commands/Commands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java similarity index 87% rename from src/main/java/frc/trigon/robot/commands/Commands.java rename to src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 37d7e89..d18c61b 100644 --- a/src/main/java/frc/trigon/robot/commands/Commands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -1,13 +1,13 @@ -package frc.trigon.robot.commands; +package frc.trigon.robot.commands.factories; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.CommandConstants; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import java.util.function.BooleanSupplier; -public class Commands { +public class GeneralCommands { public static boolean IS_BRAKING = true; public static Command withoutRequirements(Command command) { @@ -61,6 +61,10 @@ public static Command runWhen(Command command, BooleanSupplier condition) { return new WaitUntilCommand(condition).andThen(command); } + public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { + return new WaitUntilCommand(condition).andThen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition))); + } + public static Command duplicate(Command command) { return new FunctionalCommand( command::initialize, diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index ef6ef2b..6008bd5 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -1,5 +1,8 @@ package frc.trigon.robot.constants; public class CameraConstants { - //todo: implement CameraConstants + public static final double + TRANSLATIONS_STD_EXPONENT = 0.02, + THETA_STD_EXPONENT = 0.02; + //TODO: implement CameraConstants } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index cefaf1d..3295e73 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -1,7 +1,40 @@ package frc.trigon.robot.constants; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +import java.io.IOException; +import java.util.HashMap; + public class FieldConstants { + private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; + private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; + + static { + try { + APRIL_TAG_FIELD_LAYOUT = SHOULD_USE_HOME_TAG_LAYOUT ? + AprilTagFieldLayout.loadFromResource("path/to/layout.json") : + AprilTagFields.kDefaultField.loadAprilTagLayoutField();//TODO:Switch for year + } catch (IOException e) { + throw new RuntimeException(e); + } + } + + public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); + public static final double FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.02; + + private static HashMap fieldLayoutToTagIdToPoseMap() { + final HashMap tagIdToPose = new HashMap<>(); + for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) + tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); + return tagIdToPose; + } } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 5b29aa4..34fb932 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -21,6 +21,7 @@ public class OperatorConstants { public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r().or(DRIVER_CONTROLLER.a()), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1); diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index d7b6a1d..c70f2eb 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -8,12 +8,13 @@ public class RobotConstants { private static final boolean IS_SIMULATION = true, IS_REPLAY = false; + private static final RobotHardwareStats.ReplayType REPLAY_TYPE = IS_SIMULATION ? RobotHardwareStats.ReplayType.SIMULATION_REPLAY : RobotHardwareStats.ReplayType.REAL_REPLAY; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = IS_SIMULATION && Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; public static void init() { - RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_REPLAY ? REPLAY_TYPE : RobotHardwareStats.ReplayType.NONE); RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); } } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java deleted file mode 100644 index e9658b0..0000000 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ /dev/null @@ -1,63 +0,0 @@ -package frc.trigon.robot.misc.objectdetectioncamera; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; -import org.trigon.hardware.RobotHardwareStats; - -public class ObjectDetectionCamera extends SubsystemBase { - private final ObjectDetectionCameraInputsAutoLogged objectDetectionCameraInputs = new ObjectDetectionCameraInputsAutoLogged(); - private final ObjectDetectionCameraIO objectDetectionCameraIO; - private final String hostname; - private double lastVisibleObjectYaw = 0; - - public ObjectDetectionCamera(String hostname) { - this.hostname = hostname; - objectDetectionCameraIO = generateIO(hostname); - } - - @Override - public void periodic() { - objectDetectionCameraIO.updateInputs(objectDetectionCameraInputs); - Logger.processInputs(hostname, objectDetectionCameraInputs); - } - - public boolean hasTargets() { - return objectDetectionCameraInputs.hasTargets; - } - - /** - * @return the yaw (x-axis position) of the target object - */ - public double getBestObjectYaw() { - return objectDetectionCameraInputs.bestObjectYaw; - } - - public double getTrackedObjectYaw() { - double closestYawDifference = 10000000; - double closestYaw = 10000000; - for (double currentYaw : objectDetectionCameraInputs.visibleObjectsYaw) { - final double yawDifference = Math.abs(currentYaw - lastVisibleObjectYaw); - if (yawDifference < closestYawDifference) { - closestYawDifference = yawDifference; - closestYaw = currentYaw; - } - } - if (closestYawDifference != 10000000) { - lastVisibleObjectYaw = closestYaw; - return lastVisibleObjectYaw; - } - return lastVisibleObjectYaw; - } - - public void startTrackingBestObject() { - lastVisibleObjectYaw = getBestObjectYaw(); - } - - private ObjectDetectionCameraIO generateIO(String hostname) { - if (RobotHardwareStats.isReplay()) - return new ObjectDetectionCameraIO(); - if (RobotHardwareStats.isSimulation()) - return new SimulationObjectDetectionCameraIO(hostname); - return new PhotonObjectDetectionCameraIO(hostname); - } -} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java deleted file mode 100644 index 4ebaf10..0000000 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.trigon.robot.misc.objectdetectioncamera; - -import org.littletonrobotics.junction.AutoLog; - -public class ObjectDetectionCameraIO { - protected ObjectDetectionCameraIO() { - } - - protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - } - - @AutoLog - public static class ObjectDetectionCameraInputs { - public boolean hasTargets = false; - public double bestObjectYaw = 0; - public double[] visibleObjectsYaw = new double[0]; - } -} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java deleted file mode 100644 index ed5da4d..0000000 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.trigon.robot.misc.objectdetectioncamera; - -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -public class PhotonObjectDetectionCameraIO extends ObjectDetectionCameraIO { - private final PhotonCamera photonCamera; - - protected PhotonObjectDetectionCameraIO(String hostname) { - PhotonCamera.setVersionCheckEnabled(false); - photonCamera = new PhotonCamera(hostname); - } - - @Override - protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - if (!photonCamera.isConnected()) - return; - final PhotonPipelineResult result = photonCamera.getLatestResult(); - - inputs.hasTargets = result.hasTargets(); - if (inputs.hasTargets) { - inputs.bestObjectYaw = getBestTargetYaw(result); - inputs.visibleObjectsYaw = result.getTargets().stream().mapToDouble((it) -> -it.getYaw()).toArray(); - } - } - - private double getBestTargetYaw(PhotonPipelineResult result) { - double lowestSum = 100000; - double chosenOne = 0; - for (PhotonTrackedTarget target : result.getTargets()) { - final double current = Math.abs(target.getYaw()) + Math.abs(target.getPitch()); - if (lowestSum > current) { - lowestSum = current; - chosenOne = -target.getYaw(); - } - } - return chosenOne; - } -} diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java deleted file mode 100644 index 0413bd7..0000000 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java +++ /dev/null @@ -1,166 +0,0 @@ -package frc.trigon.robot.misc.objectdetectioncamera; - -import edu.wpi.first.math.geometry.*; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.Commands; -import org.littletonrobotics.junction.Logger; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; - -public class SimulationObjectDetectionCameraIO extends ObjectDetectionCameraIO { - public static boolean HAS_OBJECTS = true; - private static final Rotation2d HORIZONTAL_FOV = Rotation2d.fromDegrees(75); - private static final double - MAXIMUM_DISTANCE_METERS = 5, - MINIMUM_DISTANCE_METERS = 0.05; - private static final double PICKING_UP_TOLERANCE_METERS = 0.3; - - private final ArrayList objectsOnField = new ArrayList<>(Arrays.asList( - new Translation2d(2.9, 7), - new Translation2d(2.9, 5.5), - new Translation2d(2.9, 4.1), - new Translation2d(8.3, 7.45), - new Translation2d(8.3, 5.75), - new Translation2d(8.3, 4.1), - new Translation2d(8.3, 2.45), - new Translation2d(8.3, 0.75), - new Translation2d(13.65, 7), - new Translation2d(13.65, 5.5), - new Translation2d(13.65, 4.1) - )); - private final String hostname; - private Pose3d[] heldObject = {new Pose3d()}; - private boolean isDelayingEjection = false; - - protected SimulationObjectDetectionCameraIO(String hostname) { - this.hostname = hostname; - } - - @Override - protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); - if (closestObjectYaw == null) { - inputs.hasTargets = false; - } else { - inputs.hasTargets = true; - inputs.bestObjectYaw = closestObjectYaw.getDegrees(); - inputs.visibleObjectsYaw = new double[]{inputs.bestObjectYaw}; - } - - updateObjectCollection(); - updateObjectEjection(); - updateHeldObjectPose(); - HAS_OBJECTS = heldObject.length != 0; - Logger.recordOutput("Poses/GamePieces/HeldGamePiece", heldObject); - Logger.recordOutput("Poses/GamePieces/ObjectsOnField", toPosesArray(objectsOnField)); - } - - private void updateHeldObjectPose() { - if (heldObject.length == 0) - return; - heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); - } - - private void updateObjectEjection() { - if (heldObject.length == 0 || !isEjecting() || isDelayingEjection) - return; - isDelayingEjection = true; - Commands.getDelayedCommand(0.04, () -> { - heldObject = new Pose3d[0]; - isDelayingEjection = false; - }).schedule(); - } - - private void updateObjectCollection() { - if (heldObject.length == 1 || !isCollecting()) - return; - final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); - final Translation2d robotTranslation = robotPose.getTranslation(); - for (Translation2d objectPlacement : objectsOnField) { - if (objectPlacement.getDistance(robotTranslation) <= PICKING_UP_TOLERANCE_METERS) { - heldObject = new Pose3d[]{getHeldObjectPose(robotPose)}; - objectsOnField.remove(objectPlacement); - Commands.getDelayedCommand(10, () -> objectsOnField.add(objectPlacement)).schedule(); - break; - } - } - } - - private Pose3d getHeldObjectPose(Pose2d robotPose) { - //todo: get the pose of the object held by the robot - return new Pose3d(); - } - - private Transform3d toTransform(Pose3d pose) { - return new Transform3d(pose.getTranslation(), pose.getRotation()); - } - - private Pose3d[] toPosesArray(List translationsList) { - final Pose3d[] posesArray = new Pose3d[translationsList.size()]; - for (int i = 0; i < translationsList.size(); i++) { - final Translation2d translation = translationsList.get(i); - posesArray[i] = new Pose3d(translation.getX(), translation.getY(), 0.1, new Rotation3d()); - } - return posesArray; - } - - private boolean isEjecting() { - //todo: check if the robot is ejecting - return false; - } - - private boolean isCollecting() { - //todo: check if the robot is collecting - return false; - } - - private Rotation2d getClosestVisibleObjectYaw(Pose2d robotPose) { - Translation2d closestObject = null; - Rotation2d closestObjectYaw = null; - double closestDistance = Double.POSITIVE_INFINITY; - - for (Translation2d objectPlacement : objectsOnField) { - final Rotation2d angleToObject = getAngleToObject(objectPlacement, robotPose); - if (!isWithinHorizontalFOV(angleToObject, robotPose) || !isWithinDistance(objectPlacement, robotPose)) - continue; - - final double distance = getObjectDistance(objectPlacement, robotPose); - if (distance < closestDistance) { - closestObject = objectPlacement; - closestObjectYaw = angleToObject.minus(robotPose.getRotation()); - closestDistance = distance; - } - } - - logObjectPlacement(closestObject); - return closestObjectYaw; - } - - private void logObjectPlacement(Translation2d objectPlacement) { - if (objectPlacement != null) - Logger.recordOutput(hostname + "/ClosestObject", objectPlacement); - else - Logger.recordOutput(hostname + "/ClosestObject", new Translation2d[0]); - } - - private boolean isWithinHorizontalFOV(Rotation2d objectYaw, Pose2d robotPose) { - return Math.abs(objectYaw.minus(robotPose.getRotation()).getRadians()) <= HORIZONTAL_FOV.getRadians() / 2; - } - - private boolean isWithinDistance(Translation2d objectPlacement, Pose2d robotPose) { - final double distance = getObjectDistance(objectPlacement, robotPose); - return distance <= MAXIMUM_DISTANCE_METERS && distance >= MINIMUM_DISTANCE_METERS; - } - - private double getObjectDistance(Translation2d objectPlacement, Pose2d robotPose) { - final Translation2d difference = objectPlacement.minus(robotPose.getTranslation()); - return difference.getNorm(); - } - - private Rotation2d getAngleToObject(Translation2d objectPlacement, Pose2d robotPose) { - final Translation2d difference = objectPlacement.minus(robotPose.getTranslation()); - return Rotation2d.fromRadians(Math.atan2(difference.getY(), difference.getX())); - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java new file mode 100644 index 0000000..4db5e81 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -0,0 +1,231 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.Robot; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; +import org.littletonrobotics.junction.Logger; + +/** + * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. + * An april tag is like a 2D barcode used to find the robot's position on the field. + * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field. + */ +public class AprilTagCamera { + protected final String name; + private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); + private final Transform3d robotCenterToCamera; + private final double + thetaStandardDeviationExponent, + translationStandardDeviationExponent; + private final AprilTagCameraIO aprilTagCameraIO; + private double lastUpdatedTimestamp; + private Pose2d robotPose = null; + + /** + * Constructs a new AprilTagCamera. + * + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param thetaStandardDeviationExponent a calibrated gain for the standard theta deviations of the estimated robot pose + * @param translationStandardDeviationExponent a calibrated gain for the standard translation deviations of the estimated robot pose + */ + public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, + String name, Transform3d robotCenterToCamera, + double thetaStandardDeviationExponent, + double translationStandardDeviationExponent) { + this.name = name; + this.robotCenterToCamera = robotCenterToCamera; + this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; + this.translationStandardDeviationExponent = translationStandardDeviationExponent; + + if (Robot.IS_REAL) + aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); + else + aprilTagCameraIO = new AprilTagCameraIO(); + } + + public void update() { + aprilTagCameraIO.updateInputs(inputs); + + robotPose = calculateBestRobotPose(); + logCameraInfo(); + } + + public boolean hasNewResult() { + return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); + } + + public boolean hasResult() { + return inputs.hasResult; + } + + public Pose2d getEstimatedRobotPose() { + return robotPose; + } + + public Rotation2d getSolvePNPHeading() { + return inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + } + + public String getName() { + return name; + } + + public double getLatestResultTimestampSeconds() { + return inputs.latestResultTimestampSeconds; + } + + /** + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. + * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. + * + * @return the standard deviations for the pose estimation strategy used + */ + public Matrix calculateStandardDeviations() { + final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = isWithinBestTagRangeForSolvePNP() ? calculateStandardDeviations(thetaStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length) : Double.POSITIVE_INFINITY; + + return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); + } + + public double getDistanceToBestTagMeters() { + return inputs.distanceFromBestTag; + } + + /** + * If the robot is close enough to the tag, it calculates the pose using the solve PNP heading. + * If it's too far, it assumes the robot's heading from the gyro and calculates its position from there. + * Assuming the robot's heading from the gyro is more robust, but won't fix current wrong heading. + * To fix this, we use solve PNP to reset the gyro when we are close enough for an accurate result. + * + * @return the robot's pose + */ + private Pose2d calculateBestRobotPose() { + final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); + } + + /** + * Calculates the robot's pose by assuming its heading, the apriltag's position, and the camera's position on the robot. + * This method of pose estimation is more robust than solve PNP, but relies on knowing the robot's heading beforehand. + * + * @return the robot's pose + */ + private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { + if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult) + return null; + + final Translation2d fieldRelativeRobotTranslation = getFieldRelativeRobotTranslation(gyroHeading); + + if (!isWithinBestTagRangeForSolvePNP()) + return new Pose2d(fieldRelativeRobotTranslation, gyroHeading); + + final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + return new Pose2d(fieldRelativeRobotTranslation, solvePNPHeading); + } + + private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); + + final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); + final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); + final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(gyroHeading); + return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); + } + + private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { + final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); + final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); + final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); + return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); + } + + private double getRobotPlaneTargetYawRadians() { + double targetYawRadians = -inputs.bestTargetRelativeYawRadians; + for (int i = 0; i < AprilTagCameraConstants.CALCULATE_YAW_ITERATIONS; i++) { + final double projectedPitch = projectToPlane(-robotCenterToCamera.getRotation().getY(), targetYawRadians + Math.PI / 2); + targetYawRadians = -inputs.bestTargetRelativeYawRadians - Math.tan(projectedPitch) * -inputs.bestTargetRelativePitchRadians; + } + return projectToPlane(targetYawRadians, robotCenterToCamera.getRotation().getY()); + } + + private double projectToPlane(double targetAngleRadians, double cameraAngleRadians) { + if (cameraAngleRadians < 0) + return Math.atan(Math.tan(targetAngleRadians) / Math.cos(cameraAngleRadians)); + return Math.atan(Math.tan(targetAngleRadians) * Math.cos(cameraAngleRadians)); + } + + private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { + double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); + return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); + } + + private Translation2d getFieldRelativeRobotPose(Translation2d tagRelativeCameraTranslation, Pose3d tagPose) { + return tagPose.getTranslation().toTranslation2d().minus(tagRelativeCameraTranslation); + } + + private boolean isNewTimestamp() { + if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) + return false; + + lastUpdatedTimestamp = getLatestResultTimestampSeconds(); + return true; + } + + /** + * Calculates the standard deviation of the estimated pose using a formula. + * As we get further from the tag(s), this will return a less trusting (higher deviation) result. + * + * @param exponent a calibrated gain, different for each pose estimating strategy + * @param distance the distance from the tag(s) + * @param numberOfVisibleTags the number of visible tags + * @return the standard deviation + */ + private double calculateStandardDeviations(double exponent, double distance, int numberOfVisibleTags) { + return exponent * (distance * distance) / numberOfVisibleTags; + } + + private boolean isWithinBestTagRangeForSolvePNP() { + return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + } + + private void logCameraInfo() { + Logger.processInputs("Cameras/" + name, inputs); + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); + if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { + logEstimatedRobotPose(); + logSolvePNPPose(); + } else { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", AprilTagCameraConstants.EMPTY_POSE_LIST); + } + } + + private void logUsedTags() { + if (!inputs.hasResult) { + Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]); + return; + } + + final Pose3d[] usedTagPoses = isWithinBestTagRangeForSolvePNP() ? new Pose3d[inputs.visibleTagIDs.length] : new Pose3d[1]; + for (int i = 0; i < usedTagPoses.length; i++) + usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); + Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); + } + + private void logEstimatedRobotPose() { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", robotPose); + } + + private void logSolvePNPPose() { + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", inputs.cameraSolvePNPPose.plus(robotCenterToCamera.inverse())); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java new file mode 100644 index 0000000..a9cddad --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; + +import java.util.function.Function; + +public class AprilTagCameraConstants { + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; + static final int CALCULATE_YAW_ITERATIONS = 3; + static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + + public enum AprilTagCameraType { + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + LIMELIGHT(AprilTagLimelightIO::new); + + final Function createIOFunction; + + AprilTagCameraType(Function createIOFunction) { + this.createIOFunction = createIOFunction; + } + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java new file mode 100644 index 0000000..c9233bb --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose3d; +import org.littletonrobotics.junction.AutoLog; + +public class AprilTagCameraIO { + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + } + + @AutoLog + public static class AprilTagCameraInputs { + public boolean hasResult = false; + public double latestResultTimestampSeconds = 0; + public Pose3d cameraSolvePNPPose = new Pose3d(); + public int[] visibleTagIDs = new int[0]; + public double bestTargetRelativeYawRadians = 0; + public double bestTargetRelativePitchRadians = 0; + public double distanceFromBestTag = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java new file mode 100644 index 0000000..065e827 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -0,0 +1,87 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.trigon.utilities.LimelightHelpers; + +public class AprilTagLimelightIO extends AprilTagCameraIO { + private final String hostname; + + public AprilTagLimelightIO(String hostname) { + this.hostname = hostname; + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); + + inputs.hasResult = results.targets_Fiducials.length > 0; + + if (inputs.hasResult) + updateHasResultInputs(inputs, results); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { + final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); + + inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; + inputs.visibleTagIDs = getVisibleTagIDs(results); + inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); + inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); + inputs.distanceFromBestTag = getDistanceFromBestTag(results); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + final int[] visibleTagIDs = new int[visibleTags.length]; + visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = (int) visibleTags[i].fiducialID; + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; + } + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param results the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); + return new Rotation3d(0, -Units.degreesToRadians(bestTag.tx), -Units.degreesToRadians(bestTag.ty)); + } + + private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) { + return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue()); + } + + private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { + return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); + } + + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) { + return results.targets_Fiducials[0]; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java new file mode 100644 index 0000000..3a08399 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -0,0 +1,124 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.opencv.core.Point; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.TargetCorner; + +import java.util.List; + +public class AprilTagPhotonCameraIO extends AprilTagCameraIO { + private final PhotonCamera photonCamera; + + public AprilTagPhotonCameraIO(String cameraName) { + photonCamera = new PhotonCamera(cameraName); + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final PhotonPipelineResult latestResult = photonCamera.getAllUnreadResults().get(photonCamera.getAllUnreadResults().size()); + + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[]{}; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param result the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { + final List tagCorners = result.getBestTarget().getDetectedCorners(); + final Point tagCenter = getTagCenter(tagCorners); + if (photonCamera.getCameraMatrix().isPresent()) + return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + return null; + } + + /** + * Estimates the camera's pose using Solve PNP using as many tags as possible. + * + * @param result the camera's pipeline result + * @return the estimated pose + */ + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + } + + final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); + final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); + final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); + return tagPose.transformBy(targetToCamera); + } + + private int[] getVisibleTagIDs(PhotonPipelineResult result) { + final int[] visibleTagIDs = new int[result.getTargets().size()]; + + for (int i = 1; i < visibleTagIDs.length; i++) + visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); + return visibleTagIDs; + } + + private double getDistanceFromBestTag(PhotonPipelineResult result) { + return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); + } + + private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + double fx = camIntrinsics.get(0, 0); + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - pixel.x; + + double fy = camIntrinsics.get(1, 1); + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - pixel.y; + + // calculate yaw normally + var yaw = new Rotation2d(fx, xOffset); + // correct pitch based on yaw + var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + + return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java deleted file mode 100644 index dcc605b..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java +++ /dev/null @@ -1,72 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * 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. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.math.geometry.Pose3d; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; - -/** - * An estimated pose based on pipeline result - */ -public class EstimatedRobotPose { - /** - * The estimated pose - */ - public final Pose3d estimatedPose; - - /** - * The estimated time the frame used to derive the robot pose was taken - */ - public final double timestampSeconds; - - /** - * A list of the targets used to compute this pose - */ - public final List targetsUsed; - - /** - * The strategy actually used to produce this pose - */ - public final PhotonPoseEstimator.PoseStrategy strategy; - - /** - * Constructs an EstimatedRobotPose - * - * @param estimatedPose estimated pose - * @param timestampSeconds timestamp of the estimate - */ - public EstimatedRobotPose( - Pose3d estimatedPose, - double timestampSeconds, - List targetsUsed, - PhotonPoseEstimator.PoseStrategy strategy) { - this.estimatedPose = estimatedPose; - this.timestampSeconds = timestampSeconds; - this.targetsUsed = targetsUsed; - this.strategy = strategy; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java deleted file mode 100644 index 9c932c2..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java +++ /dev/null @@ -1,791 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * 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. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.wpilibj.DriverStation; -import frc.trigon.robot.RobotContainer; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; - -/** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set - * below. Example usage can be found in our apriltagExample example project. - */ -public class PhotonPoseEstimator { - private static int InstanceCount = 0; - - /** - * Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. - */ - public enum PoseStrategy { - /** - * Choose the Pose with the lowest ambiguity. - */ - LOWEST_AMBIGUITY, - - /** - * Choose the Pose which is closest to the camera height. - */ - CLOSEST_TO_CAMERA_HEIGHT, - - /** - * Choose the Pose which is closest to a set Reference position. - */ - CLOSEST_TO_REFERENCE_POSE, - - /** - * Choose the Pose which is closest to the last pose calculated - */ - CLOSEST_TO_LAST_POSE, - - /** - * Return the average of the best target poses using ambiguity as weight. - */ - AVERAGE_BEST_TARGETS, - - /** - * Use all visible tags to compute a single pose estimate on coprocessor. This option needs to - * be enabled on the PhotonVision web UI as well. - */ - MULTI_TAG_PNP_ON_COPROCESSOR, - - /** - * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can - * take a lot of time. - */ - MULTI_TAG_PNP_ON_RIO, - - CLOSEST_TO_HEADING - } - - private AprilTagFieldLayout fieldTags; - private TargetModel tagModel = TargetModel.kAprilTag16h5; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private final PhotonCamera camera; - private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; - private final Set reportedErrors = new HashSet<>(); - - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot -> camera) in the Robot - * Coordinate System. - */ - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, - PoseStrategy strategy, - PhotonCamera camera, - Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = camera; - this.robotToCamera = robotToCamera; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this(fieldTags, strategy, null, robotToCamera); - } - - /** - * Invalidates the pose cache. - */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { - invalidatePoseCache(); - } - } - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @return the AprilTagFieldLayout - */ - public AprilTagFieldLayout getFieldTags() { - return fieldTags; - } - - /** - * Set the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @param fieldTags the AprilTagFieldLayout - */ - public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); - this.fieldTags = fieldTags; - } - - /** - * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - *

By default, this is {@link TargetModel#kAprilTag16h5}. - */ - public TargetModel getTagModel() { - return tagModel; - } - - /** - * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. - */ - public void setTagModel(TargetModel tagModel) { - this.tagModel = tagModel; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - * NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - */ - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { - DriverStation.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); - strategy = PoseStrategy.LOWEST_AMBIGUITY; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns - * empty if: - * - *

    - *
  • New data has not been received since the last call to {@code update()}. - *
  • No targets were found from the camera - *
  • There is no camera set - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update() { - if (camera == null) { - DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false); - return Optional.empty(); - } - - PhotonPipelineResult cameraResult = camera.getLatestResult(); - - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult) { - if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty()); - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result -// - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - PoseStrategy strat) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - estimatedPose = closestToCameraHeightStrategy(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case CLOSEST_TO_LAST_POSE: - setReferencePose(lastPose); - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case AVERAGE_BEST_TARGETS: - estimatedPose = averageBestTargetsStrategy(cameraResult); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case CLOSEST_TO_HEADING: - estimatedPose = closestToHeadingStrategy(cameraResult); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - - private Optional closestToHeadingStrategy(PhotonPipelineResult result) { - double smallestAngleDifferenceRadians; - EstimatedRobotPose closestAngleTarget; - double currentHeadingRadians = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getRadians(); - - PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - return Optional.empty(); - } - - Transform3d bestCameraToTarget = target.getBestCameraToTarget(); - - Pose3d bestPose = - targetPosition - .get() - .transformBy(bestCameraToTarget.inverse()) - .transformBy(robotToCamera.inverse()); - double bestTransformAngle = bestPose.getRotation().getZ(); - smallestAngleDifferenceRadians = Math.abs(currentHeadingRadians - bestTransformAngle); - closestAngleTarget = - new EstimatedRobotPose( - bestPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - - Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - if (alternateCameraToTarget.getRotation().getZ() != 0) { - Pose3d altPose = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - double alternateTransformAngle = altPose.getRotation().getZ(); - double alternateTransformDelta = Math.abs(currentHeadingRadians - alternateTransformAngle); - - if (alternateTransformDelta < smallestAngleDifferenceRadians) { - closestAngleTarget = - new EstimatedRobotPose( - altPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.of(closestAngleTarget); - } - - private Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - var best_tf = result.getMultiTagResult().estimatedPose.best; - var cam = new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()); - var best = cam.plus(robotToCamera.inverse()); // field-to-robot -// Logger.recordOutput(camera.getName(), Math.toDegrees(cam.getRotation().getY())); - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - } - - private Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent) - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - var best = - new Pose3d() - .plus(pnpResult.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = fieldTags.getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - return Optional.of( - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY)); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta height - * difference between the estimated and actual height of the camera. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { - double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - - - double alternateTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - - if (bestTransformDelta < smallestHeightDifference) { - smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta in the vector - * magnitude between it and the reference pose. - * - * @param result pipeline result - * @param referencePose reference pose to check vector magnitude difference against. - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToReferencePoseStrategy( - PhotonPipelineResult result, Pose3d referencePose) { - if (referencePose == null) { -// DriverStation.reportError( -// "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!", -// false); - return Optional.empty(); - } - - double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - Pose3d altTransformPosition = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - - double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); - double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); - - if (altDifference < smallestPoseDelta) { - smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose( - altTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - if (bestDifference < smallestPoseDelta) { - smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose( - bestTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - } - return Optional.ofNullable(lowestDeltaPose); - } - - /** - * Return the average of the best target poses using ambiguity as weight. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { - if (!result.hasTargets()) - return Optional.empty(); - final PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - final Pose3d cameraPose = targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()); -// Logger.recordOutput(camera.getName(), Math.toDegrees(cameraPose.getRotation().getY())); - final Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse()); - - return Optional.of( - new EstimatedRobotPose( - robotPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.AVERAGE_BEST_TARGETS - ) - ); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 1cc1dee..c59d3c4 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -1,19 +1,15 @@ package frc.trigon.robot.poseestimation.poseestimator; import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; 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.kinematics.SwerveDriveWheelPositions; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -26,22 +22,23 @@ */ public class PoseEstimator implements AutoCloseable { private final Field2d field = new Field2d(); - private final RobotPoseSource[] robotPoseSources; + private final AprilTagCamera[] aprilTagCameras; private final PoseEstimator6328 poseEstimator6328 = PoseEstimator6328.getInstance(); /** * Constructs a new PoseEstimator. * - * @param robotPoseSources the sources that should update the pose estimator apart from the odometry. This should be cameras etc. + * @param aprilTagCameras the sources that should update the pose estimator apart from the odometry. This should be cameras etc. */ - public PoseEstimator(RobotPoseSource... robotPoseSources) { - this.robotPoseSources = robotPoseSources; + public PoseEstimator(AprilTagCamera... aprilTagCameras) { + this.aprilTagCameras = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); PathPlannerLogging.setLogActivePathCallback((pose) -> { field.getObject("path").setPoses(pose); Logger.recordOutput("Path", pose.toArray(new Pose2d[0])); }); + PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("TargetPPPose", pose)); } @Override @@ -79,11 +76,25 @@ public Pose2d getCurrentPose() { * @param swerveWheelPositions the SWERVE wheel positions accumulated since the last update * @param gyroRotations the gyro rotations accumulated since the last update */ - public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPositions, Rotation2d[] gyroRotations, double[] timestamps) { + public void updatePoseEstimatorStates(SwerveModulePosition[][] swerveWheelPositions, Rotation2d[] gyroRotations, double[] timestamps) { for (int i = 0; i < swerveWheelPositions.length; i++) poseEstimator6328.addOdometryObservation(new PoseEstimator6328.OdometryObservation(swerveWheelPositions[i], gyroRotations[i], timestamps[i])); } + /** + * Because we rely on the gyro for the robot's heading, we need to use solve PNP to reset the gyro's assumed heading. + */ + public void setGyroHeadingToBestSolvePNPHeading() { + int closestCameraToTag = 0; + for (int i = 0; i < aprilTagCameras.length; i++) { + if (aprilTagCameras[i].hasResult() && aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) + closestCameraToTag = i; + } + + final Rotation2d bestCameraHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestCameraHeading)); + } + private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) @@ -92,40 +103,33 @@ private void updateFromVision() { private List getViableVisionObservations() { List viableVisionObservations = new ArrayList<>(); - for (RobotPoseSource robotPoseSource : robotPoseSources) { - final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(robotPoseSource); + for (AprilTagCamera aprilTagCamera : aprilTagCameras) { + final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(aprilTagCamera); if (visionObservation != null) viableVisionObservations.add(visionObservation); } return viableVisionObservations; } - private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource) { - robotPoseSource.update(); - if (!robotPoseSource.hasNewResult()) + private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera aprilTagCamera) { + aprilTagCamera.update(); + if (!aprilTagCamera.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getRobotPose(); - if (robotPose == null) + final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); + if (robotPose == null || robotPose.getTranslation() == null || robotPose.getRotation() == null) return null; return new PoseEstimator6328.VisionObservation( robotPose, - robotPoseSource.getLastResultTimestamp(), - averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getVisibleTags()) + aprilTagCamera.getLatestResultTimestampSeconds(), + aprilTagCamera.calculateStandardDeviations() ); } - private Matrix averageDistanceToStdDevs(double averageDistance, int visibleTags) { - final double translationStd = PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT * Math.pow(averageDistance, 2) / (visibleTags * visibleTags); - final double thetaStd = PoseEstimatorConstants.THETA_STD_EXPONENT * Math.pow(averageDistance, 2) / visibleTags; - - return VecBuilder.fill(translationStd, translationStd, thetaStd); - } - private void putAprilTagsOnFieldWidget() { - for (Map.Entry entry : RobotPoseSourceConstants.TAG_ID_TO_POSE.entrySet()) { + for (Map.Entry entry : FieldConstants.TAG_ID_TO_POSE.entrySet()) { final Pose2d tagPose = entry.getValue().toPose2d(); field.getObject("Tag " + entry.getKey()).setPose(tagPose); } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 6819d8e..a763ea2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -26,7 +25,7 @@ public class PoseEstimator6328 { public record OdometryObservation( - SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) { + SwerveModulePosition[] wheelPositions, Rotation2d gyroAngle, double timestamp) { } public record VisionObservation(Pose2d visionPose, double timestamp, Matrix stdDevs) { @@ -49,14 +48,13 @@ public static PoseEstimator6328 getInstance() { private final Matrix qStdDevs = new Matrix<>(Nat.N3(), Nat.N1()); // Odometry private final SwerveDriveKinematics kinematics; - private SwerveDriveWheelPositions lastWheelPositions = - new SwerveDriveWheelPositions( - new SwerveModulePosition[]{ - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }); + private SwerveModulePosition[] lastWheelPositions = + new SwerveModulePosition[]{ + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private Rotation2d lastGyroAngle = new Rotation2d(); private PoseEstimator6328() { @@ -145,6 +143,13 @@ public void addVisionObservation(VisionObservation observation) { estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } + public Pose2d samplePose(double timestamp) { + Pose2d sample = poseBuffer.getSample(timestamp).orElse(new Pose2d()); + Transform2d odometryToSampleTransform = new Transform2d(odometryPose, sample); + + return estimatedPose.plus(odometryToSampleTransform); + } + /** * Reset estimated pose and odometry pose to pose
* Clear pose buffer diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index a87aa90..d8967b5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.numbers.N3; public class PoseEstimatorConstants { - public static final double ODOMETRY_FREQUENCY_HERTZ = 50; + public static final double ODOMETRY_FREQUENCY_HERTZ = 250; /** * The vector represents how ambiguous each value of the odometry is. @@ -14,9 +14,5 @@ public class PoseEstimatorConstants { * Increase these numbers to trust the estimate less. */ static final Vector ODOMETRY_AMBIGUITY = VecBuilder.fill(0.003, 0.003, 0.0002); - - static final double - TRANSLATIONS_STD_EXPONENT = 0.005, - THETA_STD_EXPONENT = 0.01; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java deleted file mode 100644 index e5c715f..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import org.trigon.utilities.LimelightHelpers; - -public class AprilTagLimelightIO extends RobotPoseSourceIO { - private final String hostname; - - protected AprilTagLimelightIO(String hostname) { - this.hostname = hostname; - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = LimelightHelpers.getTV(hostname); - if (inputs.hasResult) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(results.getBotPose3d_wpiBlue()); - inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java deleted file mode 100644 index e144ea7..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ /dev/null @@ -1,90 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.trigon.robot.poseestimation.photonposeestimator.EstimatedRobotPose; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; -import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; -import java.util.Optional; - -public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { - private final PhotonCamera photonCamera; - private final PhotonPoseEstimator photonPoseEstimator; - - protected AprilTagPhotonCameraIO(String cameraName, Transform3d robotCenterToCamera) { - photonCamera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - RobotPoseSourceConstants.APRIL_TAG_FIELD_LAYOUT, - RobotPoseSourceConstants.PRIMARY_POSE_STRATEGY, - photonCamera, - robotCenterToCamera - ); - - photonPoseEstimator.setMultiTagFallbackStrategy(RobotPoseSourceConstants.SECONDARY_POSE_STRATEGY); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - Optional optionalEstimatedRobotPose = photonPoseEstimator.update(latestResult); - - inputs.hasResult = hasResult(optionalEstimatedRobotPose); - if (inputs.hasResult) { - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(estimatedRobotPose.estimatedPose); - inputs.lastResultTimestamp = estimatedRobotPose.timestampSeconds; - inputs.visibleTags = estimatedRobotPose.targetsUsed.size(); - inputs.averageDistanceFromTags = getAverageDistanceFromTags(latestResult); - } else { - inputs.visibleTags = 0; - inputs.cameraPose = new double[0]; - } - - logVisibleTags(inputs.hasResult, optionalEstimatedRobotPose); - } - - private void logVisibleTags(boolean hasResult, Optional optionalEstimatedRobotPose) { - if (!hasResult) { - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); - return; - } - - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - final Pose2d[] visibleTagPoses = new Pose2d[estimatedRobotPose.targetsUsed.size()]; - for (int i = 0; i < visibleTagPoses.length; i++) { - final int currentId = estimatedRobotPose.targetsUsed.get(i).getFiducialId(); - final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); - visibleTagPoses[i] = currentPose; - } - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); - } - - private boolean hasResult(Optional optionalEstimatedRobotPose) { - final boolean isEmpty = optionalEstimatedRobotPose.isEmpty(); - if (isEmpty) - return false; - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - if (estimatedRobotPose.strategy == PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) - return true; - return estimatedRobotPose.targetsUsed.get(0).getPoseAmbiguity() < RobotPoseSourceConstants.MAXIMUM_AMBIGUITY; - } - - private double getAverageDistanceFromTags(PhotonPipelineResult result) { - final List targets = result.targets; - double distanceSum = 0; - - for (PhotonTrackedTarget currentTarget : targets) { - final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); - distanceSum += distanceTranslation.getNorm(); - } - - return distanceSum / targets.size(); - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java deleted file mode 100644 index ae4ff6b..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.*; -import frc.trigon.robot.Robot; -import org.littletonrobotics.junction.Logger; - -/** - * A pose source is a class that provides the robot's pose, from a camera. - */ -public class RobotPoseSource { - protected final String name; - private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); - private final Transform3d robotCenterToCamera; - private final RobotPoseSourceIO robotPoseSourceIO; - private double lastUpdatedTimestamp; - private Pose2d cachedPose = null; - - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera) { - this.name = name; - if (robotPoseSourceType != RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA) - this.robotCenterToCamera = robotCenterToCamera; - else - this.robotCenterToCamera = new Transform3d(); - - if (Robot.IS_REAL) - robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name, robotCenterToCamera); - else - robotPoseSourceIO = new RobotPoseSourceIO(); - } - - public static double[] pose3dToDoubleArray(Pose3d pose) { - if (pose == null) - return new double[0]; - - return new double[]{ - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getTranslation().getZ(), - pose.getRotation().getX(), - pose.getRotation().getY(), - pose.getRotation().getZ() - }; - } - - public void update() { - robotPoseSourceIO.updateInputs(inputs); - Logger.processInputs("Cameras/" + name, inputs); - cachedPose = getUnCachedRobotPose(); - if (!inputs.hasResult || inputs.averageDistanceFromTags == 0 || cachedPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); - else - Logger.recordOutput("Poses/Robot/" + name + "Pose", cachedPose); - } - - public int getVisibleTags() { - return inputs.visibleTags; - } - - public double getAverageDistanceFromTags() { - return inputs.averageDistanceFromTags; - } - - public boolean hasNewResult() { - return (inputs.hasResult && inputs.averageDistanceFromTags != 0) && isNewTimestamp(); - } - - public Pose2d getRobotPose() { - return cachedPose; - } - - public String getName() { - return name; - } - - public double getLastResultTimestamp() { - return inputs.lastResultTimestamp; - } - - private Pose2d getUnCachedRobotPose() { - final Pose3d cameraPose = doubleArrayToPose3d(inputs.cameraPose); - if (cameraPose == null) - return null; - - return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); - } - - private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLastResultTimestamp()) - return false; - - lastUpdatedTimestamp = getLastResultTimestamp(); - return true; - } - - private Pose3d doubleArrayToPose3d(double[] doubleArray) { - if (doubleArray == null || doubleArray.length != 6) - return null; - - return new Pose3d( - new Translation3d(doubleArray[0], doubleArray[1], doubleArray[2]), - new Rotation3d(doubleArray[3], doubleArray[4], doubleArray[5]) - ); - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java deleted file mode 100644 index 8303aae..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; - -import java.util.HashMap; -import java.util.function.BiFunction; - -public class RobotPoseSourceConstants { - public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); - static final PhotonPoseEstimator.PoseStrategy - PRIMARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - SECONDARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_HEADING; - static AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - static { - for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - TAG_ID_TO_POSE.put(aprilTag.ID, aprilTag.pose); - } - - static final double MAXIMUM_AMBIGUITY = 0.2; - static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; - - public enum RobotPoseSourceType { - PHOTON_CAMERA(AprilTagPhotonCameraIO::new), - LIMELIGHT((name, transform3d) -> new AprilTagLimelightIO(name)), - T265((name, transform3d) -> new T265IO(name)); - - final BiFunction createIOFunction; - - RobotPoseSourceType(BiFunction createIOFunction) { - this.createIOFunction = createIOFunction; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java deleted file mode 100644 index 494a71a..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import org.littletonrobotics.junction.AutoLog; - -public class RobotPoseSourceIO { - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - } - - @AutoLog - public static class RobotPoseSourceInputs { - public boolean hasResult = false; - public double lastResultTimestamp = 0; - public double[] cameraPose = new double[6]; - public double averageDistanceFromTags = 0; - public int visibleTags = 0; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java deleted file mode 100644 index 89d7c8d..0000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import org.trigon.utilities.JsonHandler; - -public class T265IO extends RobotPoseSourceIO { - private static final NetworkTable NETWORK_TABLE = NetworkTableInstance.getDefault().getTable("T265"); - private static final short CONFIDENCE_THRESHOLD = 2; - private final NetworkTableEntry jsonDump; - - protected T265IO(String name) { - jsonDump = NETWORK_TABLE.getEntry(name + "/jsonDump"); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = canUseJsonDump(); - if (inputs.hasResult) - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(getCameraPose()); - inputs.lastResultTimestamp = (double) jsonDump.getLastChange() / 1000000; - } - - private Pose3d getCameraPose() { - if (!canUseJsonDump()) - return null; - - return getRobotPoseFromJsonDump(); - } - - private Pose3d getRobotPoseFromJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - final Translation3d translation = getTranslationFromDoubleArray(jsonDump.translation); - final Rotation3d rotation = getRotationFromDoubleArray(jsonDump.rotation); - - return t265PoseToWPIPose(new Pose3d(translation, rotation)); - } - - private Pose3d t265PoseToWPIPose(Pose3d t265Pose) { - final CoordinateSystem eusCoordinateSystem = new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.U(), CoordinateAxis.S()); - final Pose3d convertedPose = CoordinateSystem.convert(t265Pose, eusCoordinateSystem, CoordinateSystem.NWU()); - final Rotation3d convertedRotation = convertedPose.getRotation().plus(new Rotation3d(0, 0, Math.toRadians(90))); - - return new Pose3d(convertedPose.getTranslation(), convertedRotation); - } - - private Translation3d getTranslationFromDoubleArray(double[] xyz) { - return new Translation3d(xyz[0], xyz[1], xyz[2]); - } - - private Rotation3d getRotationFromDoubleArray(double[] wxyz) { - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } - - private boolean canUseJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - - try { - return jsonDump.confidence >= CONFIDENCE_THRESHOLD && jsonDump.translation.length == 3 && jsonDump.rotation.length == 4; - } catch (NullPointerException e) { - return false; - } - } - - private T265JsonDump getJsonDump() { - return JsonHandler.parseJsonStringToObject(jsonDump.getString(""), T265JsonDump.class); - } - - private static class T265JsonDump { - private double[] translation; - private double[] rotation; - private int confidence; - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 3c2b13f..92c610e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -1,15 +1,17 @@ package frc.trigon.robot.subsystems; import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Voltage; +import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.commands.Commands; -import frc.trigon.robot.constants.CommandConstants; +import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.factories.GeneralCommands; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.trigon.hardware.RobotHardwareStats; import java.util.ArrayList; import java.util.List; @@ -24,13 +26,14 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase { private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); + private static final LoggedDashboardBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedDashboardBoolean("EnableExtensiveLogging", true); static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); DISABLED_TRIGGER.onFalse(new InstantCommand(() -> { setAllSubsystemsBrakeAsync(true); CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.cancel(); - Commands.IS_BRAKING = true; + GeneralCommands.IS_BRAKING = true; }).ignoringDisable(true)); } @@ -59,6 +62,22 @@ public static void setAllSubsystemsBrakeAsync(boolean brake) { CompletableFuture.runAsync(() -> forEach((subsystem) -> subsystem.setBrake(brake))); } + public static boolean isExtensiveLoggingEnabled() { + return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay(); + } + + /** + * Runs periodically, to update the subsystem, and update the mechanism of the subsystem (if there is one). + * This only updates the mechanism if the robot is in replay mode or extensive logging is enabled. + * This function cannot be overridden. Use {@linkplain MotorSubsystem#updatePeriodically} or {@linkplain MotorSubsystem#updateMechanism} (depending on the usage) instead. + */ + @Override + public final void periodic() { + updatePeriodically(); + if (isExtensiveLoggingEnabled()) + updateMechanism(); + } + /** * Creates a quasistatic (ramp up) command for characterizing the subsystem's mechanism. * @@ -99,7 +118,7 @@ public void setBrake(boolean brake) { * * @param voltageMeasure the target voltage */ - public void drive(Measure voltageMeasure) { + public void drive(Measure voltageMeasure) { } /** @@ -110,6 +129,19 @@ public void drive(Measure voltageMeasure) { public void updateLog(SysIdRoutineLog log) { } + /** + * Updates the mechanism of the subsystem periodically if the robot is in replay mode, or if {@linkplain MotorSubsystem#ENABLE_EXTENSIVE_LOGGING) is true. + * This doesn't always run in order to save resources. + */ + public void updateMechanism() { + } + + /** + * Updates periodically. Anything that should be updated periodically but isn't related to the mechanism (or shouldn't always be logged, in order to save resources) should be put here. + */ + public void updatePeriodically() { + } + public SysIdRoutine.Config getSysIdConfig() { return null; } @@ -137,4 +169,4 @@ private SysIdRoutine createSysIdRoutine() { ) ); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 8a9b8d9..917dd2c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.Robot; -import frc.trigon.robot.commands.Commands; +import frc.trigon.robot.commands.factories.GeneralCommands; import java.awt.*; import java.util.function.Function; @@ -22,7 +22,7 @@ public class LEDStrip extends SubsystemBase { private final boolean inverted; static { - Commands.getDelayedCommand(1, () -> LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getAnimateSingleFadeCommand(Color.red, LEDStripConstants.LOW_BATTERY_FLASHING_SPEED, LEDStripConstants.LED_STRIPS))).schedule(); + GeneralCommands.getDelayedCommand(1, () -> LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getAnimateSingleFadeCommand(Color.red, LEDStripConstants.LOW_BATTERY_FLASHING_SPEED, LEDStripConstants.LED_STRIPS))).schedule(); } /** diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index fcf78f9..d8149dc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -2,12 +2,18 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathfindingCommand; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.*; +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.util.Units; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -34,13 +40,17 @@ public Swerve() { } @Override - public void periodic() { + public void updatePeriodically() { Phoenix6SignalThread.SIGNALS_LOCK.lock(); updateHardware(); Phoenix6SignalThread.SIGNALS_LOCK.unlock(); updatePoseEstimatorStates(); RobotContainer.POSE_ESTIMATOR.periodic(); + } + + @Override + public void updateMechanism() { updateNetworkTables(); } @@ -56,9 +66,27 @@ public void setBrake(boolean brake) { currentModule.setBrake(brake); } + @Override + public void drive(Measure voltageMeasure) { + for (SwerveModule swerveModule : swerveModules) { + swerveModule.setTargetDriveMotorCurrent(voltageMeasure.in(edu.wpi.first.units.Units.Volts)); + swerveModule.setTargetAngle(new Rotation2d()); + } + } + + @Override + public void updateLog(SysIdRoutineLog log) { + for (SwerveModule swerveModule : swerveModules) + swerveModule.driveMotorUpdateLog(log); + } + + @Override + public SysIdRoutine.Config getSysIdConfig() { + return SwerveModuleConstants.DRIVE_MOTOR_SYSID_CONFIG; + } + public Rotation2d getHeading() { - final double inputtedHeading = MathUtil.inputModulus(gyro.getSignal(Pigeon2Signal.YAW), -0.5, 0.5); - return Rotation2d.fromRotations(inputtedHeading); + return Rotation2d.fromDegrees(SwerveConstants.GYRO.getSignal(Pigeon2Signal.YAW)); } public void setHeading(Rotation2d heading) { @@ -102,15 +130,20 @@ public boolean atAngle(MirrorableRotation2d angle) { return atTargetAngle/* && isAngleStill*/; } - public SwerveModulePosition[] getWheelPositions() { - final SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; + public double[] getDriveWheelPositionsRadians() { + final double[] swerveModulesPositions = new double[swerveModules.length]; for (int i = 0; i < swerveModules.length; i++) - swerveModulePositions[i] = swerveModules[i].getOdometryPosition(swerveModules[i].getLastOdometryUpdateIndex()); - return swerveModulePositions; + swerveModulesPositions[i] = swerveModules[i].getDriveWheelPosition(); + return swerveModulesPositions; + } + + public Rotation2d getDriveRelativeAngle() { + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; } - public void runWheelRadiusCharacterization(double omegaRadsPerSec) { - selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadsPerSec)); + public void runWheelRadiusCharacterization(double omegaRadiansPerSecond) { + selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)); } /** @@ -244,7 +277,7 @@ private ChassisSpeeds discretize(ChassisSpeeds chassisSpeeds) { private void updatePoseEstimatorStates() { final double[] odometryUpdatesYawDegrees = gyro.getThreadedSignal(Pigeon2Signal.YAW); final int odometryUpdates = odometryUpdatesYawDegrees.length; - final SwerveDriveWheelPositions[] swerveWheelPositions = new SwerveDriveWheelPositions[odometryUpdates]; + final SwerveModulePosition[][] swerveWheelPositions = new SwerveModulePosition[odometryUpdates][]; final Rotation2d[] gyroRotations = new Rotation2d[odometryUpdates]; for (int i = 0; i < odometryUpdates; i++) { @@ -255,11 +288,11 @@ private void updatePoseEstimatorStates() { RobotContainer.POSE_ESTIMATOR.updatePoseEstimatorStates(swerveWheelPositions, gyroRotations, phoenix6SignalThread.getLatestTimestamps()); } - private SwerveDriveWheelPositions getSwerveWheelPositions(int odometryUpdateIndex) { + private SwerveModulePosition[] getSwerveWheelPositions(int odometryUpdateIndex) { final SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; for (int i = 0; i < swerveModules.length; i++) swerveModulePositions[i] = swerveModules[i].getOdometryPosition(odometryUpdateIndex); - return new SwerveDriveWheelPositions(swerveModulePositions); + return swerveModulePositions; } private void updateHardware() { @@ -272,13 +305,14 @@ private void updateHardware() { } private void configurePathPlanner() { - AutoBuilder.configureHolonomic( - () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose(), + AutoBuilder.configure( + RobotContainer.POSE_ESTIMATOR::getCurrentPose, (pose) -> { }, this::getSelfRelativeVelocity, - this::selfRelativeDrive, - SwerveConstants.HOLONOMIC_PATH_FOLLOWER_CONFIG, + (speeds, feedforwards) -> selfRelativeDrive(speeds), + SwerveConstants.PATH_FOLLOWING_CONTROLLER, + SwerveConstants.ROBOT_CONFIG, Mirrorable::isRedAlliance, this ); @@ -304,11 +338,6 @@ private ChassisSpeeds fieldRelativeSpeedsToSelfRelativeSpeeds(ChassisSpeeds fiel return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getDriveRelativeAngle()); } - private Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; - } - private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaPower) { return new ChassisSpeeds( xPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 95ecaf8..11ec70d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -1,11 +1,8 @@ package frc.trigon.robot.subsystems.swerve; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.*; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import org.trigon.commands.InitExecuteCommand; @@ -162,14 +159,15 @@ private static Command getPIDToPoseCommand(MirrorablePose2d targetPose) { } private static Command createOnTheFlyPathCommand(MirrorablePose2d targetPose, PathConstraints constraints) { - List bezierPoints = PathPlannerPath.bezierFromPoses( + List waypoints = PathPlannerPath.waypointsFromPoses( RobotContainer.POSE_ESTIMATOR.getCurrentPose(), targetPose.get() ); PathPlannerPath path = new PathPlannerPath( - bezierPoints, + waypoints, constraints, + new IdealStartingState(0, SWERVE.getHeading()), new GoalEndState(0, targetPose.get().getRotation()) ); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 77dd748..825a243 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -1,25 +1,29 @@ package frc.trigon.robot.subsystems.swerve; import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PathFollowingController; +import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; 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.trajectory.TrapezoidProfile; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import org.json.simple.parser.ParseException; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Signal; -import org.trigon.utilities.Conversions; +import java.io.IOException; import java.util.function.DoubleSupplier; -public abstract class SwerveConstants { +public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double @@ -27,15 +31,15 @@ public abstract class SwerveConstants { GYRO_MOUNT_POSITION_PITCH = 0, GYRO_MOUNT_POSITION_ROLL = 0; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(225.263672 - 360), - FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-256.904297 + 360), - REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(108.369141), - REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-36.035156); + FRONT_LEFT_STEER_ENCODER_OFFSET = 0, + FRONT_RIGHT_STEER_ENCODER_OFFSET = 0, + REAR_LEFT_STEER_ENCODER_OFFSET = 0, + REAR_RIGHT_STEER_ENCODER_OFFSET = 0; private static final int - FRONT_LEFT_ID = 0, - FRONT_RIGHT_ID = 1, - REAR_LEFT_ID = 2, - REAR_RIGHT_ID = 3; + FRONT_LEFT_ID = 1, + FRONT_RIGHT_ID = 2, + REAR_LEFT_ID = 3, + REAR_RIGHT_ID = 4; static final SwerveModule[] SWERVE_MODULES = { new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET), new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET), @@ -46,17 +50,18 @@ public abstract class SwerveConstants { private static final DoubleSupplier SIMULATION_YAW_VELOCITY_SUPPLIER = () -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond; private static final double - MODULE_X_DISTANCE_FROM_CENTER = 0.6457 / 2, - MODULE_Y_DISTANCE_FROM_CENTER = 0.5357 / 2; - private static final Translation2d[] LOCATIONS = { - new Translation2d(MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER), - new Translation2d(MODULE_X_DISTANCE_FROM_CENTER, -MODULE_Y_DISTANCE_FROM_CENTER), - new Translation2d(-MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER), - new Translation2d(-MODULE_X_DISTANCE_FROM_CENTER, -MODULE_Y_DISTANCE_FROM_CENTER) + MODULE_Y_DISTANCE_FROM_CENTER = 0.27285, + FRONT_MODULE_X_DISTANCE_FROM_CENTER = 0.17215, + REAR_MODULE_X_DISTANCE_FROM_CENTER = -0.24285; + public static final Translation2d[] MODULE_LOCATIONS = { + new Translation2d(FRONT_MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER), + new Translation2d(FRONT_MODULE_X_DISTANCE_FROM_CENTER, -MODULE_Y_DISTANCE_FROM_CENTER), + new Translation2d(REAR_MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER), + new Translation2d(REAR_MODULE_X_DISTANCE_FROM_CENTER, -MODULE_Y_DISTANCE_FROM_CENTER) }; - public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(LOCATIONS); - public static final double DRIVE_RADIUS_METERS = Math.hypot( - MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS); + private static final double FURTHEST_MODULE_DISTANCE_FROM_CENTER = Math.hypot( + REAR_MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER ); static final double @@ -76,14 +81,14 @@ public abstract class SwerveConstants { new PIDConstants(5, 0, 0) : new PIDConstants(5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(8, 0, 0) : - new PIDConstants(5, 0, 0), + new PIDConstants(4, 0, 0.05) : + new PIDConstants(3, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(9, 0, 0) : - new PIDConstants(6.5, 0, 0), + new PIDConstants(5, 0, 0.1) : + new PIDConstants(2, 0, 0.1), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(8.9, 0, 0) : - new PIDConstants(3, 0, 0); + new PIDConstants(2.5, 0, 0.2) : + new PIDConstants(6.5, 0, 0); private static final double MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 720, MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 720; @@ -103,14 +108,32 @@ public abstract class SwerveConstants { TRANSLATION_PID_CONSTANTS.kD ); - private static final ReplanningConfig REPLANNING_CONFIG = new ReplanningConfig(true, false); - static final HolonomicPathFollowerConfig HOLONOMIC_PATH_FOLLOWER_CONFIG = new HolonomicPathFollowerConfig( - AUTO_TRANSLATION_PID_CONSTANTS, - AUTO_ROTATION_PID_CONSTANTS, - MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND, - SwerveConstants.DRIVE_RADIUS_METERS, - REPLANNING_CONFIG - ); + static final RobotConfig ROBOT_CONFIG; + + static { + try { + ROBOT_CONFIG = RobotConfig.fromGUISettings(); + } catch (IOException | ParseException e) { + throw new RuntimeException(e); + } + } + + static final PathFollowingController PATH_FOLLOWING_CONTROLLER = new PathFollowingController() { + @Override + public ChassisSpeeds calculateRobotRelativeSpeeds(Pose2d currentPose, PathPlannerTrajectoryState targetState) { + return null; + } + + @Override + public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) { + + } + + @Override + public boolean isHolonomic() { + return false; + } + }; static { final Pigeon2Configuration config = new Pigeon2Configuration(); @@ -120,6 +143,6 @@ public abstract class SwerveConstants { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(SIMULATION_YAW_VELOCITY_SUPPLIER); - GYRO.registerThreadedSignal(Pigeon2Signal.YAW, Pigeon2Signal.ANGULAR_VELOCITY_Z_WORLD, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index fbc2f0b..c33811b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -1,11 +1,14 @@ package frc.trigon.robot.subsystems.swerve; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; @@ -20,6 +23,7 @@ public class SwerveModule { private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0); private final VoltageOut driveVoltageRequest = new VoltageOut(0); + private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0); private boolean driveMotorClosedLoop = false; private double[] latestOdometryDrivePositions = new double[0], @@ -29,10 +33,21 @@ public class SwerveModule { public SwerveModule(int moduleID, double offsetRotations) { driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); - steerEncoder = new CANcoderEncoder(moduleID, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); configureHardware(offsetRotations); } + void setTargetDriveMotorCurrent(double targetCurrent) { + driveMotor.setControl(driveTorqueCurrentFOCRequest.withOutput(targetCurrent)); + } + + void driveMotorUpdateLog(SysIdRoutineLog log) { + log.motor("Module" + driveMotor.getID() + "Drive") + .angularPosition(Units.Rotations.of(driveMotor.getSignal(TalonFXSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(driveMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(driveMotor.getSignal(TalonFXSignal.TORQUE_CURRENT))); + } + void stop() { driveMotor.stopMotor(); steerMotor.stopMotor(); @@ -61,6 +76,14 @@ void setTargetState(SwerveModuleState targetState) { setTargetVelocity(this.targetState.speedMetersPerSecond, this.targetState.angle); } + void setTargetAngle(Rotation2d angle) { + steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); + } + + double getDriveWheelPosition() { + return edu.wpi.first.math.util.Units.rotationsToRadians(driveMotor.getSignal(TalonFXSignal.POSITION)); + } + /** * The odometry thread can update itself faster than the main code loop (which is 50 hertz). * Instead of using the latest odometry update, the accumulated odometry positions since the last loop to get a more accurate position. @@ -91,10 +114,6 @@ private double driveRotationsToMeters(double rotations) { return Conversions.rotationsToDistance(rotations, SwerveModuleConstants.WHEEL_DIAMETER_METERS); } - private void setTargetAngle(Rotation2d angle) { - steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); - } - /** * Sets the target velocity for the module. * @@ -155,11 +174,15 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 714bccc..cba2464 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.*; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.simulation.SimpleMotorSimulation; @@ -23,16 +25,19 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 100, - STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 50; + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 80, + STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 30; private static final double STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 50 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 52, DRIVE_MOTOR_I = 0, - DRIVE_MOTOR_D = 0; + DRIVE_MOTOR_D = 0, + DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 6.2176, + DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0.0017378, + DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 2.4345; static final boolean ENABLE_FOC = true; static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = generateDriveConfiguration(), @@ -49,7 +54,13 @@ public class SwerveModuleConstants { DRIVE_MOTOR_GEARBOX = DCMotor.getKrakenX60Foc(DRIVE_MOTOR_AMOUNT), STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); - static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049149 * 2; + static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(5).per(Units.Second), + Units.Volts.of(8), + Units.Second.of(1000) + ); + + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049274 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { @@ -80,6 +91,9 @@ private static TalonFXConfiguration generateDriveConfiguration() { config.Slot0.kP = DRIVE_MOTOR_P; config.Slot0.kI = DRIVE_MOTOR_I; config.Slot0.kD = DRIVE_MOTOR_D; + config.Slot0.kS = DRIVE_MOTOR_KS; + config.Slot0.kV = DRIVE_MOTOR_KV; + config.Slot0.kA = DRIVE_MOTOR_KA; return config; } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 63dacbb..498e436 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,33 +1,33 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.1", + "version": "4.0.0-alpha-1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ + "javaDependencies": [ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.2.1" - }, + "version": "4.0.0-alpha-1" + }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.2.1" - }, + "version": "4.0.0-alpha-1" + }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.2.1" + "version": "4.0.0-alpha-1" } ], - "jniDependencies": [ + "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.2.1", + "version": "4.0.0-alpha-1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 6dc648d..4083333 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.0.0-beta-4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.0.0-beta-4" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.0.0-beta-4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64", "osxuniversal", + "linuxx86-64", "linuxathena", "linuxarm32", "linuxarm64" diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json index ff7359e..e27638b 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5.json @@ -1,13 +1,13 @@ { - "fileName": "Phoenix5.json", + "fileName": "Phoenix5-5.34.0-beta-2.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, + "version": "5.34.0-beta-2", + "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json", "requires": [ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", @@ -20,19 +20,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.33.1" + "version": "5.34.0-beta-2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.1" + "version": "5.34.0-beta-2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.34.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +45,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -60,7 +60,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -75,7 +75,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -105,7 +105,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 0322385..c56e61a 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,32 +1,50 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-25.0.0-beta-2.json", "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, + "version": "25.0.0-beta-2", + "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", "offlineFileName": "Phoenix6And5.json" + }, + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.3.0" + "version": "25.0.0-beta-2" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -38,8 +56,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", + "artifactId": "api-cpp-sim", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -51,8 +69,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", + "artifactId": "tools-sim", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -64,8 +82,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +109,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +122,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +135,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +148,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +161,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +176,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +191,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +206,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +221,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +236,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -230,25 +248,10 @@ ], "simMode": "swsim" }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +266,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +281,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +296,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +311,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f85acd4..ee18ef4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,25 +1,25 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.4", - "frcYear": "2024", + "version": "2025.0.0-beta-2", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.4" + "version": "2025.0.0-beta-2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index e3e494e..3718e0a 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": 2024, + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0e80a16..6276732 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,57 +1,57 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2024.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.3.1" - } - ] + "fileName": "photonlib.json", + "name": "photonlib", + "version": "dev-v2025.0.0-beta-1-3-g152b4391", + "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": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "dev-v2025.0.0-beta-1-3-g152b4391", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "dev-v2025.0.0-beta-1-3-g152b4391", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "dev-v2025.0.0-beta-1-3-g152b4391" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "dev-v2025.0.0-beta-1-3-g152b4391" + } + ] } \ No newline at end of file