From ad0cba313b5d84d4e06332c47680c02175d054cf Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Tue, 29 Aug 2023 18:19:56 -0700 Subject: [PATCH] port over swerve from spike hector & susan --- src/main/java/frc/robot/config/Config.java | 82 ++ src/main/java/frc/robot/imu/ImuSubsystem.java | 65 ++ .../robot/localization/JacksonLagUtil.java | 32 + .../frc/robot/localization/Landmarks.java | 134 ++++ .../robot/localization/LimelightHelpers.java | 729 ++++++++++++++++++ .../localization/LocalizationSubsystem.java | 206 +++++ .../frc/robot/localization/VisionMode.java | 11 + .../java/frc/robot/swerve/SwerveCorner.java | 12 + .../java/frc/robot/swerve/SwerveModule.java | 189 +++++ .../robot/swerve/SwerveModuleConstants.java | 22 + .../frc/robot/swerve/SwerveSubsystem.java | 340 ++++++++ .../java/frc/robot/util/CircleConverter.java | 39 + .../java/frc/robot/util/CtreModuleState.java | 62 ++ ...{PhoenixProAnd5.json => Phoenix6And5.json} | 124 +-- 14 files changed, 1985 insertions(+), 62 deletions(-) create mode 100644 src/main/java/frc/robot/imu/ImuSubsystem.java create mode 100644 src/main/java/frc/robot/localization/JacksonLagUtil.java create mode 100644 src/main/java/frc/robot/localization/Landmarks.java create mode 100644 src/main/java/frc/robot/localization/LimelightHelpers.java create mode 100644 src/main/java/frc/robot/localization/LocalizationSubsystem.java create mode 100644 src/main/java/frc/robot/localization/VisionMode.java create mode 100644 src/main/java/frc/robot/swerve/SwerveCorner.java create mode 100644 src/main/java/frc/robot/swerve/SwerveModule.java create mode 100644 src/main/java/frc/robot/swerve/SwerveModuleConstants.java create mode 100644 src/main/java/frc/robot/swerve/SwerveSubsystem.java create mode 100644 src/main/java/frc/robot/util/CircleConverter.java create mode 100644 src/main/java/frc/robot/util/CtreModuleState.java rename vendordeps/{PhoenixProAnd5.json => Phoenix6And5.json} (81%) diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index ce1fa31..6caa657 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -4,6 +4,14 @@ package frc.robot.config; +import com.pathplanner.lib.auto.PIDConstants; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.localization.VisionMode; +import frc.robot.swerve.SwerveCorner; +import frc.robot.swerve.SwerveModuleConstants; + public class Config { public static final String SERIAL_NUMBER = System.getenv("serialnum"); @@ -12,5 +20,79 @@ public class Config { public static final String CANIVORE_ID = "581CANivore"; + public static final VisionMode VISION_MODE = VisionMode.ENABLED_UNUSED; +public static final Translation2d SWERVE_FRONT_LEFT_LOCATION = + new Translation2d(0.263525, 0.263525); +public static final Translation2d SWERVE_FRONT_RIGHT_LOCATION = + new Translation2d(0.263525, -0.263525); +public static final Translation2d SWERVE_BACK_LEFT_LOCATION = + new Translation2d(-0.263525, 0.263525); +public static final Translation2d SWERVE_BACK_RIGHT_LOCATION = + new Translation2d(-0.263525, -0.263525); + + public static final double WHEEL_DIAMETER = Units.inchesToMeters(3.84); + public static final double SWERVE_STEER_GEARING_REDUCTION = 150.0 / 7.0; + public static final double SWERVE_DRIVE_GEARING_REDUCTION = + 50.0 * 16.0 * 45.0 / 14.0 / 28.0 / 15.0; + public static final double SWERVE_STEER_KV = 0.0; + public static final double SWERVE_STEER_KP = 5.0; + public static final double SWERVE_STEER_KI = 0.0; + public static final double SWERVE_STEER_KD = 0.0; + public static final double SWERVE_STEER_KS = 0.0; + + public static final int SWERVE_DRIVE_VOLTAGE_PEAK_FORWARD_VOLTAGE = 12; + public static final int SWERVE_DRIVE_VOLTAGE_PEAK_REVERSE_VOLTAGE = -12; + public static final double SWERVE_DRIVE_CURRENT_LIMIT = 35.0; + public static final boolean SWERVE_DRIVE_LIMITS_ENABLE = true; + + public static final double SWERVE_DRIVE_KP = 0.24; + public static final double SWERVE_DRIVE_KI = 0.0; + public static final double SWERVE_DRIVE_KD = 0.0; + public static final double SWERVE_DRIVE_KV = 0.1185; + public static final double SWERVE_DRIVE_KS = 0.0; + + public static final double STEER_MOTOR_LIMITS = 35; + public static final boolean SWERVE_MOTOR_LIMITS_ENABLED = true; + public static final PIDConstants SWERVE_TRANSLATION_PID = new PIDConstants(2.5, 0, 0); + public static final PIDConstants SWERVE_ROTATION_PID = new PIDConstants(4.5, 0, 0.1); + public static final PIDConstants SWERVE_ROTATION_SNAP_PID = PIDConstants(7.5, 0, 0.5); + public static final boolean SWERVE_USE_FOC = true; + + final int SWERVE_FL_DRIVE_MOTOR_ID = 8; + public static final int SWERVE_FL_STEER_MOTOR_ID = 9; + public static final int SWERVE_FL_CANCODER_ID = 13; + public static final SwerveModuleConstants SWERVE_FL_CONSTANTS = + new SwerveModuleConstants( + Rotation2d.fromDegrees(182.021484375), SwerveCorner.FRONT_LEFT, true, true); + + // -62.84 + public static final int SWERVE_FR_DRIVE_MOTOR_ID = 6; + public static final int SWERVE_FR_STEER_MOTOR_ID = 7; + public static final int SWERVE_FR_CANCODER_ID = 12; + public static final SwerveModuleConstants SWERVE_FR_CONSTANTS = + new SwerveModuleConstants( + Rotation2d.fromDegrees(159.521484375), SwerveCorner.FRONT_RIGHT, true, true); + + // -147.8 + public static final int SWERVE_BL_DRIVE_MOTOR_ID = 4; + public static final int SWERVE_BL_STEER_MOTOR_ID = 5; + public static final int SWERVE_BL_CANCODER_ID = 11; + public static final SwerveModuleConstants SWERVE_BL_CONSTANTS = + new SwerveModuleConstants( + Rotation2d.fromDegrees(2.548828125), SwerveCorner.BACK_LEFT, true, true); + // 78.75 + public static final int SWERVE_BR_DRIVE_MOTOR_ID = 2; + public static final int SWERVE_BR_STEER_MOTOR_ID = 3; + public static final int SWERVE_BR_CANCODER_ID = 10; + public static final SwerveModuleConstants SWERVE_BR_CONSTANTS = + new SwerveModuleConstants( + Rotation2d.fromDegrees(110.390625), SwerveCorner.BACK_RIGHT, true, true); + + // 104.58 + private Config() {} + + private static PIDConstants PIDConstants(double d, int i, double e) { + return null; + } } diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java new file mode 100644 index 0000000..1f7f9fa --- /dev/null +++ b/src/main/java/frc/robot/imu/ImuSubsystem.java @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.imu; + +import com.ctre.phoenix6.configs.MountPoseConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import org.littletonrobotics.junction.Logger; + +public class ImuSubsystem extends LifecycleSubsystem { + private final Pigeon2 imu; + + public ImuSubsystem(Pigeon2 imu) { + super(SubsystemPriority.IMU); + + this.imu = imu; + + Pigeon2Configuration imuConfig = new Pigeon2Configuration(); + + imu.getConfigurator().apply(imuConfig); + } + + public void robotPeriodic() { + Logger.getInstance().recordOutput("Imu/RobotHeading", this.getRobotHeading().getDegrees()); + Logger.getInstance() + .recordOutput("Imu/RobotHeadingRadians", this.getRobotHeading().getRadians()); + } + + public Rotation2d getRobotHeading() { + return Rotation2d.fromDegrees(imu.getYaw().getValue()); + } + + public Rotation2d getRoll() { + return Rotation2d.fromDegrees(imu.getRoll().getValue()); + } + + public void zero() { + setAngle(new Rotation2d()); + + MountPoseConfigs mountPoseConfig = new MountPoseConfigs(); + + mountPoseConfig.MountPoseRoll = getRoll().getDegrees(); + + imu.getConfigurator().apply(mountPoseConfig); + } + + public void setAngle(Rotation2d zeroAngle) { + this.imu.setYaw(zeroAngle.getDegrees()); + } + + public boolean atAngle(Rotation2d angle) { + return Math.abs(getRobotHeading().minus(angle).getDegrees()) < 3; + } + + public Command getZeroCommand() { + return Commands.runOnce(() -> zero()); + } +} diff --git a/src/main/java/frc/robot/localization/JacksonLagUtil.java b/src/main/java/frc/robot/localization/JacksonLagUtil.java new file mode 100644 index 0000000..72f7d47 --- /dev/null +++ b/src/main/java/frc/robot/localization/JacksonLagUtil.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.localization; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.localization.LimelightHelpers.LimelightResults; + +public class JacksonLagUtil { + public static void causeLag() { + ObjectMapper mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + + System.out.println("JacksonLagUtil: Causing lag spike..."); + double time = Timer.getFPGATimestamp(); + try { + LimelightResults results = new LimelightResults(); + mapper.readValue(mapper.writeValueAsString(results), LimelightHelpers.LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("JacksonLagUtil: Failed to cause lag spike, JSON parser error:"); + e.printStackTrace(); + } + System.out.println( + "JacksonLagUtil: Lag spike caused in " + (Timer.getFPGATimestamp() - time) + "s"); + } + + private JacksonLagUtil() {} +} diff --git a/src/main/java/frc/robot/localization/Landmarks.java b/src/main/java/frc/robot/localization/Landmarks.java new file mode 100644 index 0000000..ce28554 --- /dev/null +++ b/src/main/java/frc/robot/localization/Landmarks.java @@ -0,0 +1,134 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.localization; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import java.util.List; + +public class Landmarks { + public static final Pose2d APRILTAG_1 = + new Pose2d( + Units.inchesToMeters(610.77), Units.inchesToMeters(42.19), Rotation2d.fromDegrees(180)); + public static final Pose2d APRILTAG_2 = + new Pose2d( + Units.inchesToMeters(610.77), Units.inchesToMeters(108.19), Rotation2d.fromDegrees(180)); + public static final Pose2d APRILTAG_3 = + new Pose2d( + Units.inchesToMeters(610.77), Units.inchesToMeters(174.19), Rotation2d.fromDegrees(180)); + public static final Pose2d APRILTAG_4 = + new Pose2d( + Units.inchesToMeters(636.96), Units.inchesToMeters(265.74), Rotation2d.fromDegrees(180)); + public static final Pose2d APRILTAG_5 = + new Pose2d( + Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), Rotation2d.fromDegrees(0)); + public static final Pose2d APRILTAG_6 = + new Pose2d( + Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), Rotation2d.fromDegrees(0)); + public static final Pose2d APRILTAG_7 = + new Pose2d( + Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), Rotation2d.fromDegrees(0)); + public static final Pose2d APRILTAG_8 = + new Pose2d( + Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), Rotation2d.fromDegrees(0)); + + public static final Pose2d BLUE_GRID_RIGHT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(42), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_CENTER = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(108), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_LEFT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(174), Rotation2d.fromDegrees(0)); + + public static final Pose2d BLUE_GRID_LEFT_NODE_LEFT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(196), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_LEFT_NODE_CENTER = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(174), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_LEFT_NODE_RIGHT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(152), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_CENTER_NODE_LEFT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(130), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_CENTER_NODE_CENTER = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(108), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_CENTER_NODE_RIGHT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(86), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_RIGHT_NODE_LEFT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(64), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_RIGHT_NODE_CENTER = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(42), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_GRID_RIGHT_NODE_RIGHT = + new Pose2d(Units.inchesToMeters(54.05), Units.inchesToMeters(20), Rotation2d.fromDegrees(0)); + + public static final Pose2d RED_GRID_LEFT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(42), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_CENTER = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(108), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_RIGHT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(174), Rotation2d.fromDegrees(180)); + + public static final Pose2d RED_GRID_LEFT_NODE_RIGHT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(64), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_LEFT_NODE_CENTER = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(42), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_LEFT_NODE_LEFT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(20), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_CENTER_NODE_RIGHT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(130), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_CENTER_NODE_CENTER = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(108), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_CENTER_NODE_LEFT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(86), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_RIGHT_NODE_RIGHT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(196), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_RIGHT_NODE_CENTER = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(174), Rotation2d.fromDegrees(180)); + public static final Pose2d RED_GRID_RIGHT_NODE_LEFT = + new Pose2d( + Units.inchesToMeters(596.77), Units.inchesToMeters(152), Rotation2d.fromDegrees(180)); + + public static final Pose2d BLUE_STAGING_MARK_FAR_RIGHT = + new Pose2d( + Units.inchesToMeters(278.05), Units.inchesToMeters(36.19), Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_STAGING_MARK_RIGHT = + new Pose2d( + Units.inchesToMeters(278.05), Units.inchesToMeters(84.19), Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_STAGING_MARK_LEFT = + new Pose2d( + Units.inchesToMeters(278.05), Units.inchesToMeters(132.19), Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_STAGING_MARK_FAR_LEFT = + new Pose2d( + Units.inchesToMeters(278.05), Units.inchesToMeters(180.19), Rotation2d.fromDegrees(180)); + + public static final Pose2d RED_STAGING_MARK_FAR_LEFT = + new Pose2d( + Units.inchesToMeters(372.77), Units.inchesToMeters(36.19), Rotation2d.fromDegrees(0)); + public static final Pose2d RED_STAGING_MARK_LEFT = + new Pose2d( + Units.inchesToMeters(372.77), Units.inchesToMeters(84.19), Rotation2d.fromDegrees(0)); + public static final Pose2d RED_STAGING_MARK_RIGHT = + new Pose2d( + Units.inchesToMeters(372.77), Units.inchesToMeters(132.19), Rotation2d.fromDegrees(0)); + public static final Pose2d RED_STAGING_MARK_FAR_RIGHT = + new Pose2d( + Units.inchesToMeters(372.77), Units.inchesToMeters(180.19), Rotation2d.fromDegrees(0)); + + public static final List RED_GRIDS = + List.of(RED_GRID_LEFT, RED_GRID_CENTER, RED_GRID_RIGHT); + public static final List BLUE_GRIDS = + List.of(BLUE_GRID_LEFT, BLUE_GRID_CENTER, BLUE_GRID_RIGHT); + + private Landmarks() {} +} diff --git a/src/main/java/frc/robot/localization/LimelightHelpers.java b/src/main/java/frc/robot/localization/LimelightHelpers.java new file mode 100644 index 0000000..da60d6e --- /dev/null +++ b/src/main/java/frc/robot/localization/LimelightHelpers.java @@ -0,0 +1,729 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.localization; + +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fid") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode {} + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() {} + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() {} + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("tl_cap") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + public double valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public LimelightResults() { + targetingResults = new Results(); + } + } + + private static ObjectMapper mapper; + + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d( + Units.degreesToRadians(inData[3]), + Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "tl_cap"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetSpace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + public static double getTV(String limelightName) { + return getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** The LEDs will be controlled by Limelight pipeline settings, and not by robot code. */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + /** + * Sets the crop window. The crop window in the UI must be completely open for dynamic cropping to + * work. + */ + public static void setCropWindow( + String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** Asynchronously take snapshot. */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync( + () -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** Parses Limelight's JSON results dump into a LimelightResults Object */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java new file mode 100644 index 0000000..3c1232a --- /dev/null +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -0,0 +1,206 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.localization; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.InterpolatingTreeMap; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.config.Config; +import frc.robot.fms.FmsSubsystem; +import frc.robot.imu.ImuSubsystem; +import frc.robot.localization.LimelightHelpers.LimelightResults; +import frc.robot.swerve.SwerveSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import org.littletonrobotics.junction.Logger; + +public class LocalizationSubsystem extends LifecycleSubsystem { + + private final SwerveSubsystem swerve; + private final ImuSubsystem imu; + + private final SwerveDrivePoseEstimator poseEstimator; + private final SwerveDriveOdometry odometry; + private boolean visionWorking = false; + + private Pose2d previousPose = new Pose2d(); + + private final InterpolatingTreeMap visionStdLookup = + new InterpolatingTreeMap(); + + public LocalizationSubsystem(SwerveSubsystem swerve, ImuSubsystem imu) { + super(SubsystemPriority.LOCALIZATION); + + this.swerve = swerve; + this.imu = imu; + poseEstimator = + new SwerveDrivePoseEstimator( + SwerveSubsystem.KINEMATICS, + imu.getRobotHeading(), + swerve.getModulePositions(), + new Pose2d(), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(0.0334, 0.1391, Units.degreesToRadians(30))); + + odometry = + new SwerveDriveOdometry( + SwerveSubsystem.KINEMATICS, imu.getRobotHeading(), swerve.getModulePositions()); + + visionStdLookup.put(0.1, 0.8); + visionStdLookup.put(0.5, 1.0); + visionStdLookup.put(1.0, 2.0); + visionStdLookup.put(2.0, 2.5); + } + + @Override + public void robotPeriodic() { + update(); + + Logger.getInstance().recordOutput("Localization/CombinedPose", getPose()); + Logger.getInstance().recordOutput("Localization/OdometryPose", odometry.getPoseMeters()); + } + + private void update() { + try { + poseEstimator.update(imu.getRobotHeading(), swerve.getModulePositions()); + } catch (Exception e) { + System.err.println("Pose estimator threw while adding odometry measurement:"); + e.printStackTrace(); + } + try { + odometry.update(imu.getRobotHeading(), swerve.getModulePositions()); + } catch (Exception e) { + System.err.println("Odometry threw while adding measurement:"); + e.printStackTrace(); + } + + boolean visionIsValid = false; // Indicates if vision is valid in this loop. + + if (Config.VISION_MODE == VisionMode.OFF) { + visionWorking = false; + } else if (Config.VISION_MODE == VisionMode.ENABLED_UNUSED) { + Pose2d ntCurrentVisionPose = LimelightHelpers.getBotPose2d_wpiBlue(""); + Pose2d angleAdjustedVisionPose = + new Pose2d(ntCurrentVisionPose.getTranslation(), imu.getRobotHeading()); + Logger.getInstance().recordOutput("Localization/VisionPose", angleAdjustedVisionPose); + visionWorking = false; + } else if (LimelightHelpers.getTV("") == 1) { + Pose2d ntCurrentVisionPose = LimelightHelpers.getBotPose2d_wpiBlue(""); + if (previousPose.getX() != ntCurrentVisionPose.getX() + && previousPose.getY() != ntCurrentVisionPose.getY()) { + previousPose = ntCurrentVisionPose; + + LimelightResults results = LimelightHelpers.getLatestResults(""); + Pose2d angleAdjustedVisionPose = + new Pose2d(ntCurrentVisionPose.getTranslation(), imu.getRobotHeading()); + + double visionTimestamp = + Timer.getFPGATimestamp() + - ((results.targetingResults.latency_capture + + results.targetingResults.latency_jsonParse + + results.targetingResults.latency_pipeline) + / 1000); + + double averageDistanceToIndividualFiducialTags = 0; + double fiducialTagCount = 0; + + // Calculate average distance of each tag seen. + if (results.targetingResults.valid == 1 + && ntCurrentVisionPose.getX() != 0.0 + && ntCurrentVisionPose.getY() != 0.0) { + for (int i = 0; i < results.targetingResults.targets_Fiducials.length; ++i) { + Pose2d fiducialPose = + results.targetingResults.targets_Fiducials[i].getTargetPose_RobotSpace2D(); + double fiducialDistanceAway = + Math.sqrt(Math.pow(fiducialPose.getX(), 2) + Math.pow(fiducialPose.getY(), 2)); + averageDistanceToIndividualFiducialTags += fiducialDistanceAway; + fiducialTagCount++; + } + visionIsValid = true; + } + + // Update pose estimator if vision is valid. + if (visionIsValid) { + // Adjust vision measurement standard deviation by average distance from tags. + double stdForVision = + visionStdLookup.get(averageDistanceToIndividualFiducialTags) / fiducialTagCount; + try { + poseEstimator.addVisionMeasurement( + angleAdjustedVisionPose, + visionTimestamp, + VecBuilder.fill(stdForVision, stdForVision, Units.degreesToRadians(360))); + + } catch (Exception e) { + System.err.println("Pose estimator threw while adding vision measurement:"); + e.printStackTrace(); + } + Logger.getInstance().recordOutput("Localization/VisionPose", angleAdjustedVisionPose); + visionWorking = true; + } + } + } + + if (!visionIsValid) { + Logger.getInstance().recordOutput("Localization/VisionPose", new Pose2d()); + } + } + + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + public Pose2d getOdometryPose() { + return odometry.getPoseMeters(); + } + + public void resetPose(Pose2d pose) { + imu.setAngle(pose.getRotation()); + poseEstimator.resetPosition(pose.getRotation(), swerve.getModulePositions(), pose); + odometry.resetPosition(pose.getRotation(), swerve.getModulePositions(), pose); + } + + public void resetGyro(Rotation2d gyroAngle) { + Pose2d pose = new Pose2d(getPose().getTranslation(), gyroAngle); + resetPose(pose); + } + + public boolean isVisionWorking() { + return visionWorking; + } + + public Command getZeroCommand() { + return Commands.runOnce( + () -> resetGyro(Rotation2d.fromDegrees(FmsSubsystem.isRedAlliance() ? 180 : 0))); + } + + public Command getZeroAwayCommand() { + return Commands.runOnce( + () -> resetGyro(Rotation2d.fromDegrees(FmsSubsystem.isRedAlliance() ? 0 : 180))); + } + + public boolean atPose(Pose2d goal) { + + Pose2d pose = getPose(); + double distanceRelative = goal.getTranslation().getDistance(pose.getTranslation()); + + Logger.getInstance().recordOutput("Localization/AtPoseGoal", goal); + + Rotation2d rotationDifference = goal.getRotation().minus(pose.getRotation()); + if (distanceRelative < 0.2 && Math.abs(rotationDifference.getDegrees()) < 5) { + Logger.getInstance().recordOutput("Localization/AtPose", true); + return true; + } else { + Logger.getInstance().recordOutput("Localization/AtPose", false); + return false; + } + } +} diff --git a/src/main/java/frc/robot/localization/VisionMode.java b/src/main/java/frc/robot/localization/VisionMode.java new file mode 100644 index 0000000..2b7f379 --- /dev/null +++ b/src/main/java/frc/robot/localization/VisionMode.java @@ -0,0 +1,11 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.localization; + +public enum VisionMode { + OFF, + ENABLED_UNUSED, + FULLY_ENABLED; +} diff --git a/src/main/java/frc/robot/swerve/SwerveCorner.java b/src/main/java/frc/robot/swerve/SwerveCorner.java new file mode 100644 index 0000000..39d3450 --- /dev/null +++ b/src/main/java/frc/robot/swerve/SwerveCorner.java @@ -0,0 +1,12 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.swerve; + +public enum SwerveCorner { + FRONT_LEFT, + FRONT_RIGHT, + BACK_LEFT, + BACK_RIGHT; +} diff --git a/src/main/java/frc/robot/swerve/SwerveModule.java b/src/main/java/frc/robot/swerve/SwerveModule.java new file mode 100644 index 0000000..bd87783 --- /dev/null +++ b/src/main/java/frc/robot/swerve/SwerveModule.java @@ -0,0 +1,189 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.swerve; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import frc.robot.config.Config; +import frc.robot.util.CircleConverter; +import org.littletonrobotics.junction.Logger; +import frc.robot.util.CtreModuleState; + +public class SwerveModule { + private static final CircleConverter DRIVE_MOTOR_WHEEL_CONVERTER = + CircleConverter.fromDiameter(Config.WHEEL_DIAMETER); + + private final SwerveModuleConstants constants; + private final TalonFX driveMotor; + private final TalonFX steerMotor; + private final DutyCycleOut driveVoltageOpenLoopRequest = + new DutyCycleOut(0, Config.SWERVE_USE_FOC, true); + private final PositionVoltage steerMotorControl = + new PositionVoltage(0, Config.SWERVE_USE_FOC, 0, 0, false); + private final VelocityVoltage driveVoltageClosedLoopRequest = + new VelocityVoltage(0, Config.SWERVE_USE_FOC, 0, 0, false); + private Rotation2d previousAngle = new Rotation2d(); + + private StatusSignal driveMotorStatorCurrent; + + public SwerveModule( + SwerveModuleConstants constants, TalonFX driveMotor, TalonFX steerMotor, CANcoder encoder) { + this.constants = constants; + this.driveMotor = driveMotor; + this.steerMotor = steerMotor; + + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + + cancoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + cancoderConfig.MagnetSensor.MagnetOffset = constants.angleOffset.getRotations(); + encoder.getConfigurator().apply(cancoderConfig); + + TalonFXConfiguration driveMotorConfigs = new TalonFXConfiguration(); + + driveMotorConfigs.Slot0.kP = Config.SWERVE_DRIVE_KP; + driveMotorConfigs.Slot0.kI = Config.SWERVE_DRIVE_KI; + driveMotorConfigs.Slot0.kD = Config.SWERVE_DRIVE_KD; + driveMotorConfigs.Slot0.kV = Config.SWERVE_DRIVE_KV; + driveMotorConfigs.Slot0.kS = Config.SWERVE_DRIVE_KS; + + driveMotorConfigs.Voltage.PeakForwardVoltage = 12; + driveMotorConfigs.Voltage.PeakReverseVoltage = -12; + + CurrentLimitsConfigs currentLimits = new CurrentLimitsConfigs(); + currentLimits.SupplyCurrentLimit = 35; + currentLimits.SupplyCurrentLimitEnable = true; + + if (constants.driveInversion) { + driveMotorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } else { + driveMotorConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } + + StatusCode driveStatus = StatusCode.StatusCodeNotInitialized; + for (int i = 0; i < 5; ++i) { + driveStatus = driveMotor.getConfigurator().apply(driveMotorConfigs); + if (driveStatus.isOK()) break; + } + if (!driveStatus.isOK()) { + System.out.println("Could not apply configs, error code: " + driveStatus.toString()); + } + + TalonFXConfiguration steerMotorConfigs = new TalonFXConfiguration(); + steerMotorConfigs.ClosedLoopGeneral.ContinuousWrap = true; + + steerMotorConfigs.Slot0.kV = Config.SWERVE_STEER_KV; + steerMotorConfigs.Slot0.kP = Config.SWERVE_STEER_KP; + steerMotorConfigs.Slot0.kI = Config.SWERVE_STEER_KI; + steerMotorConfigs.Slot0.kD = Config.SWERVE_STEER_KD; + steerMotorConfigs.Slot0.kS = Config.SWERVE_STEER_KS; + + steerMotorConfigs.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); + steerMotorConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + steerMotorConfigs.Feedback.SensorToMechanismRatio = 1.0; + steerMotorConfigs.Feedback.RotorToSensorRatio = Config.SWERVE_STEER_GEARING_REDUCTION; + + steerMotorConfigs.CurrentLimits.SupplyCurrentLimit = 35; + steerMotorConfigs.CurrentLimits.SupplyCurrentLimitEnable = true; + steerMotorConfigs.MotorOutput.DutyCycleNeutralDeadband = 0; + + steerMotorConfigs.Feedback.SensorToMechanismRatio = Config.SWERVE_STEER_GEARING_REDUCTION; + + if (constants.angleInversion) { + steerMotorConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } else { + steerMotorConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } + + StatusCode steerStatus = StatusCode.StatusCodeNotInitialized; + for (int i = 0; i < 5; ++i) { + steerStatus = steerMotor.getConfigurator().apply(steerMotorConfigs); + if (steerStatus.isOK()) break; + } + if (!steerStatus.isOK()) { + System.out.println("Could not apply configs, error code: " + steerStatus.toString()); + } + + driveMotorStatorCurrent = driveMotor.getStatorCurrent(); + } + + public void log() { + Logger.getInstance() + .recordOutput( + "Swerve/" + constants.corner.toString() + "/DriveMotorStatorCurrent", + driveMotorStatorCurrent.refresh().getValue()); + } + + public void setDesiredState( + SwerveModuleState state, boolean openLoop, boolean skipJitterOptimization) { + final var steerMotorPosition = getSteerMotorPosition(); + state = CtreModuleState.optimize(state, steerMotorPosition); + + steerMotor.setControl(steerMotorControl.withPosition(state.angle.getRotations())); + + boolean isStopped = Math.abs(state.speedMetersPerSecond) <= SwerveSubsystem.MAX_VELOCITY * 0.01; + Rotation2d angle = isStopped && !skipJitterOptimization ? this.previousAngle : state.angle; + this.previousAngle = angle; + + var wheelRotationsPerSecond = + DRIVE_MOTOR_WHEEL_CONVERTER.distanceToRotations(state.speedMetersPerSecond); + + if (openLoop) { + driveMotor.setControl( + driveVoltageOpenLoopRequest.withOutput( + state.speedMetersPerSecond / SwerveSubsystem.MAX_VELOCITY)); + } else { + driveMotor.setControl(driveVoltageClosedLoopRequest.withVelocity(wheelRotationsPerSecond)); + } + } + + public SwerveModuleState getState() { + final var steerMotorPosition = getSteerMotorPosition(); + final var driveMotorVelocity = Units.inchesToMeters(getDriveMotorVelocity()); + + return new SwerveModuleState(driveMotorVelocity, steerMotorPosition); + } + + public SwerveModulePosition getPosition() { + final var steerMotorPosition = getSteerMotorPosition(); + final var driveMotorPosition = getDriveMotorPosition(); + + return new SwerveModulePosition(driveMotorPosition, steerMotorPosition); + } + + private Rotation2d getSteerMotorPosition() { + double rotations = steerMotor.getPosition().getValue(); + return Rotation2d.fromRotations(rotations); + } + + private double getDriveMotorPosition() { + final var rotations = driveMotor.getPosition().getValue(); + final var meters = DRIVE_MOTOR_WHEEL_CONVERTER.rotationsToDistance(rotations); + return meters; + } + + private double getDriveMotorVelocity() { + final var rotationsPerSecond = driveMotor.getVelocity().getValue(); + final var metersPerSecond = DRIVE_MOTOR_WHEEL_CONVERTER.rotationsToDistance(rotationsPerSecond); + final var inchesPerSecond = metersPerSecond * 39.37; + return inchesPerSecond; + } +} diff --git a/src/main/java/frc/robot/swerve/SwerveModuleConstants.java b/src/main/java/frc/robot/swerve/SwerveModuleConstants.java new file mode 100644 index 0000000..6c01ae3 --- /dev/null +++ b/src/main/java/frc/robot/swerve/SwerveModuleConstants.java @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.swerve; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class SwerveModuleConstants { + public final Rotation2d angleOffset; + public final SwerveCorner corner; + public final boolean angleInversion; + public final boolean driveInversion; + + public SwerveModuleConstants( + Rotation2d angleOffset, SwerveCorner corner, boolean angleInversion, boolean driveInversion) { + this.angleOffset = angleOffset; + this.corner = corner; + this.angleInversion = angleInversion; + this.driveInversion = driveInversion; + } +} diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java new file mode 100644 index 0000000..d9e65bc --- /dev/null +++ b/src/main/java/frc/robot/swerve/SwerveSubsystem.java @@ -0,0 +1,340 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.swerve; + +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.commands.PPSwerveControllerCommand; +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.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.config.Config; +import frc.robot.controller.DriveController; +import frc.robot.fms.FmsSubsystem; +import frc.robot.imu.ImuSubsystem; +import frc.robot.localization.LocalizationSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import org.littletonrobotics.junction.Logger; + +public class SwerveSubsystem extends LifecycleSubsystem { + private static final Translation2d FRONT_LEFT_LOCATION = Config.SWERVE_FRONT_LEFT_LOCATION; + private static final Translation2d FRONT_RIGHT_LOCATION = Config.SWERVE_FRONT_RIGHT_LOCATION; + private static final Translation2d BACK_LEFT_LOCATION = Config.SWERVE_BACK_LEFT_LOCATION; + private static final Translation2d BACK_RIGHT_LOCATION = Config.SWERVE_BACK_RIGHT_LOCATION; + public static final SwerveDriveKinematics KINEMATICS = + new SwerveDriveKinematics( + FRONT_LEFT_LOCATION, FRONT_RIGHT_LOCATION, BACK_LEFT_LOCATION, BACK_RIGHT_LOCATION); + public static final double MAX_VELOCITY = + ((6080.0 / 60.0) / Config.SWERVE_DRIVE_GEARING_REDUCTION) * (Config.WHEEL_DIAMETER * Math.PI); + public static final double MAX_ANGULAR_VELOCITY = 20; + + private final ImuSubsystem imu; + private final SwerveModule frontRight; + private final SwerveModule frontLeft; + private final SwerveModule backRight; + private final SwerveModule backLeft; + private boolean doneResetting = false; + + private boolean snapToAngle = false; + private boolean xSwerveEnabled = false; + + private final PIDController xController = + new PIDController( + Config.SWERVE_TRANSLATION_PID.kP, + Config.SWERVE_TRANSLATION_PID.kI, + Config.SWERVE_TRANSLATION_PID.kD); + private final PIDController yController = + new PIDController( + Config.SWERVE_TRANSLATION_PID.kP, + Config.SWERVE_TRANSLATION_PID.kI, + Config.SWERVE_TRANSLATION_PID.kD); + private final PIDController thetaController = + new PIDController( + Config.SWERVE_ROTATION_PID.kP, + Config.SWERVE_ROTATION_PID.kI, + Config.SWERVE_ROTATION_PID.kD); + private final PIDController snapThetaController = + new PIDController( + Config.SWERVE_ROTATION_SNAP_PID.kP, + Config.SWERVE_ROTATION_SNAP_PID.kI, + Config.SWERVE_ROTATION_SNAP_PID.kD); + + private final ProfiledPIDController xProfiledController = + new ProfiledPIDController( + Config.SWERVE_TRANSLATION_PID.kP, + Config.SWERVE_TRANSLATION_PID.kI, + Config.SWERVE_TRANSLATION_PID.kD, + new TrapezoidProfile.Constraints(2.0, 1.5)); + private final ProfiledPIDController yProfiledController = + new ProfiledPIDController( + Config.SWERVE_TRANSLATION_PID.kP, + Config.SWERVE_TRANSLATION_PID.kI, + Config.SWERVE_TRANSLATION_PID.kD, + new TrapezoidProfile.Constraints(2.0, 1.5)); + private final ProfiledPIDController thetaProfiledController = + new ProfiledPIDController( + Config.SWERVE_ROTATION_PID.kP, + Config.SWERVE_ROTATION_PID.kI, + Config.SWERVE_ROTATION_PID.kD, + new TrapezoidProfile.Constraints(Math.PI * 2.0, Math.PI * 0.75)); + private Rotation2d goalAngle = new Rotation2d(); + + public SwerveSubsystem( + ImuSubsystem imu, + SwerveModule frontRight, + SwerveModule frontLeft, + SwerveModule backRight, + SwerveModule backLeft) { + super(SubsystemPriority.SWERVE); + + this.imu = imu; + this.frontRight = frontRight; + this.frontLeft = frontLeft; + this.backRight = backRight; + this.backLeft = backLeft; + + thetaController.enableContinuousInput(-Math.PI, Math.PI); + snapThetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaProfiledController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void robotPeriodic() { + frontLeft.log(); + frontRight.log(); + backLeft.log(); + backRight.log(); + + Logger.getInstance().recordOutput("Swerve/ModuleStates", getModuleStates()); + + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/X", chassisSpeeds.vxMetersPerSecond); + Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/Y", chassisSpeeds.vyMetersPerSecond); + Logger.getInstance() + .recordOutput("Swerve/ChassisSpeeds/Omega", chassisSpeeds.omegaRadiansPerSecond); + + Translation2d t = + new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + double velocity = t.getDistance(new Translation2d()); + Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/theta", t.getAngle().getRadians()); + Logger.getInstance().recordOutput("Swerve/ChassisSpeeds/velocity", velocity); + Logger.getInstance() + .recordOutput( + "Swerve/ChassisSpeeds/spinRatio", chassisSpeeds.omegaRadiansPerSecond / velocity); + + Logger.getInstance().recordOutput("Swerve/SnapToAngle/Goal", goalAngle.getDegrees()); + Logger.getInstance().recordOutput("Swerve/SnapToAngle/Enabled", snapToAngle); + } + + @Override + public void enabledPeriodic() { + if (xSwerveEnabled) { + xSwerve(); + } + } + + public ChassisSpeeds getChassisSpeeds() { + final var frontLeftState = frontLeft.getState(); + final var frontRightState = frontRight.getState(); + final var backLeftState = backLeft.getState(); + final var backRightState = backRight.getState(); + + return KINEMATICS.toChassisSpeeds( + frontLeftState, frontRightState, backLeftState, backRightState); + } + + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + frontLeft.getState(), frontRight.getState(), backLeft.getState(), backRight.getState() + }; + } + + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition() + }; + } + + public void setSnapToAngle(Rotation2d angle) { + goalAngle = angle; + snapToAngle = true; + } + + public void disableSnapToAngle() { + snapToAngle = false; + } + + public void setXSwerve(boolean xSwerve) { + this.xSwerveEnabled = xSwerve; + } + + public void setChassisSpeeds(ChassisSpeeds speeds, boolean openLoop) { + if (xSwerveEnabled) { + return; + } + + if (snapToAngle) { + speeds.omegaRadiansPerSecond = + snapThetaController.calculate(imu.getRobotHeading().getRadians(), goalAngle.getRadians()); + } + Logger.getInstance().recordOutput("Swerve/CommandedSpeeds/X", speeds.vxMetersPerSecond); + Logger.getInstance().recordOutput("Swerve/CommandedSpeeds/Y", speeds.vyMetersPerSecond); + Logger.getInstance().recordOutput("Swerve/CommandedSpeeds/Omega", speeds.omegaRadiansPerSecond); + + // Twist computation. + double lookAheadSeconds = 0.1; + Pose2d target_pose = + new Pose2d( + lookAheadSeconds * speeds.vxMetersPerSecond, + lookAheadSeconds * speeds.vyMetersPerSecond, + Rotation2d.fromRadians(lookAheadSeconds * speeds.omegaRadiansPerSecond)); + Twist2d twist = (new Pose2d()).log(target_pose); + speeds.vxMetersPerSecond = twist.dx / lookAheadSeconds; + speeds.vyMetersPerSecond = twist.dy / lookAheadSeconds; + speeds.omegaRadiansPerSecond = twist.dtheta / lookAheadSeconds; // omega should stay the same. + // Kinematics to convert target chassis speeds to module states. + final var moduleStates = KINEMATICS.toSwerveModuleStates(speeds); + setModuleStates(moduleStates, openLoop, false); + } + + public void setModuleStates( + SwerveModuleState[] moduleStates, boolean openLoop, boolean skipJitterOptimization) { + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, MAX_VELOCITY); + Logger.getInstance().recordOutput("Swerve/GoalModuleStates", moduleStates); + frontLeft.setDesiredState(moduleStates[0], openLoop, skipJitterOptimization); + frontRight.setDesiredState(moduleStates[1], openLoop, skipJitterOptimization); + backLeft.setDesiredState(moduleStates[2], openLoop, skipJitterOptimization); + backRight.setDesiredState(moduleStates[3], openLoop, skipJitterOptimization); + } + + public void xSwerve() { + setModuleStates( + new SwerveModuleState[] { + new SwerveModuleState(0.0, Rotation2d.fromDegrees(45)), + new SwerveModuleState(0.0, Rotation2d.fromDegrees(135)), + new SwerveModuleState(0.0, Rotation2d.fromDegrees(135)), + new SwerveModuleState(0.0, Rotation2d.fromDegrees(45)) + }, + true, + true); + } + + public Command getXSwerveCommand() { + return run(() -> setXSwerve(true)); + } + + public Command disableXSwerveCommand() { + return runOnce(() -> setXSwerve(false)); + } + + public void driveTeleop( + double sidewaysPercentage, + double forwardPercentage, + double thetaPercentage, + boolean fieldRelative, + boolean openLoop) { + Translation2d robotTranslation = + new Translation2d(forwardPercentage, sidewaysPercentage).times(MAX_VELOCITY); + Rotation2d fieldRelativeHeading = imu.getRobotHeading(); + + if (FmsSubsystem.isRedAlliance()) { + fieldRelativeHeading = fieldRelativeHeading.plus(Rotation2d.fromDegrees(180)); + } + + ChassisSpeeds chassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + robotTranslation.getX(), + robotTranslation.getY(), + thetaPercentage * MAX_ANGULAR_VELOCITY, + fieldRelative ? fieldRelativeHeading : new Rotation2d()); + SwerveModuleState[] moduleStates = KINEMATICS.toSwerveModuleStates(chassisSpeeds); + setChassisSpeeds(KINEMATICS.toChassisSpeeds(moduleStates), openLoop); + } + + public Command getDriveTeleopCommand(DriveController controller) { + return Commands.run( + () -> { + if (!DriverStation.isTeleopEnabled()) { + return; + } + + boolean openLoop = true; + + driveTeleop( + -controller.getSidewaysPercentage(), + controller.getForwardPercentage(), + controller.getThetaPercentage(), + true, + openLoop); + }, + this) + .withName("SwerveDriveTeleop"); + } + + public Command getFollowTrajectoryCommand( + PathPlannerTrajectory traj, LocalizationSubsystem localization) { + return new PPSwerveControllerCommand( + traj, + localization::getPose, + SwerveSubsystem.KINEMATICS, + // x controller + xController, + // y controller + yController, + // theta controller + thetaController, + states -> setModuleStates(states, false, false), + false, + this) + .withName("SwerveFollowTrajectory"); + } + + // Create a command that accepts a Pose2d and drives to it using a PPHolonomicDriveController + // The command should exit once it's at the pose + public Command goToPoseCommand(Pose2d goal, LocalizationSubsystem localization) { + return run(() -> { + Logger.getInstance().recordOutput("AutoAlign/TargetPose", goal); + Pose2d pose = localization.getPose(); + double xVelocity = xProfiledController.calculate(pose.getX(), goal.getX()); + double yVelocity = yProfiledController.calculate(pose.getY(), goal.getY()); + double thetaVelocity = + thetaProfiledController.calculate( + pose.getRotation().getRadians(), goal.getRotation().getRadians()); + + ChassisSpeeds chassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + xVelocity, yVelocity, thetaVelocity, pose.getRotation()); + + setChassisSpeeds(chassisSpeeds, false); + }) + .until( + () -> { + // 3 degree rotation and 0.1 meter distance + Pose2d pose = localization.getPose(); + double distanceRelative = goal.getTranslation().getDistance(pose.getTranslation()); + Rotation2d rotationDifference = goal.getRotation().minus(pose.getRotation()); + if (distanceRelative < 0.1 && Math.abs(rotationDifference.getDegrees()) < 3) { + return true; + } else { + return false; + } + }) + .withName("SwerveGoToPose"); + } +} diff --git a/src/main/java/frc/robot/util/CircleConverter.java b/src/main/java/frc/robot/util/CircleConverter.java new file mode 100644 index 0000000..7df9f8f --- /dev/null +++ b/src/main/java/frc/robot/util/CircleConverter.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class CircleConverter { + public static CircleConverter fromRadius(double radius) { + return new CircleConverter(radius); + } + + public static CircleConverter fromDiameter(double diameter) { + return new CircleConverter(diameter / 2); + } + + private final double radius; + + private CircleConverter(double radius) { + this.radius = radius; + } + + /** Converts a rotation to a distance. */ + public double rotationsToDistance(double rotationsPerMinute) { + return Units.rotationsToRadians(rotationsPerMinute) * radius; + } + + /** Converts a rotation to a distance. */ + public double rotationsToDistance(Rotation2d rotation) { + return rotationsToDistance(Units.radiansToRotations(rotation.getRadians())); + } + + /** Converts a distance to a rotation. */ + public double distanceToRotations(double distance) { + return Units.radiansToRotations(distance / radius); + } +} diff --git a/src/main/java/frc/robot/util/CtreModuleState.java b/src/main/java/frc/robot/util/CtreModuleState.java new file mode 100644 index 0000000..3ba8c3f --- /dev/null +++ b/src/main/java/frc/robot/util/CtreModuleState.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public class CtreModuleState { + + /** + * Minimize the change in heading the desired swerve module state would require by potentially + * reversing the direction the wheel spins. Customized from WPILib's version to include placing in + * appropriate scope for CTRE onboard control. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + */ + public static SwerveModuleState optimize( + SwerveModuleState desiredState, Rotation2d currentAngle) { + double targetAngle = + placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees()); + double targetSpeed = desiredState.speedMetersPerSecond; + double delta = targetAngle - currentAngle.getDegrees(); + if (Math.abs(delta) > 90) { + targetSpeed = -targetSpeed; + targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180); + } + return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); + } + + /** + * @param scopeReference Current Angle + * @param newAngle Target Angle + * @return Closest angle within scope + */ + private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { + double lowerBound; + double upperBound; + double lowerOffset = scopeReference % 360; + if (lowerOffset >= 0) { + lowerBound = scopeReference - lowerOffset; + upperBound = scopeReference + (360 - lowerOffset); + } else { + upperBound = scopeReference - lowerOffset; + lowerBound = scopeReference - (360 + lowerOffset); + } + while (newAngle < lowerBound) { + newAngle += 360; + } + while (newAngle > upperBound) { + newAngle -= 360; + } + if (newAngle - scopeReference > 180) { + newAngle -= 360; + } else if (newAngle - scopeReference < -180) { + newAngle += 360; + } + return newAngle; + } +} diff --git a/vendordeps/PhoenixProAnd5.json b/vendordeps/Phoenix6And5.json similarity index 81% rename from vendordeps/PhoenixProAnd5.json rename to vendordeps/Phoenix6And5.json index 327679f..4862b90 100644 --- a/vendordeps/PhoenixProAnd5.json +++ b/vendordeps/Phoenix6And5.json @@ -3,23 +3,23 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.30.4" + "version": "5.31.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.30.4" + "version": "5.31.0" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "23.0.12" + "version": "23.2.2" } ], - "fileName": "PhoenixProAnd5.json", + "fileName": "Phoenix6And5.json", "frcYear": 2023, - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", - "name": "CTRE-Phoenix (Pro And v5)", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6And5-frc2023-latest.json", + "name": "CTRE-Phoenix (v6 And v5)", "jniDependencies": [ { "simMode": "hwsim", @@ -32,7 +32,7 @@ "artifactId": "cci", "skipInvalidPlatforms": true, "isJar": false, - "version": "5.30.4" + "version": "5.31.0" }, { "simMode": "swsim", @@ -45,7 +45,7 @@ "artifactId": "cci-sim", "skipInvalidPlatforms": true, "isJar": false, - "version": "5.30.4" + "version": "5.31.0" }, { "simMode": "hwsim", @@ -54,11 +54,11 @@ "linuxx86-64", "linuxathena" ], - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -67,11 +67,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -80,11 +80,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -93,11 +93,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -106,11 +106,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -119,11 +119,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -132,11 +132,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -145,11 +145,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -158,11 +158,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" }, { "simMode": "swsim", @@ -171,11 +171,11 @@ "linuxx86-64", "osxuniversal" ], - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", "skipInvalidPlatforms": true, "isJar": false, - "version": "23.0.12" + "version": "23.2.2" } ], "mavenUrls": ["https://maven.ctr-electronics.com/release/"], @@ -186,7 +186,7 @@ "libName": "CTRE_Phoenix_WPI", "artifactId": "wpiapi-cpp", "skipInvalidPlatforms": true, - "version": "5.30.4", + "version": "5.31.0", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -201,7 +201,7 @@ "libName": "CTRE_Phoenix", "artifactId": "api-cpp", "skipInvalidPlatforms": true, - "version": "5.30.4", + "version": "5.31.0", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -216,7 +216,7 @@ "libName": "CTRE_PhoenixCCI", "artifactId": "cci", "skipInvalidPlatforms": true, - "version": "5.30.4", + "version": "5.31.0", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -227,11 +227,11 @@ }, { "simMode": "hwsim", - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "libName": "CTRE_PhoenixTools", "artifactId": "tools", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -246,7 +246,7 @@ "libName": "CTRE_Phoenix_WPISim", "artifactId": "wpiapi-cpp-sim", "skipInvalidPlatforms": true, - "version": "5.30.4", + "version": "5.31.0", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -261,7 +261,7 @@ "libName": "CTRE_PhoenixSim", "artifactId": "api-cpp-sim", "skipInvalidPlatforms": true, - "version": "5.30.4", + "version": "5.31.0", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -276,7 +276,7 @@ "libName": "CTRE_PhoenixCCISim", "artifactId": "cci-sim", "skipInvalidPlatforms": true, - "version": "5.30.4", + "version": "5.31.0", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -287,11 +287,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_PhoenixTools_Sim", "artifactId": "tools-sim", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -302,11 +302,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimTalonSRX", "artifactId": "simTalonSRX", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -317,11 +317,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimTalonFX", "artifactId": "simTalonFX", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -332,11 +332,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimVictorSPX", "artifactId": "simVictorSPX", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -347,11 +347,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimPigeonIMU", "artifactId": "simPigeonIMU", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -362,11 +362,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimCANCoder", "artifactId": "simCANCoder", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -377,11 +377,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimProTalonFX", "artifactId": "simProTalonFX", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -392,11 +392,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimProCANcoder", "artifactId": "simProCANcoder", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -407,11 +407,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "libName": "CTRE_SimProPigeon2", "artifactId": "simProPigeon2", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -422,11 +422,11 @@ }, { "simMode": "hwsim", - "groupId": "com.ctre.phoenixpro", - "libName": "CTRE_PhoenixPro_WPI", + "groupId": "com.ctre.phoenix6", + "libName": "CTRE_Phoenix6_WPI", "artifactId": "wpiapi-cpp", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -437,11 +437,11 @@ }, { "simMode": "swsim", - "groupId": "com.ctre.phoenixpro.sim", - "libName": "CTRE_PhoenixPro_WPISim", + "groupId": "com.ctre.phoenix6.sim", + "libName": "CTRE_Phoenix6_WPISim", "artifactId": "wpiapi-cpp-sim", "skipInvalidPlatforms": true, - "version": "23.0.12", + "version": "23.2.2", "headerClassifier": "headers", "binaryPlatforms": [ "windowsx86-64", @@ -451,6 +451,6 @@ "sharedLibrary": true } ], - "version": "23.0.12", + "version": "23.2.2", "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a" }