From 0d1114904b32c2ee24524a86fd26171103b00423 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 25 Jan 2025 04:27:10 -0500 Subject: [PATCH] Da Code --- .../frc/robot/{constants => }/Constants.java | 5 +- src/main/java/frc/robot/FieldConstants.java | 203 ++++++++++++++ src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/commands/DriveCommands.java | 42 +-- .../frc/robot/subsystems/drive/DriveBase.java | 251 ++++++++++++++++-- .../subsystems/drive/DriveConstants.java | 91 +++++++ .../frc/robot/subsystems/drive/GyroIO.java | 18 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 48 +++- .../frc/robot/subsystems/drive/Module.java | 137 +++++++++- .../frc/robot/subsystems/drive/ModuleIO.java | 49 +++- .../robot/subsystems/drive/ModuleIOSpark.java | 213 ++++++++++++++- .../subsystems/drive/OdometryManager.java | 76 ++++-- .../java/frc/robot/util/AllianceFlipUtil.java | 99 +++++++ .../frc/robot/util/LoggedTunableNumber.java | 2 +- src/main/java/frc/robot/util/LoggerUtil.java | 2 +- .../util/swerve/SwerveSetpointGenerator.java | 3 +- vendordeps/REVLib-2025.json | 12 +- 17 files changed, 1172 insertions(+), 80 deletions(-) rename src/main/java/frc/robot/{constants => }/Constants.java (91%) create mode 100644 src/main/java/frc/robot/FieldConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/Constants.java similarity index 91% rename from src/main/java/frc/robot/constants/Constants.java rename to src/main/java/frc/robot/Constants.java index ab9a0c7..ea50321 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,10 +1,10 @@ -package frc.robot.constants; +package frc.robot; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; public class Constants { - private static RobotType kRobotType = RobotType.ROBOT_SIMBOT; + private static RobotType kRobotType = RobotType.ROBOT_2025_COMP; // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector public static final boolean TUNING_MODE = false; // Disable the AdvantageKit logger from running @@ -20,7 +20,6 @@ public enum RobotMode { public enum RobotType { ROBOT_2025_COMP, - ROBOT_2024_OFFSEASON, ROBOT_SIMBOT } diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..aad1e00 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,203 @@ +package frc.robot; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Contains various field dimensions and useful reference points. All units are in meters and poses + * have a blue alliance origin. + */ +public class FieldConstants { + public static final double fieldLength = Units.inchesToMeters(690.876); + public static final double fieldWidth = Units.inchesToMeters(317); + public static final double startingLineX = + Units.inchesToMeters(299.438); // Measured from the inside of starting line + + public static class Processor { + public static final Pose2d centerFace = + new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90)); + } + + public static class Barge { + public static final Translation2d farCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); + public static final Translation2d middleCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855)); + public static final Translation2d closeCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); + + // Measured from floor to bottom of cage + public static final double deepHeight = Units.inchesToMeters(3.125); + public static final double shallowHeight = Units.inchesToMeters(30.125); + } + + public static class CoralStation { + public static final Pose2d leftCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176), + Rotation2d.fromDegrees(90 - 144.011)); + public static final Pose2d rightCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + } + + public static class Reef { + public static final Translation2d center = + new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + public static final double faceToZoneLine = + Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line + + public static final Pose2d[] centerFaces = + new Pose2d[6]; // Starting facing the driver station in clockwise order + public static final List> branchPositions = + new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise + + static { + // Initialize faces + centerFaces[0] = + new Pose2d( + Units.inchesToMeters(144.003), + Units.inchesToMeters(158.500), + Rotation2d.fromDegrees(180)); + centerFaces[1] = + new Pose2d( + Units.inchesToMeters(160.373), + Units.inchesToMeters(186.857), + Rotation2d.fromDegrees(120)); + centerFaces[2] = + new Pose2d( + Units.inchesToMeters(193.116), + Units.inchesToMeters(186.858), + Rotation2d.fromDegrees(60)); + centerFaces[3] = + new Pose2d( + Units.inchesToMeters(209.489), + Units.inchesToMeters(158.502), + Rotation2d.fromDegrees(0)); + centerFaces[4] = + new Pose2d( + Units.inchesToMeters(193.118), + Units.inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)); + centerFaces[5] = + new Pose2d( + Units.inchesToMeters(160.375), + Units.inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)); + + // Initialize branch positions + for (int face = 0; face < 6; face++) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + for (var level : ReefHeight.values()) { + Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face))); + double adjustX = Units.inchesToMeters(30.738); + double adjustY = Units.inchesToMeters(6.469); + + fillRight.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + fillLeft.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + } + branchPositions.add((face * 2) + 1, fillRight); + branchPositions.add((face * 2) + 2, fillLeft); + } + } + } + + public static class StagingPositions { + // Measured from the center of the ice cream + public static final Pose2d leftIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d()); + public static final Pose2d middleIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); + public static final Pose2d rightIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); + } + + public enum ReefHeight { + L4(Units.inchesToMeters(72), -90), + L3(Units.inchesToMeters(47.625), -35), + L2(Units.inchesToMeters(31.875), -35), + L1(Units.inchesToMeters(18), 0); + + ReefHeight(double height, double pitch) { + this.height = height; + this.pitch = pitch; // in degrees + } + + public final double height; + public final double pitch; + } + + // TODO + // public static final double aprilTagWidth = Units.inchesToMeters(6.50); + // public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + // public static final int aprilTagCount = 22; + // + // @Getter + // public enum AprilTagLayoutType { + // OFFICIAL("2025-official"); + // + // AprilTagLayoutType(String name) { + // if (Constants.disableHAL) { + // layout = null; + // } else { + // try { + // layout = + // new AprilTagFieldLayout( + // Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + + // ".json")); + // } catch (IOException e) { + // throw new RuntimeException(e); + // } + // } + // if (layout == null) { + // layoutString = ""; + // } else { + // try { + // layoutString = new ObjectMapper().writeValueAsString(layout); + // } catch (JsonProcessingException e) { + // throw new RuntimeException( + // "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + // } + // } + // } + // + // private final AprilTagFieldLayout layout; + // private final String layoutString; + // } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d96407c..0981370 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.constants.Constants; import frc.robot.util.LoggerUtil; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index be28a5b..4027666 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -10,7 +10,8 @@ public class DriveCommands { /** - * Field relative drive command using two joysticks (controlling linear and angular velocities). + * Field relative drive command using two joysticks (controlling linear and angular +velocities). */ public static Command joystickDrive( DriveBase driveBase, @@ -20,22 +21,25 @@ public static Command joystickDrive( return Commands.run(() -> {}, driveBase); } - // /** - // * Field relative drive command using joystick for linear control and PID for angular control. - // * Possible use cases include snapping to an angle, aiming at a vision target, or controlling - // * absolute rotation with a joystick. - // */ - // public static Command joystickDriveAtAngle() {} - - /** Measures the velocity feedforward constants for the drive motors. */ - public static Command feedforwardCharacterization() { - List velocitySamples = new LinkedList<>(); - List voltageSamples = new LinkedList<>(); - Timer timer = new Timer(); - - return Commands.sequence(); - } - - // /** Measures the robot's wheel radius by spinning in a circle. */ - // public static Command wheelRadiusCharacterization() {} +// +// // /** +// // * Field relative drive command using joystick for linear control and PID for angular +// control. +// // * Possible use cases include snapping to an angle, aiming at a vision target, or +// controlling +// // * absolute rotation with a joystick. +// // */ +// // public static Command joystickDriveAtAngle() {} +// +// /** Measures the velocity feedforward constants for the drive motors. */ +// public static Command feedforwardCharacterization() { +// List velocitySamples = new LinkedList<>(); +// List voltageSamples = new LinkedList<>(); +// Timer timer = new Timer(); +// +// return Commands.sequence(); +// } +// +// // /** Measures the robot's wheel radius by spinning in a circle. */ +// // public static Command wheelRadiusCharacterization() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index eb2fa70..929af19 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,23 +1,240 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.util.swerve.SwerveSetpointGenerator; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class DriveBase extends SubsystemBase { - // private final GyroIO gyroIO; - // private final Module[] modules = new Module[4]; // FL, FR, BL, BR - // private final Alert gyroDisconnectedAlert = - // new Alert("Disconnected gyro, using kinematic approximation as fallback.", - // Alert.AlertType.kError); - // - // - // public static Translation2d[] getModuleTranslations(double trackWidthX, double trackWidthY) { - // return new Translation2d[] { - // new Translation2d(trackWidthX / 2.0, trackWidthY / 2.0), - // new Translation2d(trackWidthX / 2.0, -trackWidthY / 2.0), - // new Translation2d(-trackWidthX / 2.0, trackWidthY / 2.0), - // new Translation2d(-trackWidthX / 2.0, -trackWidthY / 2.0) - // }; - // } - - public void test() {} + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final OdometryTimestampsInputAutoLogged m_timestampInputs = new OdometryTimestampsInputAutoLogged(); + private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematic approximation as fallback.", Alert.AlertType.kError); + + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(DriveConstants.moduleTranslations); + + private SwerveSetpointGenerator.SwerveSetpoint currentSetpoint = + new SwerveSetpointGenerator.SwerveSetpoint( + new ChassisSpeeds(), + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }); + private final SwerveSetpointGenerator swerveSetpointGenerator; + + @AutoLogOutput(key = "Drive/VelocityMode") + private boolean VELOCITY_MODE = false; + + @AutoLogOutput(key = "Drive/BrakeModeEnabled") + private boolean BRAKE_MODE = true; + + public DriveBase( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + swerveSetpointGenerator = + new SwerveSetpointGenerator(kinematics, DriveConstants.moduleTranslations); + + // Start odometry thread + OdometryManager.getInstance().start(); + } + + @Override + public void periodic() { + OdometryManager.odometryLock.lock(); + try { + gyroIO.updateInputs(m_gyroInputs); + Logger.processInputs("Drive/Gyro", m_gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + m_timestampInputs.timestamps = + OdometryManager.getInstance().getTimestampQueue().stream() + .mapToDouble((Double value) -> value) + .toArray(); + OdometryManager.getInstance().getTimestampQueue().clear(); + Logger.processInputs("Drive/OdometryTimestamps", m_timestampInputs); + } finally { + OdometryManager.odometryLock.unlock(); + } + + // Call periodic on modules + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsUnoptimized", new SwerveModuleState[] {}); + } + + var timestamps = m_timestampInputs.timestamps; + var sampleCount = timestamps.length; + + for(int i = 0; i < sampleCount; i++) { + SwerveModulePosition[] wheelPositions = new SwerveModulePosition[4]; + for (int j = 0; j < 4; j++) { + wheelPositions[j] = modules[j].getOdometryPositions()[i]; + } + + Rotation2d gyroAngle; + if(m_gyroInputs.connected) { + gyroAngle = m_gyroInputs.odometryYawPositions[i]; + } else { + + } + + // TODO Update Odometry + } + + + // Update current setpoint if not in velocity mode + if (!VELOCITY_MODE) { + currentSetpoint = new SwerveSetpointGenerator.SwerveSetpoint(getChassisSpeeds(), getModuleStates()); + } + + // Update gyro alert + gyroDisconnectedAlert.set(!m_gyroInputs.connected && Constants.getRobotMode() != Constants.RobotMode.SIM); + } + + /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */ + private void setBrakeMode(boolean enabled) { + if (BRAKE_MODE != enabled) { + for(var module : modules) { + module.setDriveBrakeMode(enabled); + } + } + BRAKE_MODE = enabled; + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + VELOCITY_MODE = true; + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.kLoopPeriodSecs); + SwerveModuleState[] setpointStatesUnoptimized = kinematics.toSwerveModuleStates(discreteSpeeds); + currentSetpoint = + swerveSetpointGenerator.generateSetpoint( + DriveConstants.moduleLimitsFree, + currentSetpoint, + discreteSpeeds, + Constants.kLoopPeriodSecs); + SwerveModuleState[] setpointStates = currentSetpoint.moduleStates(); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/SetpointsUnoptimized", setpointStatesUnoptimized); + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", currentSetpoint.chassisSpeeds()); + + // Send setpoints to modules + for (int i = 0; i < 4; i++) { + modules[i].runSetpoint(setpointStates[i]); + } + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + VELOCITY_MODE = false; + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = DriveConstants.moduleTranslations[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns the module states (turn angles and drive velocities) for all the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + /** Returns the raw gyro rotation read by the IMU */ + public Rotation2d getGyroRotation() { + return m_gyroInputs.yawPosition; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..8e9fab1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,91 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.util.swerve.SwerveSetpointGenerator.ModuleLimits; +import lombok.Builder; + +public class DriveConstants { + public static final double odometryFrequencyHz = 250; + + public static final double trackWidthX = Units.inchesToMeters(20.75); + public static final double trackWidthY = Units.inchesToMeters(20.75); + public static final double driveBaseRadius = Math.hypot(trackWidthX / 2, trackWidthY / 2); + + public static final double maxLinearVelocityMetersPerSec = Units.feetToMeters(15.1); + public static final double maxLinearAccelerationMetersPerSecSquared = Units.feetToMeters(75.0); + public static final double maxAngularSpeedRadPerSec = + maxLinearVelocityMetersPerSec / driveBaseRadius; + public static final double maxAngularAccelerationRadPerSecSquared = 2.0 * Math.PI; + + public static final Translation2d[] moduleTranslations = { + new Translation2d(trackWidthX / 2, trackWidthY / 2), + new Translation2d(trackWidthX / 2, -trackWidthY / 2), + new Translation2d(-trackWidthX / 2, trackWidthY / 2), + new Translation2d(-trackWidthX / 2, -trackWidthY / 2) + }; + + public static final double wheelRadius = Units.inchesToMeters(2.0); + + private static final double mk4iDriveGearing = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double mk4iTurnGearing = (150.0 / 7.0); + + public static final ModuleConfig[] moduleConfigs = { + // FL + ModuleConfig.builder() + .turnMotorId(2) + .driveMotorId(3) + .encoderChannel(2) + .encoderOffset(Rotation2d.fromRadians(0.0)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .build(), + // FR + ModuleConfig.builder() + .turnMotorId(4) + .driveMotorId(5) + .encoderChannel(3) + .encoderOffset(Rotation2d.fromRadians(0.0)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .build(), + // BL + ModuleConfig.builder() + .turnMotorId(6) + .driveMotorId(7) + .encoderChannel(1) + .encoderOffset(Rotation2d.fromRadians(0.0)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .build(), + // BR + ModuleConfig.builder() + .turnMotorId(8) + .driveMotorId(9) + .encoderChannel(0) + .encoderOffset(Rotation2d.fromRadians(0.0)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .build(), + }; + + public static class PigeonConstants { + public static final int id = 10; + } + + @Builder + public record ModuleConfig( + int turnMotorId, + int driveMotorId, + int encoderChannel, + Rotation2d encoderOffset, + double driveGearing, + double turnGearing) {} + + public static final ModuleLimits moduleLimitsFree = + new ModuleLimits( + maxLinearVelocityMetersPerSec, + maxLinearAccelerationMetersPerSecSquared, + Units.degreesToRadians(1080.0)); +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 7fe49af..8adc971 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -1,3 +1,19 @@ package frc.robot.subsystems.drive; -public interface GyroIO {} +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 230d0c3..4801ab0 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -1,3 +1,49 @@ package frc.robot.subsystems.drive; -public class GyroIOPigeon2 {} +import static frc.robot.subsystems.drive.DriveConstants.PigeonConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import java.util.Queue; + +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 m_gyro = new Pigeon2(id); + + private final StatusSignal yaw; + private final StatusSignal yawVelocity; + + private final Queue yawPositionQueue; + + public GyroIOPigeon2() { + m_gyro.getConfigurator().apply(new Pigeon2Configuration()); + m_gyro.getConfigurator().setYaw(0.0); + + yaw = m_gyro.getYaw(); + yawVelocity = m_gyro.getAngularVelocityZWorld(); + + yaw.setUpdateFrequency(DriveConstants.odometryFrequencyHz); + yawVelocity.setUpdateFrequency(50.0); + m_gyro.optimizeBusUtilization(); + + yawPositionQueue = + OdometryManager.getInstance().registerSignal(() -> m_gyro.getYaw().getValueAsDouble()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawPositions = + yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 45bb17c..b5909f2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,3 +1,138 @@ package frc.robot.subsystems.drive; -public class Module {} +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.Constants; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import org.littletonrobotics.junction.Logger; + +public class Module { + private static final LoggedTunableNumber drivekS = new LoggedTunableNumber("Drive/Module/DrivekS"); + private static final LoggedTunableNumber drivekV = new LoggedTunableNumber("Drive/Module/DrivekV"); + private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("Drive/Module/DrivekP"); + private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("Drive/Module/DrivekD"); + private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); + private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); + + static { + switch (Constants.getRobotType()) { + case ROBOT_2025_COMP -> { + drivekS.initDefault(0.0); // TODO + drivekV.initDefault(0.0); // TODO + drivekP.initDefault(0.0); // TODO + drivekD.initDefault(0.0); // TODO + turnkP.initDefault(4000.0); + turnkD.initDefault(50.0); + } + default -> { + drivekS.initDefault(0.0); // TODO + drivekV.initDefault(0.0); // TODO + drivekP.initDefault(0.0); // TODO + drivekD.initDefault(0.0); // TODO + turnkP.initDefault(0.0); // TODO + turnkD.initDefault(0.0); // TODO + } + } + } + + private final ModuleIO m_io; + private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + @Getter + private SwerveModulePosition[] odometryPositions; + + public Module(ModuleIO io, int index) { + m_io = io; + this.index = index; + } + + public void updateInputs() { + m_io.updateInputs(m_inputs); + Logger.processInputs("Drive/Module" + index, m_inputs); + } + + public void periodic() { + // Update tunable numbers + if (drivekS.hasChanged(hashCode()) || drivekV.hasChanged(hashCode())) { + m_io.setDriveFF(drivekS.get(), drivekV.get()); + } + if (drivekP.hasChanged(hashCode()) || drivekD.hasChanged(hashCode())) { + m_io.setDrivePID(drivekP.get(), 0, drivekD.get()); + } + if (turnkP.hasChanged(hashCode()) || turnkD.hasChanged(hashCode())) { + m_io.setTurnPID(turnkP.get(), 0, turnkD.get()); + } + + // Update Odometry Positions + int sampleCount = m_inputs.odometryDrivePositionsRad.length; + odometryPositions = new SwerveModulePosition[sampleCount]; + for(int i = 0; i < sampleCount; i++) { + double positionMeters = m_inputs.odometryDrivePositionsRad[i] * DriveConstants.wheelRadius; + Rotation2d angle = m_inputs.odometryTurnPositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Runs the module with the specified setpoint state. */ + public void runSetpoint(SwerveModuleState state) { + double speedRadPerSec = state.speedMetersPerSecond / DriveConstants.wheelRadius; + m_io.runDriveVelocity(speedRadPerSec); + m_io.runTurnPosition(state.angle); + } + + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + m_io.runDriveOpenLoop(output); + m_io.runTurnPosition(Rotation2d.kZero); + } + + /** Disables all outputs to motors. */ + public void stop() { + m_io.runDriveOpenLoop(0.0); + m_io.runTurnOpenLoop(0.0); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + return m_inputs.turnPosition; + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return m_inputs.drivePositionRad * DriveConstants.wheelRadius; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return m_inputs.driveVelocityRadPerSec * DriveConstants.wheelRadius; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return m_inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(m_inputs.driveVelocityRadPerSec); + } + + /* Sets brake mode to {@code enabled} */ + public void setDriveBrakeMode(boolean enabled) { + m_io.setDriveBrakeMode(enabled); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 3db322c..b3bc609 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,3 +1,50 @@ package frc.robot.subsystems.drive; -public interface ModuleIO {} +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified open loop value. */ + public default void runDriveOpenLoop(double output) {} + + /** Run the turn motor at the specified open loop value. */ + public default void runTurnOpenLoop(double output) {} + + /** Run the drive motor at the specified velocity. */ + public default void runDriveVelocity(double velocityRadPerSec) {} + + /** Run the turn motor to the specified rotation. */ + public default void runTurnPosition(Rotation2d rotation) {} + + /** Set P, I, and D gains for closed loop control on drive motor. */ + public default void setDrivePID(double kP, double kI, double kD) {} + + /** Set kS, kV gains for closed loop control on drive motor. */ + public default void setDriveFF(double kS, double kV) {} + + /** Set P, I, and D gains for closed loop control on turn motor. */ + public default void setTurnPID(double kP, double kI, double kD) {} + + /** Set brake mode on drive motor */ + public default void setDriveBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 48923a5..8775b93 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -1,3 +1,214 @@ package frc.robot.subsystems.drive; -public class ModuleIOSpark {} +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.AnalogEncoder; +import frc.robot.Constants; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Queue; + +public class ModuleIOSpark implements ModuleIO { + private final ModuleConfig config; + + // Hardware objects + private final SparkMax driveSpark; + private final SparkMax turnSpark; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnEncoder; + private final AnalogEncoder turnAbsoluteEncoder; + + // Closed loop controllers + private final SparkClosedLoopController driveController; + private final SparkClosedLoopController turnController; + private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); + + // Queue inputs from odometry thread + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private boolean hasResetTurnPosition = false; + + public ModuleIOSpark(int index) { + switch (Constants.getRobotType()) { + case ROBOT_2025_COMP -> { + config = moduleConfigs[index]; + } + default -> throw new IllegalStateException("Unexpected RobotType for Spark Module: " + Constants.getRobotType()); + } + + // Initialize Hardware Devices + driveSpark = new SparkMax(config.driveMotorId(), MotorType.kBrushless); + turnSpark = new SparkMax(config.turnMotorId(), MotorType.kBrushless); + + driveEncoder = driveSpark.getEncoder(); + turnEncoder = turnSpark.getEncoder(); + turnAbsoluteEncoder = new AnalogEncoder(config.encoderChannel(), Math.PI, -Math.PI); + + driveController = driveSpark.getClosedLoopController(); + turnController = turnSpark.getClosedLoopController(); + + // Configure Drive + var driveConfig = new SparkMaxConfig(); + driveConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(50) + .voltageCompensation(12.0); + driveConfig + .encoder + .positionConversionFactor(2 * Math.PI / config.driveGearing()) + .velocityConversionFactor((2 * Math.PI) / 60.0 / config.driveGearing()) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + driveConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pidf(0.0, 0.0, 0.0, 0.0); + driveConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequencyHz)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + driveSpark.configure(driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + // Configure Turn + var turnConfig = new SparkMaxConfig(); + turnConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(20) + .voltageCompensation(12.0); + turnConfig + .encoder + .positionConversionFactor(2 * Math.PI / config.turnGearing()) + .velocityConversionFactor((2 * Math.PI) / 60.0 / config.turnGearing()) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + turnConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(-Math.PI, Math.PI) // TODO? + .pidf(0.0, 0.0, 0.0, 0.0); + turnConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequencyHz)) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + turnSpark.configure(turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + drivePositionQueue = OdometryManager.getInstance().registerSignal(driveEncoder::getPosition); + turnPositionQueue = OdometryManager.getInstance().registerSignal(turnEncoder::getPosition); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = driveEncoder.getPosition(); + inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); + inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage(); + inputs.driveCurrentAmps = driveSpark.getOutputCurrent(); + + inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); + inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition()); + inputs.turnVelocityRadPerSec = turnEncoder.getVelocity(); + inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); + inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); + + inputs.odometryDrivePositionsRad = drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryTurnPositions = drivePositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void runDriveOpenLoop(double output) { + driveSpark.setVoltage(output); + } + + @Override + public void runTurnOpenLoop(double output) { + turnSpark.setVoltage(output); + } + + @Override + public void runDriveVelocity(double velocityRadPerSec) { + double ffVolts = driveFeedforward.calculate(velocityRadPerSec); + driveController.setReference( + velocityRadPerSec, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); + } + + @Override + public void runTurnPosition(Rotation2d rotation) { + if(!hasResetTurnPosition) { + turnEncoder.setPosition(getOffsetAbsoluteAngle().getRadians()); + hasResetTurnPosition = true; + } + + // Ensure Setpoint is within range + var updatedSetpoint = MathUtil.angleModulus(rotation.getRadians()); + turnController.setReference(updatedSetpoint, ControlType.kPosition); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + var drivePIDConfig = new SparkMaxConfig(); + drivePIDConfig + .closedLoop + .pid(kP, kI, kD); + + driveSpark.configure(drivePIDConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + @Override + public void setDriveFF(double kS, double kV) { + driveFeedforward = new SimpleMotorFeedforward(kS, kV); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + var turnPIDConfig = new SparkMaxConfig(); + turnPIDConfig + .closedLoop + .pid(kP, kI, kD); + + turnSpark.configure(turnPIDConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + @Override + public void setDriveBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); + + driveSpark.configure(brakeModeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + private Rotation2d getOffsetAbsoluteAngle() { + return Rotation2d.fromRadians(turnAbsoluteEncoder.get()).minus(config.encoderOffset()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java index ca455f7..0086533 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -1,20 +1,19 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Notifier; import java.util.ArrayList; import java.util.List; -import java.util.Optional; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Supplier; +import java.util.function.DoubleSupplier; +import lombok.Getter; import org.littletonrobotics.junction.AutoLog; -public class OdometryManager { - public static Lock odometryLock = new ReentrantLock(); - public static double ODOMETRY_FREQUENCY_HZ = 200.0; +public class OdometryManager implements AutoCloseable { + public static Lock odometryLock = + new ReentrantLock(); // Prevent conflicts when reading and writing data private static OdometryManager instance = null; @@ -25,32 +24,57 @@ public static OdometryManager getInstance() { return instance; } - private final Queue timestampQueue = new ArrayBlockingQueue<>(20); - private final List m_moduleSources = new ArrayList<>(4); - private GyroSource m_gyroSource = null; + @Getter private final Queue timestampQueue = new ArrayBlockingQueue<>(20); + private final List signalSuppliers = new ArrayList<>(9); + private final List> signalQueues = new ArrayList<>(9); - public void registerModuleSource( - Supplier> drivePositionSupplier, - Supplier> turnAngleSupplier) {} + private final Notifier notifier = new Notifier(this::run); - public void registerGyroSource(Supplier> robotYawSupplier) {} + private OdometryManager() { + notifier.setName("Odometry Data Collection Thread"); + } - private static void run() {} + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + odometryLock.lock(); + try { + signalSuppliers.add(signal); + signalQueues.add(queue); + } finally { + odometryLock.unlock(); + } + return queue; + } - private record ModuleSource( - Queue drivePositionQueue, - Supplier> drivePositionSupplier, - Queue turnAngleQueue, - Supplier> turnAngleSupplier) {} + private void run() { + odometryLock.lock(); + try { + for (int i = 0; i < signalSuppliers.size(); i++) { + signalQueues.get(i).add(signalSuppliers.get(i).getAsDouble()); + } + } finally { + odometryLock.unlock(); + } + } - private record GyroSource( - Queue robotYawQueue, Supplier> robotYawSupplier) {} + public void start() throws IllegalStateException { + // Check that any sources are supplied + if (signalSuppliers.isEmpty()) { + throw new IllegalStateException( + "Tried to start OdometryManager, but not all sources have been set."); + } + + notifier.startPeriodic(1.0 / DriveConstants.odometryFrequencyHz); + } + + @Override + public void close() { + notifier.stop(); + notifier.close(); + } @AutoLog - public class TimestampInputs { + public class OdometryTimestampsInput { public double[] timestamps; - - public Rotation2d[] gyroYaws; - public SwerveModulePosition[][] modulePositions; } } diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..f632cec --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,99 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.FieldConstants; +import java.util.function.Function; + +public class AllianceFlipUtil { + private static double applyX(double x) { + return shouldFlip() ? FieldConstants.fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? FieldConstants.fieldWidth - y : y; + } + + private static Translation2d applyTranslation(Translation2d translation2d) { + return new Translation2d(applyX(translation2d.getX()), translation2d.getY()); + } + + private static Translation3d applyTranslation(Translation3d translation3d) { + return new Translation3d( + applyX(translation3d.getX()), translation3d.getY(), translation3d.getZ()); + } + + private static Rotation2d applyRotation(Rotation2d rotation) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } + + private static Pose2d applyPose(Pose2d pose) { + return new Pose2d(applyTranslation(pose.getTranslation()), applyRotation(pose.getRotation())); + } + + private static boolean shouldFlip() { + var currentAllianceOpt = DriverStation.getAlliance(); + return currentAllianceOpt.isPresent() && currentAllianceOpt.get() == DriverStation.Alliance.Red; + } + + public static double apply(double x) { + return shouldFlip() ? applyX(x) : x; + } + + public static Translation2d apply(Translation2d translation) { + return shouldFlip() ? applyTranslation(translation) : translation; + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? applyRotation(rotation) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() ? applyPose(pose) : pose; + } + + public static Translation3d apply(Translation3d translation3d) { + return shouldFlip() ? applyTranslation(translation3d) : translation3d; + } + + public static class AllianceRelative { + private final T value; + private final Function transformer; + + public AllianceRelative(T value, Function transformer) { + this.value = value; + this.transformer = transformer; + } + + public T get() { + return shouldFlip() ? transformer.apply(value) : value; + } + + public T getRaw() { + return value; + } + + public static AllianceRelative from(double value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyX); + } + + public static AllianceRelative from(Rotation2d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); + } + + public static AllianceRelative from(Translation2d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyTranslation); + } + + public static AllianceRelative from(Pose2d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyPose); + } + + public static AllianceRelative from(Translation3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyTranslation); + } + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 6354a44..71e4c88 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -1,6 +1,6 @@ package frc.robot.util; -import frc.robot.constants.Constants; +import frc.robot.Constants; import java.util.Arrays; import java.util.HashMap; import java.util.Map; diff --git a/src/main/java/frc/robot/util/LoggerUtil.java b/src/main/java/frc/robot/util/LoggerUtil.java index 5909361..b84fb7f 100644 --- a/src/main/java/frc/robot/util/LoggerUtil.java +++ b/src/main/java/frc/robot/util/LoggerUtil.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj.RobotBase; import frc.generated.BuildConstants; -import frc.robot.constants.Constants; +import frc.robot.Constants; import java.nio.file.Path; import java.util.Optional; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java index 3577b31..bc9a57e 100644 --- a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -15,6 +15,7 @@ import lombok.RequiredArgsConstructor; import lombok.experimental.ExtensionMethod; +// TODO JNI This /** * Ripped from 6328 @@ -186,7 +187,7 @@ protected double findDriveMaxS( * @param desiredState The desired state of motion, such as from the driver sticks or a path * following algorithm. * @param dt The loop time. - * @return A Setpoint object that satisfies all of the KinematicLimits while converging to + * @return A Setpoint object that satisfies all the KinematicLimits while converging to * desiredState quickly. */ public SwerveSetpoint generateSetpoint( diff --git a/vendordeps/REVLib-2025.json b/vendordeps/REVLib-2025.json index 552a3b0..717aa34 100644 --- a/vendordeps/REVLib-2025.json +++ b/vendordeps/REVLib-2025.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib.json", + "fileName": "REVLib-2025.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,