From 0310fca2c7fc29540745cda1eb5d09faea8773fd Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 18 Jan 2025 04:02:47 -0500 Subject: [PATCH 01/73] WIP (squash later) --- .github/workflows/build.yml | 2 +- .github/workflows/lint-format.yml | 39 - build.gradle | 4 +- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/commands/DriveCommands.java | 41 + .../frc/robot/subsystems/drive/DriveBase.java | 23 + .../frc/robot/subsystems/drive/GyroIO.java | 3 + .../robot/subsystems/drive/GyroIOPigeon2.java | 3 + .../frc/robot/subsystems/drive/Module.java | 3 + .../frc/robot/subsystems/drive/ModuleIO.java | 3 + .../robot/subsystems/drive/ModuleIOSim.java | 3 + .../robot/subsystems/drive/ModuleIOSpark.java | 3 + .../subsystems/drive/OdometryManager.java | 56 ++ src/main/java/frc/robot/util/EqualsUtil.java | 22 + src/main/java/frc/robot/util/GeomUtil.java | 154 ++++ .../frc/robot/util/LoggedTunableNumber.java | 134 +++ .../util/swerve/SwerveSetpointGenerator.java | 402 +++++++++ vendordeps/Phoenix6-frc2025-latest.json | 834 +++++++++--------- 18 files changed, 1271 insertions(+), 460 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveBase.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/drive/OdometryManager.java create mode 100644 src/main/java/frc/robot/util/EqualsUtil.java create mode 100644 src/main/java/frc/robot/util/GeomUtil.java create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java create mode 100644 src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 448dd18..842c3b5 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -8,7 +8,7 @@ jobs: build: name: Build runs-on: ubuntu-latest - container: wpilib/roborio-cross-ubuntu + container: wpilib/roborio-cross-ubuntu:2025-24.04 steps: - name: Checkout repository uses: actions/checkout@v4 diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index a646677..3915d1a 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -15,45 +15,6 @@ jobs: - uses: actions/checkout@v4 - uses: gradle/actions/wrapper-validation@v4 - wpiformat: - name: "wpiformat" - runs-on: ubuntu-22.04 - steps: - - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - name: Fetch all history and metadata - run: | - git checkout -b pr - git branch -f main origin/main - - name: Set up Python 3.12 - uses: actions/setup-python@v5 - with: - python-version: '3.12' - - name: Install wpiformat - run: | - python -m venv ${{ runner.temp }}/wpiformat - ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51 - - name: Run - run: ${{ runner.temp }}/wpiformat/bin/wpiformat - - name: Check output - run: git --no-pager diff --exit-code HEAD - - name: Generate diff - run: git diff HEAD > wpiformat-fixes.patch - if: ${{ failure() }} - - uses: actions/upload-artifact@v4 - with: - name: wpiformat fixes - path: wpiformat-fixes.patch - if: ${{ failure() }} - - name: Write to job summary - run: | - echo '```diff' >> $GITHUB_STEP_SUMMARY - cat wpiformat-fixes.patch >> $GITHUB_STEP_SUMMARY - echo '' >> $GITHUB_STEP_SUMMARY - echo '```' >> $GITHUB_STEP_SUMMARY - if: ${{ failure() }} - javaformat: name: "Java format" runs-on: ubuntu-22.04 diff --git a/build.gradle b/build.gradle index 9aeb7ea..cf63f85 100644 --- a/build.gradle +++ b/build.gradle @@ -160,7 +160,7 @@ spotless { exclude "**/build/**", "**/build-*/**" } greclipse() - indentWithSpaces(4) + leadingTabsToSpaces(4) trimTrailingWhitespace() endWithNewline() } @@ -177,7 +177,7 @@ spotless { exclude "**/build/**", "**/build-*/**" } trimTrailingWhitespace() - indentWithSpaces(2) + leadingTabsToSpaces(2) endWithNewline() } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b4dc71d..d96407c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,8 +16,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj2.command.CommandScheduler; 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; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..be28a5b --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,41 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.DriveBase; +import java.util.LinkedList; +import java.util.List; +import java.util.function.DoubleSupplier; + +public class DriveCommands { + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + DriveBase driveBase, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + 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() {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java new file mode 100644 index 0000000..eb2fa70 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +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() {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..7fe49af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.drive; + +public interface GyroIO {} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..230d0c3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.drive; + +public class GyroIOPigeon2 {} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..45bb17c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.drive; + +public class Module {} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..3db322c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.drive; + +public interface ModuleIO {} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..c469617 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,3 @@ +// package frc.robot.subsystems.drive; +// +// public class ModuleIOSim {} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java new file mode 100644 index 0000000..48923a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.drive; + +public class ModuleIOSpark {} diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java new file mode 100644 index 0000000..ca455f7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +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 org.littletonrobotics.junction.AutoLog; + +public class OdometryManager { + public static Lock odometryLock = new ReentrantLock(); + public static double ODOMETRY_FREQUENCY_HZ = 200.0; + + private static OdometryManager instance = null; + + public static OdometryManager getInstance() { + if (instance == null) { + instance = new OdometryManager(); + } + return instance; + } + + private final Queue timestampQueue = new ArrayBlockingQueue<>(20); + private final List m_moduleSources = new ArrayList<>(4); + private GyroSource m_gyroSource = null; + + public void registerModuleSource( + Supplier> drivePositionSupplier, + Supplier> turnAngleSupplier) {} + + public void registerGyroSource(Supplier> robotYawSupplier) {} + + private static void run() {} + + private record ModuleSource( + Queue drivePositionQueue, + Supplier> drivePositionSupplier, + Queue turnAngleQueue, + Supplier> turnAngleSupplier) {} + + private record GyroSource( + Queue robotYawQueue, Supplier> robotYawSupplier) {} + + @AutoLog + public class TimestampInputs { + public double[] timestamps; + + public Rotation2d[] gyroYaws; + public SwerveModulePosition[][] modulePositions; + } +} diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java new file mode 100644 index 0000000..e839a5c --- /dev/null +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -0,0 +1,22 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Twist2d; + +public class EqualsUtil { + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, 1e-9); + } + + /** Extension methods for wpi geometry objects */ + public static class GeomExtensions { + public static boolean epsilonEquals(Twist2d twist, Twist2d other) { + return EqualsUtil.epsilonEquals(twist.dx, other.dx) + && EqualsUtil.epsilonEquals(twist.dy, other.dy) + && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); + } + } +} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 0000000..f50dcad --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,154 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x coordinate of the translation + * @param y The y coordinate of the translation + * @return The resulting transform + */ + public static Transform2d toTransform2d(double x, double y) { + return new Transform2d(x, y, new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + public static Pose3d toPose3d(Translation3d translation) { + return new Pose3d(translation, new Rotation3d()); + } + + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } + + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 0000000..6354a44 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,134 @@ +package frc.robot.util; + +import frc.robot.constants.Constants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private Double defaultValue = null; + + private final Map lastValues = new HashMap<>(); + + private LoggedNetworkNumber dashboardNumber; + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + * @throws IllegalStateException If a default value has already been set either by the constructor + * or by a previous call to this method. + */ + public void initDefault(double defaultValue) { + if (this.defaultValue != null) { + throw new IllegalStateException( + String.format( + "[LoggedTunableNumber][%s] Has already been initialized with a default value.", key)); + } + + this.defaultValue = defaultValue; + if (Constants.TUNING_MODE) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + * @throws IllegalStateException If a default value hasn't been set yet. + */ + public double get() { + if (defaultValue == null) { + throw new IllegalStateException( + String.format( + "[LoggedTunableNumber][%s] Hasn't been initialized with a default value. Make sure to call initDefault or use the correct constructor.", + key)); + } + + return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue; + } + + /** + * Checks whether the number has changed since the last time this method was called. Returns true + * the first time this method is called. + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return Whether the value has changed since the last time this method was called + */ + public boolean hasChanged(int id) { + double currentValue = get(); + var lastValue = lastValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Checks whether the number has changed since the last time this method was called. Returns true + * the first time this method is called. + * + * @apiNote This method assumes that there is only a single object is calling this method. For + * that use case, see {@link #hasChanged(int)}. + * @return Whether the value has changed since the last time this method was called + */ + public boolean hasChanged() { + return hasChanged(0); + } + + /** + * Run callback if any tunable number has changed. See {@link #hasChanged(int)} for usage. + * + * @param action action to run + * @param tunableNumbers tunable numbers to check + */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(v -> v.hasChanged(id))) { + action.run(); + } + } + + /** + * Run callback if any tunable number has changed. See {@link #hasChanged()} for usage. + * + * @param action action to run + * @param tunableNumbers tunable numbers to check + */ + public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(LoggedTunableNumber::hasChanged)) { + action.run(); + } + } +} diff --git a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java new file mode 100644 index 0000000..3577b31 --- /dev/null +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -0,0 +1,402 @@ +package frc.robot.util.swerve; + +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.SwerveModuleState; +import frc.robot.util.EqualsUtil; +import frc.robot.util.GeomUtil; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import lombok.Builder; +import lombok.RequiredArgsConstructor; +import lombok.experimental.ExtensionMethod; + +/** + * Ripped from 6328 + * swerve setpoint smoothing. + * + *

"Inspired" by FRC team 254. + * + *

Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path + * follower), and outputs a new setpoint that respects all the kinematic constraints on module + * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the + * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is + * kinematically infeasible (and can result in wheel slip or robot heading drift as a result). + */ +@Builder +@RequiredArgsConstructor +@ExtensionMethod({GeomUtil.class, EqualsUtil.GeomExtensions.class}) +public class SwerveSetpointGenerator { + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + if (iterations_left < 0 || EqualsUtil.epsilonEquals(f_0, f_1)) { + return 1.0; + } + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + protected double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation, + int max_iterations) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = + (x, y) -> { + return unwrapAngle(f_0, Math.atan2(y, x)) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + protected double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step, + int max_iterations) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = + (x, y) -> { + return Math.hypot(x, y) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + // protected double findDriveMaxS( + // double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // // Our drive velocity between s=0 and s=1 is quadratic in s: + // // v^2 = ((x_1 - x_0) * s + x_0)^2 + ((y_1 - y_0) * s + y_0)^2 + // // = a * s^2 + b * s + c + // // Where: + // // a = (x_1 - x_0)^2 + (y_1 - y_0)^2 + // // b = 2 * x_0 * (x_1 - x_0) + 2 * y_0 * (y_1 - y_0) + // // c = x_0^2 + y_0^2 + // // We want to find where this quadratic results in a velocity that is > max_vel_step from our + // // velocity at s=0: + // // sqrt(x_0^2 + y_0^2) +/- max_vel_step = ...quadratic... + // final double dx = x_1 - x_0; + // final double dy = y_1 - y_0; + // final double a = dx * dx + dy * dy; + // final double b = 2.0 * x_0 * dx + 2.0 * y_0 * dy; + // final double c = x_0 * x_0 + y_0 * y_0; + // final double v_limit_upper_2 = Math.pow(Math.hypot(x_0, y_0) + max_vel_step, 2.0); + // final double v_limit_lower_2 = Math.pow(Math.hypot(x_0, y_0) - max_vel_step, 2.0); + // return 0.0; + // } + + /** + * Generate a new setpoint. + * + * @param limits The kinematic limits to respect for this setpoint. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @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 + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final ModuleLimits limits, + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredState, + double dt) { + final Translation2d[] modules = moduleLocations; + + SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); + // Make sure desiredState respects velocity limits. + if (limits.maxDriveVelocity() > 0.0) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); + desiredState = kinematics.toChassisSpeeds(desiredModuleState); + } + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + need_to_steer = false; + for (int i = 0; i < modules.length; ++i) { + desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; + desiredModuleState[i].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[modules.length]; + double[] prev_vy = new double[modules.length]; + Rotation2d[] prev_heading = new Rotation2d[modules.length]; + double[] desired_vx = new double[modules.length]; + double[] desired_vy = new double[modules.length]; + Rotation2d[] desired_heading = new Rotation2d[modules.length]; + boolean all_modules_should_flip = true; + for (int i = 0; i < modules.length; ++i) { + prev_vx[i] = + prevSetpoint.moduleStates()[i].angle.getCos() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vy[i] = + prevSetpoint.moduleStates()[i].angle.getSin() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_heading[i] = prevSetpoint.moduleStates()[i].angle; + if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { + prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + desired_vx[i] = + desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = + desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; + desired_heading[i] = desiredModuleState[i].angle; + if (desiredModuleState[i].speedMetersPerSecond < 0.0) { + desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) + && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired + // angle, and accelerate again. + return generateSetpoint(limits, prevSetpoint, new ChassisSpeeds(), dt); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then + // find the amount we can move from start towards goal in this cycle such that no kinematic + // limit is exceeded. + double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + double dtheta = + desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since + // inverse kinematics doesn't care about angle, we can be opportunistically lazy). + List> overrideSteering = new ArrayList<>(modules.length); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, + // and then backing out the maximum interpolant between start and goal states. We remember the + // minimum across all modules, since + // that is the active constraint. + final double max_theta_step = dt * limits.maxSteeringVelocity(); + for (int i = 0; i < modules.length; ++i) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + if (EqualsUtil.epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based + // purely on rotation in place. + if (EqualsUtil.epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); + } + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); + // Don't limit the global min_s; + continue; + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + i, + Optional.of( + prevSetpoint.moduleStates()[i].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); + min_s = 0.0; + continue; + } + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + final int kMaxIterations = 8; + double s = + findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel acceleration limits. + final double max_vel_step = dt * limits.maxDriveAcceleration(); + for (int i = 0; i < modules.length; ++i) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + double vx_min_s = + min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = + min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster + // than that. + final int kMaxIterations = 10; + double s = + min_s + * findDriveMaxS( + prev_vx[i], + prev_vy[i], + Math.hypot(prev_vx[i], prev_vy[i]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + max_vel_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); + var retStates = kinematics.toSwerveModuleStates(retSpeeds); + for (int i = 0; i < modules.length; ++i) { + final var maybeOverride = overrideSteering.get(i); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { + retStates[i].speedMetersPerSecond *= -1.0; + } + retStates[i].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); + if (flipHeading(deltaRotation)) { + retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); + retStates[i].speedMetersPerSecond *= -1.0; + } + } + return new SwerveSetpoint(retSpeeds, retStates); + } + + public record ModuleLimits( + double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} + + public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {} +} diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index 51d0083..820c61a 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,419 +1,419 @@ { - "fileName": "Phoenix6-frc2025-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.2.1", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] } From 16c4eba5a4e66e6019a69086de7db2b8c3653f43 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 29 Jan 2025 12:44:39 -0500 Subject: [PATCH 02/73] Increase voltage warning on battery and add CAN error alert --- src/main/java/frc/robot/Constants.java | 2 -- src/main/java/frc/robot/Robot.java | 18 ++++++++++++++++-- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ 3 files changed, 22 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed989ab..942f1bd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,8 +12,6 @@ public class Constants { public static final double kLoopPeriodSecs = 0.02; - public static final double LOW_VOLTAGE_WARNING_THRESHOLD = 10.0; - public enum RobotMode { REAL, SIM, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 64b446b..4bc69c8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; @@ -36,12 +37,19 @@ * project. */ public class Robot extends LoggedRobot { + private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; + private Command autonomousCommand; private final RobotContainer robotContainer; + // System Alerts + private final Alert canErrorAlert = + new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + private final Debouncer canErrorDebouncer = new Debouncer(0.5); + private final Alert lowBatteryVoltageAlert = new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning); - private final Debouncer batteryVoltageDebouncer = new Debouncer(0.5); + private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); public Robot() { super(Constants.kLoopPeriodSecs); @@ -92,10 +100,16 @@ public void robotPeriodic() { // Run command scheduler CommandScheduler.getInstance().run(); + // Check CAN status + var canStatus = RobotController.getCANStatus(); + canErrorAlert.set( + canErrorDebouncer.calculate( + canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); + // Update Battery Voltage Alert lowBatteryVoltageAlert.set( batteryVoltageDebouncer.calculate( - RobotController.getBatteryVoltage() <= Constants.LOW_VOLTAGE_WARNING_THRESHOLD)); + RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fc75b6..0e2bfb2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -23,6 +24,9 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + // Alerts + private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo); + public RobotContainer() { switch (Constants.getRobotMode()) { case REAL -> { @@ -58,6 +62,8 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices"); if (Constants.TUNING_MODE) { + tuningModeAlert.set(true); + // Set up Characterization routines autoChooser.addOption( "Drive Wheel Radius Characterization", From f9babf3b43a48e78657bb152028209e1788a6230 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 1 Feb 2025 09:57:53 -0500 Subject: [PATCH 03/73] clean --- src/main/java/frc/robot/util/EqualsUtil.java | 6 ++++ .../util/swerve/SwerveSetpointGenerator.java | 32 +++++++++---------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java index e839a5c..a267032 100644 --- a/src/main/java/frc/robot/util/EqualsUtil.java +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -18,5 +18,11 @@ public static boolean epsilonEquals(Twist2d twist, Twist2d other) { && EqualsUtil.epsilonEquals(twist.dy, other.dy) && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); } + + public static boolean equalsZero(Twist2d twist) { + return EqualsUtil.epsilonEquals(twist.dx, 0.0) + && EqualsUtil.epsilonEquals(twist.dy, 0.0) + && EqualsUtil.epsilonEquals(twist.dtheta, 0.0); + } } } diff --git a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java index bc9a57e..cb2982a 100644 --- a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -195,8 +195,6 @@ public SwerveSetpoint generateSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredState, double dt) { - final Translation2d[] modules = moduleLocations; - SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); // Make sure desiredState respects velocity limits. if (limits.maxDriveVelocity() > 0.0) { @@ -207,23 +205,23 @@ public SwerveSetpoint generateSetpoint( // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so // just use the previous angle. boolean need_to_steer = true; - if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + if (desiredState.toTwist2d().equalsZero()) { need_to_steer = false; - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; desiredModuleState[i].speedMetersPerSecond = 0.0; } } // For each module, compute local Vx and Vy vectors. - double[] prev_vx = new double[modules.length]; - double[] prev_vy = new double[modules.length]; - Rotation2d[] prev_heading = new Rotation2d[modules.length]; - double[] desired_vx = new double[modules.length]; - double[] desired_vy = new double[modules.length]; - Rotation2d[] desired_heading = new Rotation2d[modules.length]; + double[] prev_vx = new double[moduleLocations.length]; + double[] prev_vy = new double[moduleLocations.length]; + Rotation2d[] prev_heading = new Rotation2d[moduleLocations.length]; + double[] desired_vx = new double[moduleLocations.length]; + double[] desired_vy = new double[moduleLocations.length]; + Rotation2d[] desired_heading = new Rotation2d[moduleLocations.length]; boolean all_modules_should_flip = true; - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { prev_vx[i] = prevSetpoint.moduleStates()[i].angle.getCos() * prevSetpoint.moduleStates()[i].speedMetersPerSecond; @@ -251,8 +249,8 @@ public SwerveSetpoint generateSetpoint( } } if (all_modules_should_flip - && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) - && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + && !prevSetpoint.chassisSpeeds().toTwist2d().equalsZero() + && !desiredState.toTwist2d().equalsZero()) { // It will (likely) be faster to stop the robot, rotate the modules in place to the complement // of the desired // angle, and accelerate again. @@ -275,14 +273,14 @@ public SwerveSetpoint generateSetpoint( // In cases where an individual module is stopped, we want to remember the right steering angle // to command (since // inverse kinematics doesn't care about angle, we can be opportunistically lazy). - List> overrideSteering = new ArrayList<>(modules.length); + List> overrideSteering = new ArrayList<>(moduleLocations.length); // Enforce steering velocity limits. We do this by taking the derivative of steering angle at // the current angle, // and then backing out the maximum interpolant between start and goal states. We remember the // minimum across all modules, since // that is the active constraint. final double max_theta_step = dt * limits.maxSteeringVelocity(); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { if (!need_to_steer) { overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); continue; @@ -344,7 +342,7 @@ public SwerveSetpoint generateSetpoint( // Enforce drive wheel acceleration limits. final double max_vel_step = dt * limits.maxDriveAcceleration(); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { if (min_s == 0.0) { // No need to carry on. break; @@ -377,7 +375,7 @@ public SwerveSetpoint generateSetpoint( prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); var retStates = kinematics.toSwerveModuleStates(retSpeeds); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { final var maybeOverride = overrideSteering.get(i); if (maybeOverride.isPresent()) { var override = maybeOverride.get(); From 93e655dd78d69d1fe1200e81de80e00050792309 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 18 Jan 2025 04:02:47 -0500 Subject: [PATCH 04/73] Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI --- .github/workflows/build.yml | 3 +- .github/workflows/lint-format.yml | 41 +- .wpilib/wpilib_preferences.json | 2 +- build.gradle | 4 +- gradlew | 0 src/main/deploy/trajectories/.gitkeep | 0 .../frc/robot/{constants => }/Constants.java | 9 +- src/main/java/frc/robot/FieldConstants.java | 203 +++++ src/main/java/frc/robot/Robot.java | 19 +- src/main/java/frc/robot/RobotContainer.java | 104 ++- src/main/java/frc/robot/RobotState.java | 104 +++ .../frc/robot/commands/DriveCommands.java | 276 ++++++ .../frc/robot/subsystems/drive/DriveBase.java | 275 ++++++ .../subsystems/drive/DriveConstants.java | 98 ++ .../frc/robot/subsystems/drive/GyroIO.java | 18 + .../robot/subsystems/drive/GyroIOPigeon2.java | 50 ++ .../frc/robot/subsystems/drive/Module.java | 153 ++++ .../frc/robot/subsystems/drive/ModuleIO.java | 52 ++ .../robot/subsystems/drive/ModuleIOSim.java | 135 +++ .../robot/subsystems/drive/ModuleIOSpark.java | 218 +++++ .../subsystems/drive/OdometryManager.java | 86 ++ .../java/frc/robot/util/AllianceFlipUtil.java | 99 +++ src/main/java/frc/robot/util/EqualsUtil.java | 22 + src/main/java/frc/robot/util/GeomUtil.java | 154 ++++ .../frc/robot/util/LoggedTunableNumber.java | 159 ++++ src/main/java/frc/robot/util/LoggerUtil.java | 2 +- .../util/swerve/SwerveSetpointGenerator.java | 403 +++++++++ vendordeps/Phoenix6-frc2025-latest.json | 834 +++++++++--------- vendordeps/REVLib-2025.json | 12 +- vendordeps/URCL.json | 10 +- 30 files changed, 3063 insertions(+), 482 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/deploy/trajectories/.gitkeep rename src/main/java/frc/robot/{constants => }/Constants.java (86%) create mode 100644 src/main/java/frc/robot/FieldConstants.java create mode 100644 src/main/java/frc/robot/RobotState.java create mode 100644 src/main/java/frc/robot/commands/DriveCommands.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveBase.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/drive/OdometryManager.java create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java create mode 100644 src/main/java/frc/robot/util/EqualsUtil.java create mode 100644 src/main/java/frc/robot/util/GeomUtil.java create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java create mode 100644 src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 448dd18..3d8202a 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,14 +1,13 @@ name: Build on: - push: pull_request: jobs: build: name: Build runs-on: ubuntu-latest - container: wpilib/roborio-cross-ubuntu + container: wpilib/roborio-cross-ubuntu:2025-24.04 steps: - name: Checkout repository uses: actions/checkout@v4 diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index a646677..fe4add2 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -1,7 +1,7 @@ name: Lint and Format on: - push: + pull_request: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} @@ -15,45 +15,6 @@ jobs: - uses: actions/checkout@v4 - uses: gradle/actions/wrapper-validation@v4 - wpiformat: - name: "wpiformat" - runs-on: ubuntu-22.04 - steps: - - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - name: Fetch all history and metadata - run: | - git checkout -b pr - git branch -f main origin/main - - name: Set up Python 3.12 - uses: actions/setup-python@v5 - with: - python-version: '3.12' - - name: Install wpiformat - run: | - python -m venv ${{ runner.temp }}/wpiformat - ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51 - - name: Run - run: ${{ runner.temp }}/wpiformat/bin/wpiformat - - name: Check output - run: git --no-pager diff --exit-code HEAD - - name: Generate diff - run: git diff HEAD > wpiformat-fixes.patch - if: ${{ failure() }} - - uses: actions/upload-artifact@v4 - with: - name: wpiformat fixes - path: wpiformat-fixes.patch - if: ${{ failure() }} - - name: Write to job summary - run: | - echo '```diff' >> $GITHUB_STEP_SUMMARY - cat wpiformat-fixes.patch >> $GITHUB_STEP_SUMMARY - echo '' >> $GITHUB_STEP_SUMMARY - echo '```' >> $GITHUB_STEP_SUMMARY - if: ${{ failure() }} - javaformat: name: "Java format" runs-on: ubuntu-22.04 diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 2171dac..7585a12 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2025", - "teamNumber": 6328 + "teamNumber": 540 } diff --git a/build.gradle b/build.gradle index 9aeb7ea..cf63f85 100644 --- a/build.gradle +++ b/build.gradle @@ -160,7 +160,7 @@ spotless { exclude "**/build/**", "**/build-*/**" } greclipse() - indentWithSpaces(4) + leadingTabsToSpaces(4) trimTrailingWhitespace() endWithNewline() } @@ -177,7 +177,7 @@ spotless { exclude "**/build/**", "**/build-*/**" } trimTrailingWhitespace() - indentWithSpaces(2) + leadingTabsToSpaces(2) endWithNewline() } } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/deploy/trajectories/.gitkeep b/src/main/deploy/trajectories/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/Constants.java similarity index 86% rename from src/main/java/frc/robot/constants/Constants.java rename to src/main/java/frc/robot/Constants.java index ab9a0c7..ed989ab 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,17 +1,19 @@ -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; + public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running public static final boolean ENABLE_LOGGING = true; public static final double kLoopPeriodSecs = 0.02; + public static final double LOW_VOLTAGE_WARNING_THRESHOLD = 10.0; + public enum RobotMode { REAL, SIM, @@ -20,7 +22,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 b4dc71d..64b446b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,12 +13,13 @@ package frc.robot; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.util.LoggerUtil; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -36,7 +37,11 @@ */ public class Robot extends LoggedRobot { private Command autonomousCommand; - private RobotContainer robotContainer; + private final RobotContainer robotContainer; + + private final Alert lowBatteryVoltageAlert = + new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning); + private final Debouncer batteryVoltageDebouncer = new Debouncer(0.5); public Robot() { super(Constants.kLoopPeriodSecs); @@ -74,6 +79,9 @@ public Robot() { // Configure brownout voltage RobotController.setBrownoutVoltage(6.0); + + // Create RobotConatiner + robotContainer = new RobotContainer(); } @Override @@ -84,6 +92,11 @@ public void robotPeriodic() { // Run command scheduler CommandScheduler.getInstance().run(); + // Update Battery Voltage Alert + lowBatteryVoltageAlert.set( + batteryVoltageDebouncer.calculate( + RobotController.getBatteryVoltage() <= Constants.LOW_VOLTAGE_WARNING_THRESHOLD)); + // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9828fc8..1fc75b6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,10 +1,112 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.drive.*; +import frc.robot.util.AllianceFlipUtil; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { + // Load RobotState class + private final RobotState robotState = RobotState.getInstance(); + + // Subsystems + private final DriveBase driveBase; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + public RobotContainer() { + switch (Constants.getRobotMode()) { + case REAL -> { + driveBase = + new DriveBase( + new GyroIOPigeon2(), + new ModuleIOSpark(0), + new ModuleIOSpark(1), + new ModuleIOSpark(2), + new ModuleIOSpark(3)); + } + case SIM -> { + driveBase = + new DriveBase( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + } + default -> { + driveBase = + new DriveBase( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + } + } + + // Set up auto routines + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + + if (Constants.TUNING_MODE) { + // Set up Characterization routines + autoChooser.addOption( + "Drive Wheel Radius Characterization", + DriveCommands.wheelRadiusCharacterization(driveBase)); + autoChooser.addOption( + "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(driveBase)); + } + + configureButtonBindings(); + } + + private void configureButtonBindings() { + // Default command, normal field-relative drive + driveBase.setDefaultCommand( + DriveCommands.joystickDrive( + driveBase, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + + // Lock to 0° when A button is held + controller + .a() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + driveBase, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> Rotation2d.kZero)); + + // Switch to X pattern when X button is pressed + controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); + + // Reset gyro to 0° when B button is pressed + controller + .b() + .onTrue( + Commands.runOnce( + () -> + RobotState.getInstance() + .resetPose( + new Pose2d( + RobotState.getInstance().getEstimatedPose().getTranslation(), + AllianceFlipUtil.apply(new Rotation2d()))), + driveBase) + .ignoringDisable(true)); + } + public Command getAutonomousCommand() { - return Commands.none(); + return autoChooser.get(); } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java new file mode 100644 index 0000000..149fb4e --- /dev/null +++ b/src/main/java/frc/robot/RobotState.java @@ -0,0 +1,104 @@ +package frc.robot; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.robot.subsystems.drive.DriveConstants; +import lombok.Getter; +import org.littletonrobotics.junction.AutoLogOutput; + +public class RobotState { + // Standard deviations of the pose estimate (x position in meters, y position in meters, and + // heading in radians). + // Increase these numbers to trust your state estimate less. + private static final Matrix odometryStateStdDevs = VecBuilder.fill(0.003, 0.003, 0.002); + private static final double poseBufferSizeSec = 2.0; + + private static RobotState instance; + + public static RobotState getInstance() { + if (instance == null) { + instance = new RobotState(); + } + return instance; + } + + @Getter + @AutoLogOutput(key = "RobotState/OdometryPose") + private Pose2d odometryPose = new Pose2d(); + + @Getter + @AutoLogOutput(key = "RobotState/EstimatedPose") + private Pose2d estimatedPose = new Pose2d(); + + private final TimeInterpolatableBuffer poseBuffer = + TimeInterpolatableBuffer.createBuffer(poseBufferSizeSec); + private final Matrix qStdDevs = new Matrix<>(Nat.N3(), Nat.N1()); + + // Odometry + private final SwerveDriveKinematics kinematics; + private SwerveModulePosition[] lastWheelPositions = + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + // Assume gyro starts at zero + private Rotation2d gyroOffset = new Rotation2d(); + + private RobotState() { + for (int i = 0; i < 3; ++i) { + qStdDevs.set(i, 0, Math.pow(odometryStateStdDevs.get(i, 0), 2)); + } + + kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); + } + + public void resetPose(Pose2d pose) { + // Gyro offset is the rotation that maps the old gyro rotation (estimated - offset) to the new + // frame of rotation + gyroOffset = pose.getRotation().minus(estimatedPose.getRotation().minus(gyroOffset)); + estimatedPose = pose; + odometryPose = pose; + poseBuffer.clear(); + } + + public void addOdometryObservation( + SwerveModulePosition[] wheelPositions, Rotation2d gyroAngle, double timestamp) { + var twist = kinematics.toTwist2d(lastWheelPositions, wheelPositions); + + // Update previous state + lastWheelPositions = wheelPositions; + Pose2d lastOdometryPose = odometryPose; + + // Update Odometry + odometryPose = odometryPose.exp(twist); + + if (gyroAngle != null) { + // Use gyro measurement + // Add offset to measured angle + Rotation2d angle = gyroAngle.plus(gyroOffset); + odometryPose = new Pose2d(odometryPose.getTranslation(), angle); + } + + // Add pose to buffer at timestamp + poseBuffer.addSample(timestamp, odometryPose); + + // Calculate diff from last odometry pose and add onto pose estimate + Twist2d finalTwist = lastOdometryPose.log(odometryPose); + estimatedPose = estimatedPose.exp(finalTwist); + } + + public Rotation2d getRotation() { + return estimatedPose.getRotation(); + } +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..26acb14 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,276 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.RobotState; +import frc.robot.subsystems.drive.DriveBase; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LoggedTunableNumber; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveCommands { + // Drive + private static final double DEADBAND = 0.1; + + private static final LoggedTunableNumber LINEAR_VELOCITY_SCALAR = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0, true); + private static final LoggedTunableNumber ANGULAR_VELOCITY_SCALAR = + new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0, true); + + private static final double ANGLE_KP = 5.0; + private static final double ANGLE_KD = 0.4; + private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_MAX_ACCELERATION = 20.0; + + // Characterization + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.85; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + DriveBase driveBase, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double x = MathUtil.applyDeadband(xSupplier.getAsDouble(), DEADBAND); + double y = MathUtil.applyDeadband(ySupplier.getAsDouble(), DEADBAND); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square rotation value for more precise control + x = Math.copySign(Math.pow(x, 2), x); + y = Math.copySign(Math.pow(y, 2), y); + omega = Math.copySign(Math.pow(omega, 2), omega); + + // Generate robot relative speeds + double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); + double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get(); + var speeds = + new ChassisSpeeds( + x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + omega * DriveConstants.maxAngularVelocityRadPerSec * angularVelocityScalar); + + // Convert to field relative + Rotation2d rotation = RobotState.getInstance().getRotation(); + rotation = AllianceFlipUtil.apply(rotation); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); + + // Apply speeds + driveBase.runVelocity(speeds); + }, + 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( + DriveBase driveBase, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + Supplier rotationSupplier) { + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + return Commands.run( + () -> { + // Apply deadband + double x = MathUtil.applyDeadband(xSupplier.getAsDouble(), DEADBAND); + double y = MathUtil.applyDeadband(ySupplier.getAsDouble(), DEADBAND); + + // Square rotation value for more precise control + x = Math.copySign(Math.pow(x, 2), x); + y = Math.copySign(Math.pow(y, 2), y); + + // Calculate angular speed + Rotation2d rotation = RobotState.getInstance().getRotation(); + double omega = + angleController.calculate( + rotation.getRadians(), rotationSupplier.get().getRadians()); + + // Generate robot relative speeds + double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); + var speeds = + new ChassisSpeeds( + x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + omega); + + // Convert to field relative + rotation = AllianceFlipUtil.apply(rotation); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); + + // Apply speeds + driveBase.runVelocity(speeds); + }, + driveBase) + .beforeStarting( + () -> angleController.reset(RobotState.getInstance().getRotation().getRadians())); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(DriveBase drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + SmartDashboard.putString("kS", formatter.format(kS)); + SmartDashboard.putString("kV", formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(DriveBase drive) { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + drive)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getGyroRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = drive.getGyroRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + + SmartDashboard.putString( + "Wheel Delta", formatter.format(wheelDelta) + " radians"); + SmartDashboard.putString( + "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); + SmartDashboard.putString( + "Wheel Radius", + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java new file mode 100644 index 0000000..b038147 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -0,0 +1,275 @@ +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.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.RobotState; +import frc.robot.util.LoggedTunableNumber; +import frc.robot.util.swerve.SwerveSetpointGenerator; +import java.util.Queue; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class DriveBase extends SubsystemBase { + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); + + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + + private final Queue m_timestampsQueue; + private final OdometryTimestampsInputAutoLogged m_timestampInputs = + new OdometryTimestampsInputAutoLogged(); + private final Alert gyroDisconnectedAlert = + new Alert( + "Disconnected gyro, using kinematic approximation as fallback.", Alert.AlertType.kError); + + private static final LoggedTunableNumber coastWaitTime = + new LoggedTunableNumber("Drive/CoastWaitTimeSeconds", 0.5); + private static final LoggedTunableNumber coastMetersPerSecondThreshold = + new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", .05); + + private final Timer lastMovementTimer = new Timer(); + + 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/ClosedLoopMode") + private boolean CLOSED_LOOP_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); + + m_timestampsQueue = OdometryManager.getInstance().getTimestampQueue(); + + // Start odometry thread + OdometryManager.getInstance().start(); + + lastMovementTimer.start(); + setBrakeMode(true); + } + + @Override + public void periodic() { + OdometryManager.odometryLock.lock(); + try { + // Update and log gyro Inputs + gyroIO.updateInputs(m_gyroInputs); + Logger.processInputs("Drive/Gyro", m_gyroInputs); + // Update and log only on modules + for (var module : modules) { + module.updateInputs(); + } + // Get current sample timestamps + m_timestampInputs.timestamps = + m_timestampsQueue.stream().mapToDouble((Double value) -> value).toArray(); + m_timestampsQueue.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]; + } + RobotState.getInstance() + .addOdometryObservation( + wheelPositions, + m_gyroInputs.connected ? m_gyroInputs.odometryYawPositions[i] : null, + timestamps[i]); + } + + // Disable brake mode a short duration after the robot is disabled + for (var module : modules) { + if (Math.abs(module.getVelocityMetersPerSec()) > coastMetersPerSecondThreshold.get()) { + lastMovementTimer.reset(); + break; + } + } + + if (DriverStation.isEnabled()) { + setBrakeMode(true); + } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) { + setBrakeMode(false); + } + + // Update current setpoint if not in velocity mode + if (!CLOSED_LOOP_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) { + CLOSED_LOOP_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(ish) line with the specified drive output. */ + public void runCharacterization(double output) { + CLOSED_LOOP_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..170e10f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,98 @@ +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.Constants; +import frc.robot.util.swerve.SwerveSetpointGenerator.ModuleLimits; +import lombok.Builder; + +public class DriveConstants { + public static final double odometryFrequencyHz = + Constants.getRobotMode() == Constants.RobotMode.SIM ? 50 : 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 maxAngularVelocityRadPerSec = + 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); + + public static final double mk4iDriveGearing = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public 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.16028737150729522)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .turnInverted(true) + .build(), + // FR + ModuleConfig.builder() + .turnMotorId(4) + .driveMotorId(5) + .encoderChannel(3) + .encoderOffset(Rotation2d.fromRadians(-0.1422097592800296)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .turnInverted(true) + .build(), + // BL + ModuleConfig.builder() + .turnMotorId(6) + .driveMotorId(7) + .encoderChannel(1) + .encoderOffset(Rotation2d.fromRadians(-3.009554996093968)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .turnInverted(true) + .build(), + // BR + ModuleConfig.builder() + .turnMotorId(8) + .driveMotorId(9) + .encoderChannel(0) + .encoderOffset(Rotation2d.fromRadians(2.559973505647124)) + .driveGearing(mk4iDriveGearing) + .turnGearing(mk4iTurnGearing) + .turnInverted(true) + .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, + boolean turnInverted) {} + + 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 new file mode 100644 index 0000000..416e243 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + + 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 new file mode 100644 index 0000000..0d67075 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,50 @@ +package frc.robot.subsystems.drive; + +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); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..13d1284 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,153 @@ +package frc.robot.subsystems.drive; + +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 edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +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.19700); + drivekV.initDefault(0.12941); + drivekP.initDefault(0.005); + drivekD.initDefault(0.0); + turnkP.initDefault(2.0); + turnkD.initDefault(0.05); + } + default -> { + drivekS.initDefault(0.11400); + drivekV.initDefault(0.84144); + drivekP.initDefault(0.1); + drivekD.initDefault(0.0); + turnkP.initDefault(10.0); + turnkD.initDefault(0.0); + } + } + } + + private final ModuleIO m_io; + private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + + @Getter private SwerveModulePosition[] odometryPositions; + + public Module(ModuleIO io, int index) { + m_io = io; + this.index = index; + + driveDisconnectedAlert = + new Alert("Disconnected drive motor on module " + index + ".", AlertType.kError); + turnDisconnectedAlert = + new Alert("Disconnected turn motor on module " + index + ".", AlertType.kError); + } + + 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); + } + + driveDisconnectedAlert.set(!m_inputs.driveConnected); + turnDisconnectedAlert.set(!m_inputs.turnConnected); + } + + /** Runs the module with the specified setpoint state. */ + public void runSetpoint(SwerveModuleState state) { + m_io.runDriveVelocity(state.speedMetersPerSecond / DriveConstants.wheelRadius); + 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 new file mode 100644 index 0000000..6739091 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + 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/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..5719ed6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,135 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; +import java.util.Queue; + +public class ModuleIOSim implements ModuleIO { + private static final DCMotor driveMotorModel = DCMotor.getNEO(1); + private static final DCMotor turnMotorModel = DCMotor.getNEO(1); + + private final DCMotorSim driveSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + driveMotorModel, 0.025, DriveConstants.mk4iDriveGearing), + driveMotorModel); + private final DCMotorSim turnSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(turnMotorModel, 0.004, DriveConstants.mk4iTurnGearing), + turnMotorModel); + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + + private final PIDController driveController = new PIDController(0, 0, 0); + private final PIDController turnController = new PIDController(0, 0, 0); + private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + + private double driveAppliedVolts = 0.0; + private double driveFFVolts = 0.0; + private double turnAppliedVolts = 0.0; + + // Queue inputs from odometry thread + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + public ModuleIOSim() { + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + + drivePositionQueue = + OdometryManager.getInstance().registerSignal(driveSim::getAngularPositionRad); + turnPositionQueue = + OdometryManager.getInstance().registerSignal(turnSim::getAngularPositionRad); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = + driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); + } else { + turnController.reset(); + } + + // Update simulation state + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(Constants.kLoopPeriodSecs); + turnSim.update(Constants.kLoopPeriodSecs); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); + drivePositionQueue.clear(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + turnPositionQueue.clear(); + } + + @Override + public void runDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void runTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void runDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = driveFeedforward.calculate(velocityRadPerSec); + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void runTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveController.setPID(kP, kI, kD); + } + + @Override + public void setDriveFF(double kS, double kV) { + driveFeedforward = new SimpleMotorFeedforward(kS, kV); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnController.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java new file mode 100644 index 0000000..c2c45e7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -0,0 +1,218 @@ +package frc.robot.subsystems.drive; + +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.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.AnalogEncoder; +import frc.robot.Constants; +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; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + + 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(), 2 * Math.PI, 0); + + 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) + .inverted(config.turnInverted()) + .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) + .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); + + // We run PID on the controller, so its requires to tell it what the current true position is + // based on the AbsoluteEncoder + turnEncoder.setPosition(getOffsetAbsoluteAngle().getRadians()); + + 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.driveConnected = driveConnectedDebounce.calculate(driveSpark.hasActiveFault()); + inputs.turnConnected = turnConnectedDebounce.calculate(turnSpark.hasActiveFault()); + + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); + drivePositionQueue.clear(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + 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) { + // Because internal encoder is relative, we want to wrap to the range -π to π radians. + 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 new file mode 100644 index 0000000..bd94521 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -0,0 +1,86 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; +import lombok.Getter; +import org.littletonrobotics.junction.AutoLog; + +public class OdometryManager implements AutoCloseable { + public static Lock odometryLock = + new ReentrantLock(); // Prevent conflicts when reading and writing data + + private static OdometryManager instance = null; + + public static OdometryManager getInstance() { + if (instance == null) { + instance = new OdometryManager(); + } + return instance; + } + + @Getter private final Queue timestampQueue = new ArrayBlockingQueue<>(20); + private final List signalSuppliers = new ArrayList<>(9); + private final List> signalQueues = new ArrayList<>(9); + + private final Notifier notifier = new Notifier(this::run); + + private OdometryManager() { + notifier.setName("Odometry Data Collection Thread"); + } + + 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 void run() { + odometryLock.lock(); + try { + // Get sample timestamp + double timestamp = RobotController.getFPGATime() / 1e6; + timestampQueue.offer(timestamp); + + // Read signals and provide them to queues + for (int i = 0; i < signalSuppliers.size(); i++) { + signalQueues.get(i).offer(signalSuppliers.get(i).getAsDouble()); + } + } finally { + odometryLock.unlock(); + } + } + + public void start() throws IllegalStateException { + // Check that any sources are supplied + if (signalSuppliers.isEmpty()) { + throw new IllegalStateException( + "Tried to start OdometryManager, but no sources have been set."); + } + + notifier.startPeriodic(1.0 / DriveConstants.odometryFrequencyHz); + } + + @Override + public void close() { + notifier.stop(); + notifier.close(); + } + + @AutoLog + public static class OdometryTimestampsInput { + public double[] timestamps; + } +} 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/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java new file mode 100644 index 0000000..e839a5c --- /dev/null +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -0,0 +1,22 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Twist2d; + +public class EqualsUtil { + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, 1e-9); + } + + /** Extension methods for wpi geometry objects */ + public static class GeomExtensions { + public static boolean epsilonEquals(Twist2d twist, Twist2d other) { + return EqualsUtil.epsilonEquals(twist.dx, other.dx) + && EqualsUtil.epsilonEquals(twist.dy, other.dy) + && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); + } + } +} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 0000000..f50dcad --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,154 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x coordinate of the translation + * @param y The y coordinate of the translation + * @return The resulting transform + */ + public static Transform2d toTransform2d(double x, double y) { + return new Transform2d(x, y, new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + public static Pose3d toPose3d(Translation3d translation) { + return new Pose3d(translation, new Rotation3d()); + } + + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } + + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 0000000..5d7bfe7 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,159 @@ +package frc.robot.util; + +import frc.robot.Constants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private Double defaultValue = null; + + private final Map lastValues = new HashMap<>(); + + private LoggedNetworkNumber dashboardNumber; + + private final boolean ntPubEnabled; + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode + */ + public LoggedTunableNumber(String dashboardKey, boolean alwaysEnabled) { + this.key = tableKey + "/" + dashboardKey; + this.ntPubEnabled = alwaysEnabled; + } + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this(dashboardKey, Constants.TUNING_MODE); + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue, boolean alwaysEnabled) { + this(dashboardKey, alwaysEnabled); + initDefault(defaultValue); + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + * @throws IllegalStateException If a default value has already been set either by the constructor + * or by a previous call to this method. + */ + public void initDefault(double defaultValue) { + if (this.defaultValue != null) { + throw new IllegalStateException( + String.format( + "[LoggedTunableNumber][%s] Has already been initialized with a default value.", key)); + } + + this.defaultValue = defaultValue; + if (ntPubEnabled) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + * @throws IllegalStateException If a default value hasn't been set yet. + */ + public double get() { + if (defaultValue == null) { + throw new IllegalStateException( + String.format( + "[LoggedTunableNumber][%s] Hasn't been initialized with a default value. Make sure to call initDefault or use the correct constructor.", + key)); + } + + return ntPubEnabled ? dashboardNumber.get() : defaultValue; + } + + /** + * Checks whether the number has changed since the last time this method was called. Returns true + * the first time this method is called. + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return Whether the value has changed since the last time this method was called + */ + public boolean hasChanged(int id) { + double currentValue = get(); + var lastValue = lastValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Checks whether the number has changed since the last time this method was called. Returns true + * the first time this method is called. + * + * @apiNote This method assumes that there is only a single object is calling this method. For + * that use case, see {@link #hasChanged(int)}. + * @return Whether the value has changed since the last time this method was called + */ + public boolean hasChanged() { + return hasChanged(0); + } + + /** + * Run callback if any tunable number has changed. See {@link #hasChanged(int)} for usage. + * + * @param action action to run + * @param tunableNumbers tunable numbers to check + */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(v -> v.hasChanged(id))) { + action.run(); + } + } + + /** + * Run callback if any tunable number has changed. See {@link #hasChanged()} for usage. + * + * @param action action to run + * @param tunableNumbers tunable numbers to check + */ + public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(LoggedTunableNumber::hasChanged)) { + action.run(); + } + } +} 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 new file mode 100644 index 0000000..bc9a57e --- /dev/null +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -0,0 +1,403 @@ +package frc.robot.util.swerve; + +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.SwerveModuleState; +import frc.robot.util.EqualsUtil; +import frc.robot.util.GeomUtil; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import lombok.Builder; +import lombok.RequiredArgsConstructor; +import lombok.experimental.ExtensionMethod; + +// TODO JNI This +/** + * Ripped from 6328 + * swerve setpoint smoothing. + * + *

"Inspired" by FRC team 254. + * + *

Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path + * follower), and outputs a new setpoint that respects all the kinematic constraints on module + * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the + * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is + * kinematically infeasible (and can result in wheel slip or robot heading drift as a result). + */ +@Builder +@RequiredArgsConstructor +@ExtensionMethod({GeomUtil.class, EqualsUtil.GeomExtensions.class}) +public class SwerveSetpointGenerator { + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + if (iterations_left < 0 || EqualsUtil.epsilonEquals(f_0, f_1)) { + return 1.0; + } + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + protected double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation, + int max_iterations) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = + (x, y) -> { + return unwrapAngle(f_0, Math.atan2(y, x)) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + protected double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step, + int max_iterations) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = + (x, y) -> { + return Math.hypot(x, y) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + // protected double findDriveMaxS( + // double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // // Our drive velocity between s=0 and s=1 is quadratic in s: + // // v^2 = ((x_1 - x_0) * s + x_0)^2 + ((y_1 - y_0) * s + y_0)^2 + // // = a * s^2 + b * s + c + // // Where: + // // a = (x_1 - x_0)^2 + (y_1 - y_0)^2 + // // b = 2 * x_0 * (x_1 - x_0) + 2 * y_0 * (y_1 - y_0) + // // c = x_0^2 + y_0^2 + // // We want to find where this quadratic results in a velocity that is > max_vel_step from our + // // velocity at s=0: + // // sqrt(x_0^2 + y_0^2) +/- max_vel_step = ...quadratic... + // final double dx = x_1 - x_0; + // final double dy = y_1 - y_0; + // final double a = dx * dx + dy * dy; + // final double b = 2.0 * x_0 * dx + 2.0 * y_0 * dy; + // final double c = x_0 * x_0 + y_0 * y_0; + // final double v_limit_upper_2 = Math.pow(Math.hypot(x_0, y_0) + max_vel_step, 2.0); + // final double v_limit_lower_2 = Math.pow(Math.hypot(x_0, y_0) - max_vel_step, 2.0); + // return 0.0; + // } + + /** + * Generate a new setpoint. + * + * @param limits The kinematic limits to respect for this setpoint. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @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 the KinematicLimits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final ModuleLimits limits, + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredState, + double dt) { + final Translation2d[] modules = moduleLocations; + + SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); + // Make sure desiredState respects velocity limits. + if (limits.maxDriveVelocity() > 0.0) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); + desiredState = kinematics.toChassisSpeeds(desiredModuleState); + } + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + need_to_steer = false; + for (int i = 0; i < modules.length; ++i) { + desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; + desiredModuleState[i].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[modules.length]; + double[] prev_vy = new double[modules.length]; + Rotation2d[] prev_heading = new Rotation2d[modules.length]; + double[] desired_vx = new double[modules.length]; + double[] desired_vy = new double[modules.length]; + Rotation2d[] desired_heading = new Rotation2d[modules.length]; + boolean all_modules_should_flip = true; + for (int i = 0; i < modules.length; ++i) { + prev_vx[i] = + prevSetpoint.moduleStates()[i].angle.getCos() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vy[i] = + prevSetpoint.moduleStates()[i].angle.getSin() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_heading[i] = prevSetpoint.moduleStates()[i].angle; + if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { + prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + desired_vx[i] = + desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = + desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; + desired_heading[i] = desiredModuleState[i].angle; + if (desiredModuleState[i].speedMetersPerSecond < 0.0) { + desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) + && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired + // angle, and accelerate again. + return generateSetpoint(limits, prevSetpoint, new ChassisSpeeds(), dt); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then + // find the amount we can move from start towards goal in this cycle such that no kinematic + // limit is exceeded. + double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + double dtheta = + desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since + // inverse kinematics doesn't care about angle, we can be opportunistically lazy). + List> overrideSteering = new ArrayList<>(modules.length); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, + // and then backing out the maximum interpolant between start and goal states. We remember the + // minimum across all modules, since + // that is the active constraint. + final double max_theta_step = dt * limits.maxSteeringVelocity(); + for (int i = 0; i < modules.length; ++i) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + if (EqualsUtil.epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based + // purely on rotation in place. + if (EqualsUtil.epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); + } + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); + // Don't limit the global min_s; + continue; + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + i, + Optional.of( + prevSetpoint.moduleStates()[i].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); + min_s = 0.0; + continue; + } + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + final int kMaxIterations = 8; + double s = + findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel acceleration limits. + final double max_vel_step = dt * limits.maxDriveAcceleration(); + for (int i = 0; i < modules.length; ++i) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + double vx_min_s = + min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = + min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster + // than that. + final int kMaxIterations = 10; + double s = + min_s + * findDriveMaxS( + prev_vx[i], + prev_vy[i], + Math.hypot(prev_vx[i], prev_vy[i]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + max_vel_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); + var retStates = kinematics.toSwerveModuleStates(retSpeeds); + for (int i = 0; i < modules.length; ++i) { + final var maybeOverride = overrideSteering.get(i); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { + retStates[i].speedMetersPerSecond *= -1.0; + } + retStates[i].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); + if (flipHeading(deltaRotation)) { + retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); + retStates[i].speedMetersPerSecond *= -1.0; + } + } + return new SwerveSetpoint(retSpeeds, retStates); + } + + public record ModuleLimits( + double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} + + public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {} +} diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index 51d0083..820c61a 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,419 +1,419 @@ { - "fileName": "Phoenix6-frc2025-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.2.1", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.2.1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] } 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, diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index c991b6a..6beb912 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,7 +1,7 @@ { "fileName": "URCL.json", "name": "URCL", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "URCL", "headerClassifier": "headers", "sharedLibrary": false, @@ -49,7 +49,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "URCLDriver", "headerClassifier": "headers", "sharedLibrary": false, From 98c071538f2695fa8bae86db29b2520814843909 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 5 Feb 2025 13:21:38 -0500 Subject: [PATCH 05/73] Fix Modules Falsely Reporting as Disconnected --- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index c2c45e7..a96e3ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -141,8 +141,8 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); - inputs.driveConnected = driveConnectedDebounce.calculate(driveSpark.hasActiveFault()); - inputs.turnConnected = turnConnectedDebounce.calculate(turnSpark.hasActiveFault()); + inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault()); + inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault()); inputs.odometryDrivePositionsRad = drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); From 1cc2430a80c8e3bc42f9ffda1be27114f37ac568 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 21:42:25 -0500 Subject: [PATCH 06/73] Cleanup typing, make errors more obvious --- src/main/java/frc/robot/Constants.java | 50 +++++++++---------- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/drive/DriveBase.java | 3 +- .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 4 +- .../robot/subsystems/drive/ModuleIOSpark.java | 6 +-- src/main/java/frc/robot/util/LoggerUtil.java | 2 +- 8 files changed, 35 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 942f1bd..6a184de 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,10 +1,10 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotBase; public class Constants { - private static RobotType kRobotType = RobotType.ROBOT_2025_COMP; + private static RobotType robotType = RobotType.SIMBOT; // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running @@ -12,36 +12,36 @@ public class Constants { public static final double kLoopPeriodSecs = 0.02; - public enum RobotMode { - REAL, - SIM, - REPLAY + public static RobotType getRobot() { + if (RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert( + "Invalid robot selected, using competition robot as default.", Alert.AlertType.kError) + .set(true); + robotType = RobotType.COMPBOT; + } + return robotType; } - public enum RobotType { - ROBOT_2025_COMP, - ROBOT_SIMBOT + public static Mode getMode() { + return switch (robotType) { + case COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; } - public static RobotType getRobotType() { - if (RobotBase.isReal() && kRobotType == RobotType.ROBOT_SIMBOT) { - DriverStation.reportError( - "Robot is set to SIM but it isn't a SIM, setting it to Competition Robot as redundancy.", - false); - kRobotType = RobotType.ROBOT_2025_COMP; - } + public enum Mode { + /** Running on a real robot. */ + REAL, - if (RobotBase.isSimulation() && kRobotType != RobotType.ROBOT_SIMBOT) { - DriverStation.reportError( - "Robot is set to REAL but it is a SIM, setting it to SIMBOT as redundancy.", false); - kRobotType = RobotType.ROBOT_SIMBOT; - } + /** Running a physics simulator. */ + SIM, - return kRobotType; + /** Replaying from a log file. */ + REPLAY } - public static RobotMode getRobotMode() { - if (getRobotType() == RobotType.ROBOT_SIMBOT) return RobotMode.SIM; - else return RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY; + public enum RobotType { + SIMBOT, + COMPBOT } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4bc69c8..8bbc602 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -56,7 +56,7 @@ public Robot() { LoggerUtil.initializeLoggerMetadata(); - switch (Constants.getRobotMode()) { + switch (Constants.getMode()) { case REAL -> { // Running on a real robot, log to a USB stick var loggerPath = LoggerUtil.getLogPath(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0e2bfb2..8be4507 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,7 +28,7 @@ public class RobotContainer { private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo); public RobotContainer() { - switch (Constants.getRobotMode()) { + switch (Constants.getMode()) { case REAL -> { driveBase = new DriveBase( diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index b038147..5db3d4e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -154,8 +154,7 @@ public void periodic() { } // Update gyro alert - gyroDisconnectedAlert.set( - !m_gyroInputs.connected && Constants.getRobotMode() != Constants.RobotMode.SIM); + gyroDisconnectedAlert.set(!m_gyroInputs.connected && Constants.getMode() != Constants.Mode.SIM); } /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */ diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 170e10f..093576b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -9,7 +9,7 @@ public class DriveConstants { public static final double odometryFrequencyHz = - Constants.getRobotMode() == Constants.RobotMode.SIM ? 50 : 250; + Constants.getMode() == Constants.Mode.SIM ? 50 : 250; public static final double trackWidthX = Units.inchesToMeters(20.75); public static final double trackWidthY = Units.inchesToMeters(20.75); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 13d1284..f3caef7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -24,8 +24,8 @@ public class Module { private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); static { - switch (Constants.getRobotType()) { - case ROBOT_2025_COMP -> { + switch (Constants.getRobot()) { + case COMPBOT -> { drivekS.initDefault(0.19700); drivekV.initDefault(0.12941); drivekP.initDefault(0.005); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index a96e3ff..9cf0ef4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -46,13 +46,13 @@ public class ModuleIOSpark implements ModuleIO { private final Debouncer turnConnectedDebounce = new Debouncer(0.5); public ModuleIOSpark(int index) { - switch (Constants.getRobotType()) { - case ROBOT_2025_COMP -> { + switch (Constants.getRobot()) { + case COMPBOT -> { config = moduleConfigs[index]; } default -> throw new IllegalStateException( - "Unexpected RobotType for Spark Module: " + Constants.getRobotType()); + "Unexpected RobotType for Spark Module: " + Constants.getRobot()); } // Initialize Hardware Devices diff --git a/src/main/java/frc/robot/util/LoggerUtil.java b/src/main/java/frc/robot/util/LoggerUtil.java index b84fb7f..4a5bbfa 100644 --- a/src/main/java/frc/robot/util/LoggerUtil.java +++ b/src/main/java/frc/robot/util/LoggerUtil.java @@ -11,7 +11,7 @@ public class LoggerUtil { /** Initialize the Logger with the auto-generated data from the build. */ public static void initializeLoggerMetadata() { // Record metadata from generated state file. - Logger.recordMetadata("ROBOT_NAME", Constants.getRobotType().toString()); + Logger.recordMetadata("ROBOT_NAME", Constants.getRobot().toString()); Logger.recordMetadata("RUNTIME_ENVIRONMENT", RobotBase.getRuntimeType().toString()); Logger.recordMetadata("TUNING_MODE", Boolean.toString(Constants.TUNING_MODE)); Logger.recordMetadata("PROJECT_NAME", BuildConstants.MAVEN_NAME); From 803498a62e7d6a01598a80db76b12c4ea44af728 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 21:42:46 -0500 Subject: [PATCH 07/73] Cleanup dashboard setting stuff --- .../frc/robot/commands/DriveCommands.java | 13 +++++--- .../frc/robot/util/LoggedTunableNumber.java | 31 ++----------------- .../util/swerve/SwerveSetpointGenerator.java | 1 - 3 files changed, 11 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 26acb14..b1c7ce5 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -15,22 +15,22 @@ import frc.robot.subsystems.drive.DriveBase; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; -import frc.robot.util.LoggedTunableNumber; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class DriveCommands { // Drive private static final double DEADBAND = 0.1; - private static final LoggedTunableNumber LINEAR_VELOCITY_SCALAR = - new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0, true); - private static final LoggedTunableNumber ANGULAR_VELOCITY_SCALAR = - new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0, true); + private static final LoggedNetworkNumber LINEAR_VELOCITY_SCALAR = + new LoggedNetworkNumber("TeleopDrive/LinearVelocityScalar", 1.0); + private static final LoggedNetworkNumber ANGULAR_VELOCITY_SCALAR = + new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 1.0); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; @@ -66,6 +66,9 @@ public static Command joystickDrive( // Generate robot relative speeds double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get(); + + SmartDashboard.putNumber("current_linear_scalar", LINEAR_VELOCITY_SCALAR.get()); + var speeds = new ChassisSpeeds( x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 5d7bfe7..71e4c88 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -20,38 +20,13 @@ public class LoggedTunableNumber { private LoggedNetworkNumber dashboardNumber; - private final boolean ntPubEnabled; - - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode - */ - public LoggedTunableNumber(String dashboardKey, boolean alwaysEnabled) { - this.key = tableKey + "/" + dashboardKey; - this.ntPubEnabled = alwaysEnabled; - } - /** * Create a new LoggedTunableNumber * * @param dashboardKey Key on dashboard */ public LoggedTunableNumber(String dashboardKey) { - this(dashboardKey, Constants.TUNING_MODE); - } - - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue, boolean alwaysEnabled) { - this(dashboardKey, alwaysEnabled); - initDefault(defaultValue); + this.key = tableKey + "/" + dashboardKey; } /** @@ -80,7 +55,7 @@ public void initDefault(double defaultValue) { } this.defaultValue = defaultValue; - if (ntPubEnabled) { + if (Constants.TUNING_MODE) { dashboardNumber = new LoggedNetworkNumber(key, defaultValue); } } @@ -99,7 +74,7 @@ public double get() { key)); } - return ntPubEnabled ? dashboardNumber.get() : defaultValue; + return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue; } /** diff --git a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java index cb2982a..85f1ae2 100644 --- a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -2,7 +2,6 @@ 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.SwerveModuleState; From 15e8caf8a87b3117208c2d49c3bfa086356575df Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 21:43:07 -0500 Subject: [PATCH 08/73] Update sim characterization constants to be more accurate, stops a massive overshooting for some reason --- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- src/main/java/frc/robot/subsystems/drive/Module.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index b1c7ce5..72bfe99 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,7 +30,7 @@ public class DriveCommands { private static final LoggedNetworkNumber LINEAR_VELOCITY_SCALAR = new LoggedNetworkNumber("TeleopDrive/LinearVelocityScalar", 1.0); private static final LoggedNetworkNumber ANGULAR_VELOCITY_SCALAR = - new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 1.0); + new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 0.7); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index f3caef7..457c216 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -34,8 +34,8 @@ public class Module { turnkD.initDefault(0.05); } default -> { - drivekS.initDefault(0.11400); - drivekV.initDefault(0.84144); + drivekS.initDefault(0.113190); + drivekV.initDefault(0.841640); drivekP.initDefault(0.1); drivekD.initDefault(0.0); turnkP.initDefault(10.0); From 37cd26f8f16c0ff68b443e1e34f1b537ac1502c9 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 21:55:27 -0500 Subject: [PATCH 09/73] Log drive motor temps --- src/main/java/frc/robot/subsystems/drive/ModuleIO.java | 2 ++ src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 6739091..4f2c590 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -11,6 +11,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; + public double driveTemperatureCelsius = 0.0; public boolean turnConnected = false; public Rotation2d turnAbsolutePosition = new Rotation2d(); @@ -18,6 +19,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; + public double turnTemperatureCelsius = 0.0; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 9cf0ef4..1dbb06f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -134,12 +134,14 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage(); inputs.driveCurrentAmps = driveSpark.getOutputCurrent(); + inputs.driveTemperatureCelsius = driveSpark.getMotorTemperature(); inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition()); inputs.turnVelocityRadPerSec = turnEncoder.getVelocity(); inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); + inputs.turnTemperatureCelsius = turnSpark.getMotorTemperature(); inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault()); inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault()); From addcdcebe1579fa6625bd3a10573b4d6e567c1f4 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 22:04:53 -0500 Subject: [PATCH 10/73] Formatting fixes --- src/main/java/frc/robot/util/EqualsUtil.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java index 797f9e0..a267032 100644 --- a/src/main/java/frc/robot/util/EqualsUtil.java +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -18,7 +18,7 @@ public static boolean epsilonEquals(Twist2d twist, Twist2d other) { && EqualsUtil.epsilonEquals(twist.dy, other.dy) && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); } - + public static boolean equalsZero(Twist2d twist) { return EqualsUtil.epsilonEquals(twist.dx, 0.0) && EqualsUtil.epsilonEquals(twist.dy, 0.0) From f7606cf1df2482dd259d1795aac8e2c650479f3f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 22:07:08 -0500 Subject: [PATCH 11/73] Inline tuning mode alert --- src/main/java/frc/robot/RobotContainer.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8be4507..60bbb9d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,9 +24,6 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; - // Alerts - private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo); - public RobotContainer() { switch (Constants.getMode()) { case REAL -> { @@ -62,7 +59,7 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices"); if (Constants.TUNING_MODE) { - tuningModeAlert.set(true); + new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo).set(true); // Set up Characterization routines autoChooser.addOption( From 9325f68f55fe599daf580740b542113f3ed9cf2b Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 22:07:52 -0500 Subject: [PATCH 12/73] Misc drive fixes (#6) * Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI Co-Authored-By: Talon540-root <122660543+Talon540-root@users.noreply.github.com> * Increase voltage warning on battery and add CAN error alert * clean * Fix Modules Falsely Reporting as Disconnected * Cleanup typing, make errors more obvious * Cleanup dashboard setting stuff * Update sim characterization constants to be more accurate, stops a massive overshooting for some reason * Log drive motor temps * Formatting fixes * Inline tuning mode alert --------- Co-authored-by: Talon540-root <122660543+Talon540-root@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 52 +++++++++---------- src/main/java/frc/robot/Robot.java | 20 +++++-- src/main/java/frc/robot/RobotContainer.java | 5 +- .../frc/robot/commands/DriveCommands.java | 11 ++-- .../frc/robot/subsystems/drive/DriveBase.java | 3 +- .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 8 +-- .../frc/robot/subsystems/drive/ModuleIO.java | 2 + .../robot/subsystems/drive/ModuleIOSpark.java | 12 +++-- src/main/java/frc/robot/util/EqualsUtil.java | 6 +++ .../frc/robot/util/LoggedTunableNumber.java | 31 ++--------- src/main/java/frc/robot/util/LoggerUtil.java | 2 +- .../util/swerve/SwerveSetpointGenerator.java | 33 ++++++------ 13 files changed, 92 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed989ab..7de34c6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,10 +1,10 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotBase; public class Constants { - private static RobotType kRobotType = RobotType.ROBOT_2025_COMP; + private static RobotType robotType = RobotType.COMPBOT; // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running @@ -12,38 +12,36 @@ public class Constants { public static final double kLoopPeriodSecs = 0.02; - public static final double LOW_VOLTAGE_WARNING_THRESHOLD = 10.0; - - public enum RobotMode { - REAL, - SIM, - REPLAY + public static RobotType getRobot() { + if (RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert( + "Invalid robot selected, using competition robot as default.", Alert.AlertType.kError) + .set(true); + robotType = RobotType.COMPBOT; + } + return robotType; } - public enum RobotType { - ROBOT_2025_COMP, - ROBOT_SIMBOT + public static Mode getMode() { + return switch (robotType) { + case COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; } - public static RobotType getRobotType() { - if (RobotBase.isReal() && kRobotType == RobotType.ROBOT_SIMBOT) { - DriverStation.reportError( - "Robot is set to SIM but it isn't a SIM, setting it to Competition Robot as redundancy.", - false); - kRobotType = RobotType.ROBOT_2025_COMP; - } + public enum Mode { + /** Running on a real robot. */ + REAL, - if (RobotBase.isSimulation() && kRobotType != RobotType.ROBOT_SIMBOT) { - DriverStation.reportError( - "Robot is set to REAL but it is a SIM, setting it to SIMBOT as redundancy.", false); - kRobotType = RobotType.ROBOT_SIMBOT; - } + /** Running a physics simulator. */ + SIM, - return kRobotType; + /** Replaying from a log file. */ + REPLAY } - public static RobotMode getRobotMode() { - if (getRobotType() == RobotType.ROBOT_SIMBOT) return RobotMode.SIM; - else return RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY; + public enum RobotType { + SIMBOT, + COMPBOT } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 64b446b..8bbc602 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; @@ -36,19 +37,26 @@ * project. */ public class Robot extends LoggedRobot { + private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; + private Command autonomousCommand; private final RobotContainer robotContainer; + // System Alerts + private final Alert canErrorAlert = + new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + private final Debouncer canErrorDebouncer = new Debouncer(0.5); + private final Alert lowBatteryVoltageAlert = new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning); - private final Debouncer batteryVoltageDebouncer = new Debouncer(0.5); + private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); public Robot() { super(Constants.kLoopPeriodSecs); LoggerUtil.initializeLoggerMetadata(); - switch (Constants.getRobotMode()) { + switch (Constants.getMode()) { case REAL -> { // Running on a real robot, log to a USB stick var loggerPath = LoggerUtil.getLogPath(); @@ -92,10 +100,16 @@ public void robotPeriodic() { // Run command scheduler CommandScheduler.getInstance().run(); + // Check CAN status + var canStatus = RobotController.getCANStatus(); + canErrorAlert.set( + canErrorDebouncer.calculate( + canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); + // Update Battery Voltage Alert lowBatteryVoltageAlert.set( batteryVoltageDebouncer.calculate( - RobotController.getBatteryVoltage() <= Constants.LOW_VOLTAGE_WARNING_THRESHOLD)); + RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fc75b6..60bbb9d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -24,7 +25,7 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooser; public RobotContainer() { - switch (Constants.getRobotMode()) { + switch (Constants.getMode()) { case REAL -> { driveBase = new DriveBase( @@ -58,6 +59,8 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices"); if (Constants.TUNING_MODE) { + new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo).set(true); + // Set up Characterization routines autoChooser.addOption( "Drive Wheel Radius Characterization", diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 26acb14..ea9d77d 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -15,22 +15,22 @@ import frc.robot.subsystems.drive.DriveBase; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; -import frc.robot.util.LoggedTunableNumber; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class DriveCommands { // Drive private static final double DEADBAND = 0.1; - private static final LoggedTunableNumber LINEAR_VELOCITY_SCALAR = - new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0, true); - private static final LoggedTunableNumber ANGULAR_VELOCITY_SCALAR = - new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0, true); + private static final LoggedNetworkNumber LINEAR_VELOCITY_SCALAR = + new LoggedNetworkNumber("TeleopDrive/LinearVelocityScalar", 1.0); + private static final LoggedNetworkNumber ANGULAR_VELOCITY_SCALAR = + new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 0.7); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; @@ -66,6 +66,7 @@ public static Command joystickDrive( // Generate robot relative speeds double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get(); + var speeds = new ChassisSpeeds( x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index b038147..5db3d4e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -154,8 +154,7 @@ public void periodic() { } // Update gyro alert - gyroDisconnectedAlert.set( - !m_gyroInputs.connected && Constants.getRobotMode() != Constants.RobotMode.SIM); + gyroDisconnectedAlert.set(!m_gyroInputs.connected && Constants.getMode() != Constants.Mode.SIM); } /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */ diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 170e10f..093576b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -9,7 +9,7 @@ public class DriveConstants { public static final double odometryFrequencyHz = - Constants.getRobotMode() == Constants.RobotMode.SIM ? 50 : 250; + Constants.getMode() == Constants.Mode.SIM ? 50 : 250; public static final double trackWidthX = Units.inchesToMeters(20.75); public static final double trackWidthY = Units.inchesToMeters(20.75); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 13d1284..457c216 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -24,8 +24,8 @@ public class Module { private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); static { - switch (Constants.getRobotType()) { - case ROBOT_2025_COMP -> { + switch (Constants.getRobot()) { + case COMPBOT -> { drivekS.initDefault(0.19700); drivekV.initDefault(0.12941); drivekP.initDefault(0.005); @@ -34,8 +34,8 @@ public class Module { turnkD.initDefault(0.05); } default -> { - drivekS.initDefault(0.11400); - drivekV.initDefault(0.84144); + drivekS.initDefault(0.113190); + drivekV.initDefault(0.841640); drivekP.initDefault(0.1); drivekD.initDefault(0.0); turnkP.initDefault(10.0); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 6739091..4f2c590 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -11,6 +11,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; + public double driveTemperatureCelsius = 0.0; public boolean turnConnected = false; public Rotation2d turnAbsolutePosition = new Rotation2d(); @@ -18,6 +19,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; + public double turnTemperatureCelsius = 0.0; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index c2c45e7..1dbb06f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -46,13 +46,13 @@ public class ModuleIOSpark implements ModuleIO { private final Debouncer turnConnectedDebounce = new Debouncer(0.5); public ModuleIOSpark(int index) { - switch (Constants.getRobotType()) { - case ROBOT_2025_COMP -> { + switch (Constants.getRobot()) { + case COMPBOT -> { config = moduleConfigs[index]; } default -> throw new IllegalStateException( - "Unexpected RobotType for Spark Module: " + Constants.getRobotType()); + "Unexpected RobotType for Spark Module: " + Constants.getRobot()); } // Initialize Hardware Devices @@ -134,15 +134,17 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage(); inputs.driveCurrentAmps = driveSpark.getOutputCurrent(); + inputs.driveTemperatureCelsius = driveSpark.getMotorTemperature(); inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition()); inputs.turnVelocityRadPerSec = turnEncoder.getVelocity(); inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); + inputs.turnTemperatureCelsius = turnSpark.getMotorTemperature(); - inputs.driveConnected = driveConnectedDebounce.calculate(driveSpark.hasActiveFault()); - inputs.turnConnected = turnConnectedDebounce.calculate(turnSpark.hasActiveFault()); + inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault()); + inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault()); inputs.odometryDrivePositionsRad = drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java index e839a5c..a267032 100644 --- a/src/main/java/frc/robot/util/EqualsUtil.java +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -18,5 +18,11 @@ public static boolean epsilonEquals(Twist2d twist, Twist2d other) { && EqualsUtil.epsilonEquals(twist.dy, other.dy) && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); } + + public static boolean equalsZero(Twist2d twist) { + return EqualsUtil.epsilonEquals(twist.dx, 0.0) + && EqualsUtil.epsilonEquals(twist.dy, 0.0) + && EqualsUtil.epsilonEquals(twist.dtheta, 0.0); + } } } diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 5d7bfe7..71e4c88 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -20,38 +20,13 @@ public class LoggedTunableNumber { private LoggedNetworkNumber dashboardNumber; - private final boolean ntPubEnabled; - - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode - */ - public LoggedTunableNumber(String dashboardKey, boolean alwaysEnabled) { - this.key = tableKey + "/" + dashboardKey; - this.ntPubEnabled = alwaysEnabled; - } - /** * Create a new LoggedTunableNumber * * @param dashboardKey Key on dashboard */ public LoggedTunableNumber(String dashboardKey) { - this(dashboardKey, Constants.TUNING_MODE); - } - - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue, boolean alwaysEnabled) { - this(dashboardKey, alwaysEnabled); - initDefault(defaultValue); + this.key = tableKey + "/" + dashboardKey; } /** @@ -80,7 +55,7 @@ public void initDefault(double defaultValue) { } this.defaultValue = defaultValue; - if (ntPubEnabled) { + if (Constants.TUNING_MODE) { dashboardNumber = new LoggedNetworkNumber(key, defaultValue); } } @@ -99,7 +74,7 @@ public double get() { key)); } - return ntPubEnabled ? dashboardNumber.get() : defaultValue; + return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue; } /** diff --git a/src/main/java/frc/robot/util/LoggerUtil.java b/src/main/java/frc/robot/util/LoggerUtil.java index b84fb7f..4a5bbfa 100644 --- a/src/main/java/frc/robot/util/LoggerUtil.java +++ b/src/main/java/frc/robot/util/LoggerUtil.java @@ -11,7 +11,7 @@ public class LoggerUtil { /** Initialize the Logger with the auto-generated data from the build. */ public static void initializeLoggerMetadata() { // Record metadata from generated state file. - Logger.recordMetadata("ROBOT_NAME", Constants.getRobotType().toString()); + Logger.recordMetadata("ROBOT_NAME", Constants.getRobot().toString()); Logger.recordMetadata("RUNTIME_ENVIRONMENT", RobotBase.getRuntimeType().toString()); Logger.recordMetadata("TUNING_MODE", Boolean.toString(Constants.TUNING_MODE)); Logger.recordMetadata("PROJECT_NAME", BuildConstants.MAVEN_NAME); diff --git a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java index bc9a57e..85f1ae2 100644 --- a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -2,7 +2,6 @@ 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.SwerveModuleState; @@ -195,8 +194,6 @@ public SwerveSetpoint generateSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredState, double dt) { - final Translation2d[] modules = moduleLocations; - SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); // Make sure desiredState respects velocity limits. if (limits.maxDriveVelocity() > 0.0) { @@ -207,23 +204,23 @@ public SwerveSetpoint generateSetpoint( // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so // just use the previous angle. boolean need_to_steer = true; - if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + if (desiredState.toTwist2d().equalsZero()) { need_to_steer = false; - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; desiredModuleState[i].speedMetersPerSecond = 0.0; } } // For each module, compute local Vx and Vy vectors. - double[] prev_vx = new double[modules.length]; - double[] prev_vy = new double[modules.length]; - Rotation2d[] prev_heading = new Rotation2d[modules.length]; - double[] desired_vx = new double[modules.length]; - double[] desired_vy = new double[modules.length]; - Rotation2d[] desired_heading = new Rotation2d[modules.length]; + double[] prev_vx = new double[moduleLocations.length]; + double[] prev_vy = new double[moduleLocations.length]; + Rotation2d[] prev_heading = new Rotation2d[moduleLocations.length]; + double[] desired_vx = new double[moduleLocations.length]; + double[] desired_vy = new double[moduleLocations.length]; + Rotation2d[] desired_heading = new Rotation2d[moduleLocations.length]; boolean all_modules_should_flip = true; - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { prev_vx[i] = prevSetpoint.moduleStates()[i].angle.getCos() * prevSetpoint.moduleStates()[i].speedMetersPerSecond; @@ -251,8 +248,8 @@ public SwerveSetpoint generateSetpoint( } } if (all_modules_should_flip - && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) - && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + && !prevSetpoint.chassisSpeeds().toTwist2d().equalsZero() + && !desiredState.toTwist2d().equalsZero()) { // It will (likely) be faster to stop the robot, rotate the modules in place to the complement // of the desired // angle, and accelerate again. @@ -275,14 +272,14 @@ public SwerveSetpoint generateSetpoint( // In cases where an individual module is stopped, we want to remember the right steering angle // to command (since // inverse kinematics doesn't care about angle, we can be opportunistically lazy). - List> overrideSteering = new ArrayList<>(modules.length); + List> overrideSteering = new ArrayList<>(moduleLocations.length); // Enforce steering velocity limits. We do this by taking the derivative of steering angle at // the current angle, // and then backing out the maximum interpolant between start and goal states. We remember the // minimum across all modules, since // that is the active constraint. final double max_theta_step = dt * limits.maxSteeringVelocity(); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { if (!need_to_steer) { overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); continue; @@ -344,7 +341,7 @@ public SwerveSetpoint generateSetpoint( // Enforce drive wheel acceleration limits. final double max_vel_step = dt * limits.maxDriveAcceleration(); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { if (min_s == 0.0) { // No need to carry on. break; @@ -377,7 +374,7 @@ public SwerveSetpoint generateSetpoint( prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); var retStates = kinematics.toSwerveModuleStates(retSpeeds); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { final var maybeOverride = overrideSteering.get(i); if (maybeOverride.isPresent()) { var override = maybeOverride.get(); From 876046fc313910b509223089d7cc4a642dcb343d Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 23:42:36 -0500 Subject: [PATCH 13/73] Rename RobotState to PoseEstimator Because only vision and drive will interact with pose (and purely position) and no other robot state is tracked, it makes more sense for this to be renammed to reflect that. --- .../robot/{RobotState.java => PoseEstimator.java} | 14 +++++++------- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../java/frc/robot/commands/DriveCommands.java | 8 ++++---- .../java/frc/robot/subsystems/drive/DriveBase.java | 4 ++-- 4 files changed, 17 insertions(+), 17 deletions(-) rename src/main/java/frc/robot/{RobotState.java => PoseEstimator.java} (91%) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/PoseEstimator.java similarity index 91% rename from src/main/java/frc/robot/RobotState.java rename to src/main/java/frc/robot/PoseEstimator.java index 149fb4e..7c2618f 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/PoseEstimator.java @@ -15,28 +15,28 @@ import lombok.Getter; import org.littletonrobotics.junction.AutoLogOutput; -public class RobotState { +public class PoseEstimator { // Standard deviations of the pose estimate (x position in meters, y position in meters, and // heading in radians). // Increase these numbers to trust your state estimate less. private static final Matrix odometryStateStdDevs = VecBuilder.fill(0.003, 0.003, 0.002); private static final double poseBufferSizeSec = 2.0; - private static RobotState instance; + private static PoseEstimator instance; - public static RobotState getInstance() { + public static PoseEstimator getInstance() { if (instance == null) { - instance = new RobotState(); + instance = new PoseEstimator(); } return instance; } @Getter - @AutoLogOutput(key = "RobotState/OdometryPose") + @AutoLogOutput(key = "PoseEstimator/OdometryPose") private Pose2d odometryPose = new Pose2d(); @Getter - @AutoLogOutput(key = "RobotState/EstimatedPose") + @AutoLogOutput(key = "PoseEstimator/EstimatedPose") private Pose2d estimatedPose = new Pose2d(); private final TimeInterpolatableBuffer poseBuffer = @@ -55,7 +55,7 @@ public static RobotState getInstance() { // Assume gyro starts at zero private Rotation2d gyroOffset = new Rotation2d(); - private RobotState() { + private PoseEstimator() { for (int i = 0; i < 3; ++i) { qStdDevs.set(i, 0, Math.pow(odometryStateStdDevs.get(i, 0), 2)); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 60bbb9d..28c1edb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,8 +12,8 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { - // Load RobotState class - private final RobotState robotState = RobotState.getInstance(); + // Load PoseEstimator class + private final PoseEstimator poseEstimator = PoseEstimator.getInstance(); // Subsystems private final DriveBase driveBase; @@ -100,10 +100,10 @@ private void configureButtonBindings() { .onTrue( Commands.runOnce( () -> - RobotState.getInstance() + PoseEstimator.getInstance() .resetPose( new Pose2d( - RobotState.getInstance().getEstimatedPose().getTranslation(), + PoseEstimator.getInstance().getEstimatedPose().getTranslation(), AllianceFlipUtil.apply(new Rotation2d()))), driveBase) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index ea9d77d..bcef915 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.RobotState; +import frc.robot.PoseEstimator; import frc.robot.subsystems.drive.DriveBase; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; @@ -74,7 +74,7 @@ public static Command joystickDrive( omega * DriveConstants.maxAngularVelocityRadPerSec * angularVelocityScalar); // Convert to field relative - Rotation2d rotation = RobotState.getInstance().getRotation(); + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); rotation = AllianceFlipUtil.apply(rotation); speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); @@ -113,7 +113,7 @@ public static Command joystickDriveAtAngle( y = Math.copySign(Math.pow(y, 2), y); // Calculate angular speed - Rotation2d rotation = RobotState.getInstance().getRotation(); + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); double omega = angleController.calculate( rotation.getRadians(), rotationSupplier.get().getRadians()); @@ -135,7 +135,7 @@ public static Command joystickDriveAtAngle( }, driveBase) .beforeStarting( - () -> angleController.reset(RobotState.getInstance().getRotation().getRadians())); + () -> angleController.reset(PoseEstimator.getInstance().getRotation().getRadians())); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index 5db3d4e..de2052a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.RobotState; +import frc.robot.PoseEstimator; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.swerve.SwerveSetpointGenerator; import java.util.Queue; @@ -126,7 +126,7 @@ public void periodic() { for (int j = 0; j < 4; j++) { wheelPositions[j] = modules[j].getOdometryPositions()[i]; } - RobotState.getInstance() + PoseEstimator.getInstance() .addOdometryObservation( wheelPositions, m_gyroInputs.connected ? m_gyroInputs.odometryYawPositions[i] : null, From 2ae942fab2a6df947bbd95de3acb054f21613be7 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 23:44:07 -0500 Subject: [PATCH 14/73] Refactor abstract alerts into util class to later roll into LEDs --- src/main/java/frc/robot/Robot.java | 26 +--------- src/main/java/frc/robot/RobotContainer.java | 6 +-- src/main/java/frc/robot/util/AlertsUtil.java | 54 ++++++++++++++++++++ 3 files changed, 58 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc/robot/util/AlertsUtil.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8bbc602..73c12cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,14 +13,12 @@ package frc.robot; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.AlertsUtil; import frc.robot.util.LoggerUtil; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -37,20 +35,9 @@ * project. */ public class Robot extends LoggedRobot { - private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; - private Command autonomousCommand; private final RobotContainer robotContainer; - // System Alerts - private final Alert canErrorAlert = - new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); - private final Debouncer canErrorDebouncer = new Debouncer(0.5); - - private final Alert lowBatteryVoltageAlert = - new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning); - private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); - public Robot() { super(Constants.kLoopPeriodSecs); @@ -100,16 +87,7 @@ public void robotPeriodic() { // Run command scheduler CommandScheduler.getInstance().run(); - // Check CAN status - var canStatus = RobotController.getCANStatus(); - canErrorAlert.set( - canErrorDebouncer.calculate( - canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); - - // Update Battery Voltage Alert - lowBatteryVoltageAlert.set( - batteryVoltageDebouncer.calculate( - RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + AlertsUtil.getInstance().periodic(); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 28c1edb..19198c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,12 +2,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.*; +import frc.robot.util.AlertsUtil; import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -59,8 +59,6 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices"); if (Constants.TUNING_MODE) { - new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo).set(true); - // Set up Characterization routines autoChooser.addOption( "Drive Wheel Radius Characterization", @@ -94,7 +92,7 @@ private void configureButtonBindings() { // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); - // Reset gyro to 0° when B button is pressed + // Reset gyro to 0° when B button is pressed controller .b() .onTrue( diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java new file mode 100644 index 0000000..0f51f72 --- /dev/null +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -0,0 +1,54 @@ +package frc.robot.util; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants; + +public class AlertsUtil { + private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; + + private static AlertsUtil instance; + + public static AlertsUtil getInstance() { + if (instance == null) { + instance = new AlertsUtil(); + } + + return instance; + } + + // System Alerts + private final Alert canErrorAlert = + new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + private final Debouncer canErrorDebouncer = new Debouncer(0.5); + + private final Alert lowBatteryVoltageAlert = + new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); + private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); + + // Program Alerts + private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); + + private AlertsUtil() { + if (Constants.TUNING_MODE) { + tuningModeAlert.set(true); + } + } + + public void periodic() { + // Update System Alerts + // Check CAN status + var canStatus = RobotController.getCANStatus(); + canErrorAlert.set( + canErrorDebouncer.calculate( + canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); + + // Update Battery Voltage Alert + lowBatteryVoltageAlert.set( + batteryVoltageDebouncer.calculate( + RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + } +} From 2af259b8051e899a7fd4f0881f469a1e9c042b21 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 12 Feb 2025 23:47:30 -0500 Subject: [PATCH 15/73] Update Phoenix6 to 2025.2.2 --- ...c2025-latest.json => Phoenix6-25.2.2.json} | 58 +++++++++---------- 1 file changed, 29 insertions(+), 29 deletions(-) rename vendordeps/{Phoenix6-frc2025-latest.json => Phoenix6-25.2.2.json} (92%) diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-25.2.2.json similarity index 92% rename from vendordeps/Phoenix6-frc2025-latest.json rename to vendordeps/Phoenix6-25.2.2.json index 820c61a..39ae6c5 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-25.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-latest.json", + "fileName": "Phoenix6-25.2.2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, From b2626e8188e48faf889353291b823806ebb62044 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu, 13 Feb 2025 00:06:04 -0500 Subject: [PATCH 16/73] Refactor code structure to not over-expose subsystem specific stuff --- src/main/java/frc/robot/PoseEstimator.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/commands/DriveCommands.java | 162 +---------------- .../frc/robot/subsystems/drive/DriveBase.java | 165 ++++++++++++++++++ .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 2 +- .../subsystems/drive/OdometryManager.java | 2 +- 7 files changed, 178 insertions(+), 165 deletions(-) diff --git a/src/main/java/frc/robot/PoseEstimator.java b/src/main/java/frc/robot/PoseEstimator.java index 7c2618f..07a05af 100644 --- a/src/main/java/frc/robot/PoseEstimator.java +++ b/src/main/java/frc/robot/PoseEstimator.java @@ -11,7 +11,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.DriveBase; import lombok.Getter; import org.littletonrobotics.junction.AutoLogOutput; @@ -60,7 +60,7 @@ private PoseEstimator() { qStdDevs.set(i, 0, Math.pow(odometryStateStdDevs.get(i, 0), 2)); } - kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); + kinematics = new SwerveDriveKinematics(DriveBase.getModuleTranslations()); } public void resetPose(Pose2d pose) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19198c9..480a909 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,10 +61,10 @@ public RobotContainer() { if (Constants.TUNING_MODE) { // Set up Characterization routines autoChooser.addOption( - "Drive Wheel Radius Characterization", - DriveCommands.wheelRadiusCharacterization(driveBase)); + "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); autoChooser.addOption( - "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(driveBase)); + "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); + } } configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index bcef915..fd2dd3d 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -2,23 +2,14 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.PoseEstimator; import frc.robot.subsystems.drive.DriveBase; -import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; -import java.text.DecimalFormat; -import java.text.NumberFormat; -import java.util.LinkedList; -import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; @@ -37,12 +28,6 @@ public class DriveCommands { private static final double ANGLE_MAX_VELOCITY = 8.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; - // Characterization - private static final double FF_START_DELAY = 2.0; // Secs - private static final double FF_RAMP_RATE = 0.85; // Volts/Sec - private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec - private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 - /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ @@ -69,9 +54,9 @@ public static Command joystickDrive( var speeds = new ChassisSpeeds( - x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - omega * DriveConstants.maxAngularVelocityRadPerSec * angularVelocityScalar); + x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + y * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + omega * DriveBase.getMaxAngularVelocityRadPerSec() * angularVelocityScalar); // Convert to field relative Rotation2d rotation = PoseEstimator.getInstance().getRotation(); @@ -122,8 +107,8 @@ public static Command joystickDriveAtAngle( double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); var speeds = new ChassisSpeeds( - x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + y * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, omega); // Convert to field relative @@ -137,141 +122,4 @@ public static Command joystickDriveAtAngle( .beforeStarting( () -> angleController.reset(PoseEstimator.getInstance().getRotation().getRadians())); } - - /** - * Measures the velocity feedforward constants for the drive motors. - * - *

This command should only be used in voltage control mode. - */ - public static Command feedforwardCharacterization(DriveBase drive) { - List velocitySamples = new LinkedList<>(); - List voltageSamples = new LinkedList<>(); - Timer timer = new Timer(); - - return Commands.sequence( - // Reset data - Commands.runOnce( - () -> { - velocitySamples.clear(); - voltageSamples.clear(); - }), - - // Allow modules to orient - Commands.run( - () -> { - drive.runCharacterization(0.0); - }, - drive) - .withTimeout(FF_START_DELAY), - - // Start timer - Commands.runOnce(timer::restart), - - // Accelerate and gather data - Commands.run( - () -> { - double voltage = timer.get() * FF_RAMP_RATE; - drive.runCharacterization(voltage); - velocitySamples.add(drive.getFFCharacterizationVelocity()); - voltageSamples.add(voltage); - }, - drive) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - int n = velocitySamples.size(); - double sumX = 0.0; - double sumY = 0.0; - double sumXY = 0.0; - double sumX2 = 0.0; - for (int i = 0; i < n; i++) { - sumX += velocitySamples.get(i); - sumY += voltageSamples.get(i); - sumXY += velocitySamples.get(i) * voltageSamples.get(i); - sumX2 += velocitySamples.get(i) * velocitySamples.get(i); - } - double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); - double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); - - NumberFormat formatter = new DecimalFormat("#0.00000"); - SmartDashboard.putString("kS", formatter.format(kS)); - SmartDashboard.putString("kV", formatter.format(kV)); - })); - } - - /** Measures the robot's wheel radius by spinning in a circle. */ - public static Command wheelRadiusCharacterization(DriveBase drive) { - SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); - WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); - - return Commands.parallel( - // Drive control sequence - Commands.sequence( - // Reset acceleration limiter - Commands.runOnce( - () -> { - limiter.reset(0.0); - }), - - // Turn in place, accelerating up to full speed - Commands.run( - () -> { - double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); - drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); - }, - drive)), - - // Measurement sequence - Commands.sequence( - // Wait for modules to fully orient before starting measurement - Commands.waitSeconds(1.0), - - // Record starting measurement - Commands.runOnce( - () -> { - state.positions = drive.getWheelRadiusCharacterizationPositions(); - state.lastAngle = drive.getGyroRotation(); - state.gyroDelta = 0.0; - }), - - // Update gyro delta - Commands.run( - () -> { - var rotation = drive.getGyroRotation(); - state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); - state.lastAngle = rotation; - }) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - double[] positions = drive.getWheelRadiusCharacterizationPositions(); - double wheelDelta = 0.0; - for (int i = 0; i < 4; i++) { - wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; - } - double wheelRadius = - (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; - - NumberFormat formatter = new DecimalFormat("#0.000"); - - SmartDashboard.putString( - "Wheel Delta", formatter.format(wheelDelta) + " radians"); - SmartDashboard.putString( - "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); - SmartDashboard.putString( - "Wheel Radius", - formatter.format(wheelRadius) - + " meters, " - + formatter.format(Units.metersToInches(wheelRadius)) - + " inches"); - }))); - } - - private static class WheelRadiusCharacterizationState { - double[] positions = new double[4]; - Rotation2d lastAngle = new Rotation2d(); - double gyroDelta = 0.0; - } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index de2052a..d46304f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,23 +1,39 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.PoseEstimator; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.swerve.SwerveSetpointGenerator; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.Queue; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class DriveBase extends SubsystemBase { + // Characterization + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.85; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + private final GyroIO gyroIO; private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); @@ -271,4 +287,153 @@ public double getFFCharacterizationVelocity() { public Rotation2d getGyroRotation() { return m_gyroInputs.yawPosition; } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public Command feedforwardCharacterization() { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + this.runCharacterization(0.0); + }, + this) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + this.runCharacterization(voltage); + velocitySamples.add(this.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + this) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + SmartDashboard.putString("kS", formatter.format(kS)); + SmartDashboard.putString("kV", formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public Command wheelRadiusCharacterization() { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + this.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + this)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = this.getWheelRadiusCharacterizationPositions(); + state.lastAngle = this.getGyroRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = this.getGyroRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + + SmartDashboard.putString( + "Wheel Delta", formatter.format(wheelDelta) + " radians"); + SmartDashboard.putString( + "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); + SmartDashboard.putString( + "Wheel Radius", + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } + + public static double getMaxLinearVelocityMetersPerSecond() { + return DriveConstants.maxLinearVelocityMetersPerSec; + } + + public static double getMaxAngularVelocityRadPerSec() { + return DriveConstants.maxAngularVelocityRadPerSec; + } + + public static Translation2d[] getModuleTranslations() { + return DriveConstants.moduleTranslations; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 093576b..4222ed9 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -7,7 +7,7 @@ import frc.robot.util.swerve.SwerveSetpointGenerator.ModuleLimits; import lombok.Builder; -public class DriveConstants { +class DriveConstants { public static final double odometryFrequencyHz = Constants.getMode() == Constants.Mode.SIM ? 50 : 250; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 457c216..c51306d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -11,7 +11,7 @@ import lombok.Getter; import org.littletonrobotics.junction.Logger; -public class Module { +class Module { private static final LoggedTunableNumber drivekS = new LoggedTunableNumber("Drive/Module/DrivekS"); private static final LoggedTunableNumber drivekV = diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java index bd94521..90389dd 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -12,7 +12,7 @@ import lombok.Getter; import org.littletonrobotics.junction.AutoLog; -public class OdometryManager implements AutoCloseable { +class OdometryManager implements AutoCloseable { public static Lock odometryLock = new ReentrantLock(); // Prevent conflicts when reading and writing data From 295299829ca2ac25090cc684e1a29941dba6a40c Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 15 Feb 2025 01:43:00 -0500 Subject: [PATCH 17/73] Shorten name for drive temp --- src/main/java/frc/robot/subsystems/drive/ModuleIO.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 4f2c590..bf0eddb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -11,7 +11,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; - public double driveTemperatureCelsius = 0.0; + public double driveTempCelsius = 0.0; public boolean turnConnected = false; public Rotation2d turnAbsolutePosition = new Rotation2d(); @@ -19,7 +19,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; - public double turnTemperatureCelsius = 0.0; + public double turnTempCelsius = 0.0; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 1dbb06f..1699340 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -134,14 +134,14 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage(); inputs.driveCurrentAmps = driveSpark.getOutputCurrent(); - inputs.driveTemperatureCelsius = driveSpark.getMotorTemperature(); + inputs.driveTempCelsius = driveSpark.getMotorTemperature(); inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition()); inputs.turnVelocityRadPerSec = turnEncoder.getVelocity(); inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); - inputs.turnTemperatureCelsius = turnSpark.getMotorTemperature(); + inputs.turnTempCelsius = turnSpark.getMotorTemperature(); inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault()); inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault()); From 5b6968a286336017765f9eb07022e52eedd2e6b0 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu, 13 Feb 2025 01:44:37 -0500 Subject: [PATCH 18/73] Fix red alliance drive commands bug --- .../frc/robot/commands/DriveCommands.java | 9 +++- .../java/frc/robot/util/AllianceFlipUtil.java | 51 ++++++++++++++----- 2 files changed, 45 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index fd2dd3d..5588836 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -60,7 +60,10 @@ public static Command joystickDrive( // Convert to field relative Rotation2d rotation = PoseEstimator.getInstance().getRotation(); - rotation = AllianceFlipUtil.apply(rotation); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds @@ -112,7 +115,9 @@ public static Command joystickDriveAtAngle( omega); // Convert to field relative - rotation = AllianceFlipUtil.apply(rotation); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java index f632cec..7c1e824 100644 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -26,15 +26,24 @@ private static Translation3d applyTranslation(Translation3d translation3d) { applyX(translation3d.getX()), translation3d.getY(), translation3d.getZ()); } - private static Rotation2d applyRotation(Rotation2d rotation) { - return new Rotation2d(-rotation.getCos(), rotation.getSin()); + private static Rotation2d applyRotation(Rotation2d rotation2d) { + return rotation2d.rotateBy(Rotation2d.kPi); + } + + private static Rotation3d applyRotation(Rotation3d rotation3d) { + return rotation3d.rotateBy(new Rotation3d(0.0, 0.0, Math.PI)); } private static Pose2d applyPose(Pose2d pose) { return new Pose2d(applyTranslation(pose.getTranslation()), applyRotation(pose.getRotation())); } - private static boolean shouldFlip() { + private static Pose3d applyPose(Pose3d pose3d) { + return new Pose3d( + applyTranslation(pose3d.getTranslation()), applyRotation(pose3d.getRotation())); + } + + public static boolean shouldFlip() { var currentAllianceOpt = DriverStation.getAlliance(); return currentAllianceOpt.isPresent() && currentAllianceOpt.get() == DriverStation.Alliance.Red; } @@ -43,20 +52,28 @@ public static double apply(double x) { return shouldFlip() ? applyX(x) : x; } - public static Translation2d apply(Translation2d translation) { - return shouldFlip() ? applyTranslation(translation) : translation; + public static Translation2d apply(Translation2d translation2d) { + return shouldFlip() ? applyTranslation(translation2d) : translation2d; } - public static Rotation2d apply(Rotation2d rotation) { - return shouldFlip() ? applyRotation(rotation) : rotation; + public static Translation3d apply(Translation3d translation3d) { + return shouldFlip() ? applyTranslation(translation3d) : translation3d; } - public static Pose2d apply(Pose2d pose) { - return shouldFlip() ? applyPose(pose) : pose; + public static Rotation2d apply(Rotation2d rotation2d) { + return shouldFlip() ? applyRotation(rotation2d) : rotation2d; } - public static Translation3d apply(Translation3d translation3d) { - return shouldFlip() ? applyTranslation(translation3d) : translation3d; + public static Rotation3d apply(Rotation3d rotation3d) { + return shouldFlip() ? applyRotation(rotation3d) : rotation3d; + } + + public static Pose2d apply(Pose2d pose2d) { + return shouldFlip() ? applyPose(pose2d) : pose2d; + } + + public static Pose3d apply(Pose3d pose3d) { + return shouldFlip() ? applyPose(pose3d) : pose3d; } public static class AllianceRelative { @@ -84,16 +101,24 @@ public static AllianceRelative from(Rotation2d value) { return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); } + public static AllianceRelative from(Rotation3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); + } + public static AllianceRelative from(Translation2d value) { return new AllianceRelative<>(value, AllianceFlipUtil::applyTranslation); } + public static AllianceRelative from(Translation3d 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); + public static AllianceRelative from(Pose3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyPose); } } } From 420fa3d8329e495d1a9422680ba392a58594e3c8 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 16 Feb 2025 15:38:07 -0500 Subject: [PATCH 19/73] Update WPILIB to 2025.3.1 --- build.gradle | 2 +- src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java | 5 +++-- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 5 +++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/build.gradle b/build.gradle index cf63f85..ea7a7aa 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "7.0.2" id "io.freefair.lombok" version "8.11" diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 5719ed6..2b670b3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -29,7 +29,7 @@ public class ModuleIOSim implements ModuleIO { private final PIDController driveController = new PIDController(0, 0, 0); private final PIDController turnController = new PIDController(0, 0, 0); - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); private double driveAppliedVolts = 0.0; private double driveFFVolts = 0.0; @@ -125,7 +125,8 @@ public void setDrivePID(double kP, double kI, double kD) { @Override public void setDriveFF(double kS, double kV) { - driveFeedforward = new SimpleMotorFeedforward(kS, kV); + driveFeedforward.setKs(kS); + driveFeedforward.setKv(kV); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 1699340..9b7bd76 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -35,7 +35,7 @@ public class ModuleIOSpark implements ModuleIO { // Closed loop controllers private final SparkClosedLoopController driveController; private final SparkClosedLoopController turnController; - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); + private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); // Queue inputs from odometry thread private final Queue drivePositionQueue; @@ -193,7 +193,8 @@ public void setDrivePID(double kP, double kI, double kD) { @Override public void setDriveFF(double kS, double kV) { - driveFeedforward = new SimpleMotorFeedforward(kS, kV); + driveFeedforward.setKs(kS); + driveFeedforward.setKv(kV); } @Override From 3be26614e98132d3837aeb0bd1bb7313c4d5e973 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 16 Feb 2025 15:43:57 -0500 Subject: [PATCH 20/73] Update stuff (#7) * Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI Co-Authored-By: Talon540-root <122660543+Talon540-root@users.noreply.github.com> * Increase voltage warning on battery and add CAN error alert * clean * Fix Modules Falsely Reporting as Disconnected * Cleanup typing, make errors more obvious * Cleanup dashboard setting stuff * Update sim characterization constants to be more accurate, stops a massive overshooting for some reason * Log drive motor temps * Formatting fixes * Inline tuning mode alert * Rename RobotState to PoseEstimator Because only vision and drive will interact with pose (and purely position) and no other robot state is tracked, it makes more sense for this to be renammed to reflect that. * Refactor abstract alerts into util class to later roll into LEDs * Update Phoenix6 to 2025.2.2 * Refactor code structure to not over-expose subsystem specific stuff * Shorten name for drive temp * Fix red alliance drive commands bug * Update WPILIB to 2025.3.1 --------- Co-authored-by: Talon540-root <122660543+Talon540-root@users.noreply.github.com> --- build.gradle | 2 +- .../{RobotState.java => PoseEstimator.java} | 18 +- src/main/java/frc/robot/Robot.java | 26 +-- src/main/java/frc/robot/RobotContainer.java | 20 +- .../frc/robot/commands/DriveCommands.java | 179 ++---------------- .../frc/robot/subsystems/drive/DriveBase.java | 169 ++++++++++++++++- .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 2 +- .../frc/robot/subsystems/drive/ModuleIO.java | 4 +- .../robot/subsystems/drive/ModuleIOSim.java | 5 +- .../robot/subsystems/drive/ModuleIOSpark.java | 9 +- .../subsystems/drive/OdometryManager.java | 2 +- src/main/java/frc/robot/util/AlertsUtil.java | 54 ++++++ .../java/frc/robot/util/AllianceFlipUtil.java | 51 +++-- ...c2025-latest.json => Phoenix6-25.2.2.json} | 58 +++--- 15 files changed, 338 insertions(+), 263 deletions(-) rename src/main/java/frc/robot/{RobotState.java => PoseEstimator.java} (88%) create mode 100644 src/main/java/frc/robot/util/AlertsUtil.java rename vendordeps/{Phoenix6-frc2025-latest.json => Phoenix6-25.2.2.json} (92%) diff --git a/build.gradle b/build.gradle index cf63f85..ea7a7aa 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "7.0.2" id "io.freefair.lombok" version "8.11" diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/PoseEstimator.java similarity index 88% rename from src/main/java/frc/robot/RobotState.java rename to src/main/java/frc/robot/PoseEstimator.java index 149fb4e..07a05af 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/PoseEstimator.java @@ -11,32 +11,32 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.DriveBase; import lombok.Getter; import org.littletonrobotics.junction.AutoLogOutput; -public class RobotState { +public class PoseEstimator { // Standard deviations of the pose estimate (x position in meters, y position in meters, and // heading in radians). // Increase these numbers to trust your state estimate less. private static final Matrix odometryStateStdDevs = VecBuilder.fill(0.003, 0.003, 0.002); private static final double poseBufferSizeSec = 2.0; - private static RobotState instance; + private static PoseEstimator instance; - public static RobotState getInstance() { + public static PoseEstimator getInstance() { if (instance == null) { - instance = new RobotState(); + instance = new PoseEstimator(); } return instance; } @Getter - @AutoLogOutput(key = "RobotState/OdometryPose") + @AutoLogOutput(key = "PoseEstimator/OdometryPose") private Pose2d odometryPose = new Pose2d(); @Getter - @AutoLogOutput(key = "RobotState/EstimatedPose") + @AutoLogOutput(key = "PoseEstimator/EstimatedPose") private Pose2d estimatedPose = new Pose2d(); private final TimeInterpolatableBuffer poseBuffer = @@ -55,12 +55,12 @@ public static RobotState getInstance() { // Assume gyro starts at zero private Rotation2d gyroOffset = new Rotation2d(); - private RobotState() { + private PoseEstimator() { for (int i = 0; i < 3; ++i) { qStdDevs.set(i, 0, Math.pow(odometryStateStdDevs.get(i, 0), 2)); } - kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); + kinematics = new SwerveDriveKinematics(DriveBase.getModuleTranslations()); } public void resetPose(Pose2d pose) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8bbc602..73c12cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,14 +13,12 @@ package frc.robot; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.AlertsUtil; import frc.robot.util.LoggerUtil; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -37,20 +35,9 @@ * project. */ public class Robot extends LoggedRobot { - private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; - private Command autonomousCommand; private final RobotContainer robotContainer; - // System Alerts - private final Alert canErrorAlert = - new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); - private final Debouncer canErrorDebouncer = new Debouncer(0.5); - - private final Alert lowBatteryVoltageAlert = - new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning); - private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); - public Robot() { super(Constants.kLoopPeriodSecs); @@ -100,16 +87,7 @@ public void robotPeriodic() { // Run command scheduler CommandScheduler.getInstance().run(); - // Check CAN status - var canStatus = RobotController.getCANStatus(); - canErrorAlert.set( - canErrorDebouncer.calculate( - canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); - - // Update Battery Voltage Alert - lowBatteryVoltageAlert.set( - batteryVoltageDebouncer.calculate( - RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + AlertsUtil.getInstance().periodic(); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 60bbb9d..480a909 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,18 +2,18 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.*; +import frc.robot.util.AlertsUtil; import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { - // Load RobotState class - private final RobotState robotState = RobotState.getInstance(); + // Load PoseEstimator class + private final PoseEstimator poseEstimator = PoseEstimator.getInstance(); // Subsystems private final DriveBase driveBase; @@ -59,14 +59,12 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices"); if (Constants.TUNING_MODE) { - new Alert("Robot in Tuning Mode", Alert.AlertType.kInfo).set(true); - // Set up Characterization routines autoChooser.addOption( - "Drive Wheel Radius Characterization", - DriveCommands.wheelRadiusCharacterization(driveBase)); + "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); autoChooser.addOption( - "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(driveBase)); + "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); + } } configureButtonBindings(); @@ -94,16 +92,16 @@ private void configureButtonBindings() { // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); - // Reset gyro to 0° when B button is pressed + // Reset gyro to 0° when B button is pressed controller .b() .onTrue( Commands.runOnce( () -> - RobotState.getInstance() + PoseEstimator.getInstance() .resetPose( new Pose2d( - RobotState.getInstance().getEstimatedPose().getTranslation(), + PoseEstimator.getInstance().getEstimatedPose().getTranslation(), AllianceFlipUtil.apply(new Rotation2d()))), driveBase) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index ea9d77d..5588836 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -2,23 +2,14 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.RobotState; +import frc.robot.PoseEstimator; import frc.robot.subsystems.drive.DriveBase; -import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; -import java.text.DecimalFormat; -import java.text.NumberFormat; -import java.util.LinkedList; -import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; @@ -37,12 +28,6 @@ public class DriveCommands { private static final double ANGLE_MAX_VELOCITY = 8.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; - // Characterization - private static final double FF_START_DELAY = 2.0; // Secs - private static final double FF_RAMP_RATE = 0.85; // Volts/Sec - private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec - private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 - /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ @@ -69,13 +54,16 @@ public static Command joystickDrive( var speeds = new ChassisSpeeds( - x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - omega * DriveConstants.maxAngularVelocityRadPerSec * angularVelocityScalar); + x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + y * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + omega * DriveBase.getMaxAngularVelocityRadPerSec() * angularVelocityScalar); // Convert to field relative - Rotation2d rotation = RobotState.getInstance().getRotation(); - rotation = AllianceFlipUtil.apply(rotation); + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds @@ -113,7 +101,7 @@ public static Command joystickDriveAtAngle( y = Math.copySign(Math.pow(y, 2), y); // Calculate angular speed - Rotation2d rotation = RobotState.getInstance().getRotation(); + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); double omega = angleController.calculate( rotation.getRadians(), rotationSupplier.get().getRadians()); @@ -122,12 +110,14 @@ public static Command joystickDriveAtAngle( double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); var speeds = new ChassisSpeeds( - x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + y * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, omega); // Convert to field relative - rotation = AllianceFlipUtil.apply(rotation); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds @@ -135,143 +125,6 @@ public static Command joystickDriveAtAngle( }, driveBase) .beforeStarting( - () -> angleController.reset(RobotState.getInstance().getRotation().getRadians())); - } - - /** - * Measures the velocity feedforward constants for the drive motors. - * - *

This command should only be used in voltage control mode. - */ - public static Command feedforwardCharacterization(DriveBase drive) { - List velocitySamples = new LinkedList<>(); - List voltageSamples = new LinkedList<>(); - Timer timer = new Timer(); - - return Commands.sequence( - // Reset data - Commands.runOnce( - () -> { - velocitySamples.clear(); - voltageSamples.clear(); - }), - - // Allow modules to orient - Commands.run( - () -> { - drive.runCharacterization(0.0); - }, - drive) - .withTimeout(FF_START_DELAY), - - // Start timer - Commands.runOnce(timer::restart), - - // Accelerate and gather data - Commands.run( - () -> { - double voltage = timer.get() * FF_RAMP_RATE; - drive.runCharacterization(voltage); - velocitySamples.add(drive.getFFCharacterizationVelocity()); - voltageSamples.add(voltage); - }, - drive) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - int n = velocitySamples.size(); - double sumX = 0.0; - double sumY = 0.0; - double sumXY = 0.0; - double sumX2 = 0.0; - for (int i = 0; i < n; i++) { - sumX += velocitySamples.get(i); - sumY += voltageSamples.get(i); - sumXY += velocitySamples.get(i) * voltageSamples.get(i); - sumX2 += velocitySamples.get(i) * velocitySamples.get(i); - } - double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); - double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); - - NumberFormat formatter = new DecimalFormat("#0.00000"); - SmartDashboard.putString("kS", formatter.format(kS)); - SmartDashboard.putString("kV", formatter.format(kV)); - })); - } - - /** Measures the robot's wheel radius by spinning in a circle. */ - public static Command wheelRadiusCharacterization(DriveBase drive) { - SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); - WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); - - return Commands.parallel( - // Drive control sequence - Commands.sequence( - // Reset acceleration limiter - Commands.runOnce( - () -> { - limiter.reset(0.0); - }), - - // Turn in place, accelerating up to full speed - Commands.run( - () -> { - double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); - drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); - }, - drive)), - - // Measurement sequence - Commands.sequence( - // Wait for modules to fully orient before starting measurement - Commands.waitSeconds(1.0), - - // Record starting measurement - Commands.runOnce( - () -> { - state.positions = drive.getWheelRadiusCharacterizationPositions(); - state.lastAngle = drive.getGyroRotation(); - state.gyroDelta = 0.0; - }), - - // Update gyro delta - Commands.run( - () -> { - var rotation = drive.getGyroRotation(); - state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); - state.lastAngle = rotation; - }) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - double[] positions = drive.getWheelRadiusCharacterizationPositions(); - double wheelDelta = 0.0; - for (int i = 0; i < 4; i++) { - wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; - } - double wheelRadius = - (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; - - NumberFormat formatter = new DecimalFormat("#0.000"); - - SmartDashboard.putString( - "Wheel Delta", formatter.format(wheelDelta) + " radians"); - SmartDashboard.putString( - "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); - SmartDashboard.putString( - "Wheel Radius", - formatter.format(wheelRadius) - + " meters, " - + formatter.format(Units.metersToInches(wheelRadius)) - + " inches"); - }))); - } - - private static class WheelRadiusCharacterizationState { - double[] positions = new double[4]; - Rotation2d lastAngle = new Rotation2d(); - double gyroDelta = 0.0; + () -> angleController.reset(PoseEstimator.getInstance().getRotation().getRadians())); } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index 5db3d4e..d46304f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,23 +1,39 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.RobotState; +import frc.robot.PoseEstimator; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.swerve.SwerveSetpointGenerator; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.Queue; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class DriveBase extends SubsystemBase { + // Characterization + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.85; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + private final GyroIO gyroIO; private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); @@ -126,7 +142,7 @@ public void periodic() { for (int j = 0; j < 4; j++) { wheelPositions[j] = modules[j].getOdometryPositions()[i]; } - RobotState.getInstance() + PoseEstimator.getInstance() .addOdometryObservation( wheelPositions, m_gyroInputs.connected ? m_gyroInputs.odometryYawPositions[i] : null, @@ -271,4 +287,153 @@ public double getFFCharacterizationVelocity() { public Rotation2d getGyroRotation() { return m_gyroInputs.yawPosition; } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public Command feedforwardCharacterization() { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + this.runCharacterization(0.0); + }, + this) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + this.runCharacterization(voltage); + velocitySamples.add(this.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + this) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + SmartDashboard.putString("kS", formatter.format(kS)); + SmartDashboard.putString("kV", formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public Command wheelRadiusCharacterization() { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + this.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + this)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = this.getWheelRadiusCharacterizationPositions(); + state.lastAngle = this.getGyroRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = this.getGyroRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + + SmartDashboard.putString( + "Wheel Delta", formatter.format(wheelDelta) + " radians"); + SmartDashboard.putString( + "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); + SmartDashboard.putString( + "Wheel Radius", + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } + + public static double getMaxLinearVelocityMetersPerSecond() { + return DriveConstants.maxLinearVelocityMetersPerSec; + } + + public static double getMaxAngularVelocityRadPerSec() { + return DriveConstants.maxAngularVelocityRadPerSec; + } + + public static Translation2d[] getModuleTranslations() { + return DriveConstants.moduleTranslations; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 093576b..4222ed9 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -7,7 +7,7 @@ import frc.robot.util.swerve.SwerveSetpointGenerator.ModuleLimits; import lombok.Builder; -public class DriveConstants { +class DriveConstants { public static final double odometryFrequencyHz = Constants.getMode() == Constants.Mode.SIM ? 50 : 250; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 457c216..c51306d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -11,7 +11,7 @@ import lombok.Getter; import org.littletonrobotics.junction.Logger; -public class Module { +class Module { private static final LoggedTunableNumber drivekS = new LoggedTunableNumber("Drive/Module/DrivekS"); private static final LoggedTunableNumber drivekV = diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 4f2c590..bf0eddb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -11,7 +11,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; - public double driveTemperatureCelsius = 0.0; + public double driveTempCelsius = 0.0; public boolean turnConnected = false; public Rotation2d turnAbsolutePosition = new Rotation2d(); @@ -19,7 +19,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; - public double turnTemperatureCelsius = 0.0; + public double turnTempCelsius = 0.0; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 5719ed6..2b670b3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -29,7 +29,7 @@ public class ModuleIOSim implements ModuleIO { private final PIDController driveController = new PIDController(0, 0, 0); private final PIDController turnController = new PIDController(0, 0, 0); - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); private double driveAppliedVolts = 0.0; private double driveFFVolts = 0.0; @@ -125,7 +125,8 @@ public void setDrivePID(double kP, double kI, double kD) { @Override public void setDriveFF(double kS, double kV) { - driveFeedforward = new SimpleMotorFeedforward(kS, kV); + driveFeedforward.setKs(kS); + driveFeedforward.setKv(kV); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 1dbb06f..9b7bd76 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -35,7 +35,7 @@ public class ModuleIOSpark implements ModuleIO { // Closed loop controllers private final SparkClosedLoopController driveController; private final SparkClosedLoopController turnController; - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); + private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); // Queue inputs from odometry thread private final Queue drivePositionQueue; @@ -134,14 +134,14 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage(); inputs.driveCurrentAmps = driveSpark.getOutputCurrent(); - inputs.driveTemperatureCelsius = driveSpark.getMotorTemperature(); + inputs.driveTempCelsius = driveSpark.getMotorTemperature(); inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition()); inputs.turnVelocityRadPerSec = turnEncoder.getVelocity(); inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); - inputs.turnTemperatureCelsius = turnSpark.getMotorTemperature(); + inputs.turnTempCelsius = turnSpark.getMotorTemperature(); inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault()); inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault()); @@ -193,7 +193,8 @@ public void setDrivePID(double kP, double kI, double kD) { @Override public void setDriveFF(double kS, double kV) { - driveFeedforward = new SimpleMotorFeedforward(kS, kV); + driveFeedforward.setKs(kS); + driveFeedforward.setKv(kV); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java index bd94521..90389dd 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -12,7 +12,7 @@ import lombok.Getter; import org.littletonrobotics.junction.AutoLog; -public class OdometryManager implements AutoCloseable { +class OdometryManager implements AutoCloseable { public static Lock odometryLock = new ReentrantLock(); // Prevent conflicts when reading and writing data diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java new file mode 100644 index 0000000..0f51f72 --- /dev/null +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -0,0 +1,54 @@ +package frc.robot.util; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants; + +public class AlertsUtil { + private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; + + private static AlertsUtil instance; + + public static AlertsUtil getInstance() { + if (instance == null) { + instance = new AlertsUtil(); + } + + return instance; + } + + // System Alerts + private final Alert canErrorAlert = + new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + private final Debouncer canErrorDebouncer = new Debouncer(0.5); + + private final Alert lowBatteryVoltageAlert = + new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); + private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); + + // Program Alerts + private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); + + private AlertsUtil() { + if (Constants.TUNING_MODE) { + tuningModeAlert.set(true); + } + } + + public void periodic() { + // Update System Alerts + // Check CAN status + var canStatus = RobotController.getCANStatus(); + canErrorAlert.set( + canErrorDebouncer.calculate( + canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); + + // Update Battery Voltage Alert + lowBatteryVoltageAlert.set( + batteryVoltageDebouncer.calculate( + RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + } +} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java index f632cec..7c1e824 100644 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -26,15 +26,24 @@ private static Translation3d applyTranslation(Translation3d translation3d) { applyX(translation3d.getX()), translation3d.getY(), translation3d.getZ()); } - private static Rotation2d applyRotation(Rotation2d rotation) { - return new Rotation2d(-rotation.getCos(), rotation.getSin()); + private static Rotation2d applyRotation(Rotation2d rotation2d) { + return rotation2d.rotateBy(Rotation2d.kPi); + } + + private static Rotation3d applyRotation(Rotation3d rotation3d) { + return rotation3d.rotateBy(new Rotation3d(0.0, 0.0, Math.PI)); } private static Pose2d applyPose(Pose2d pose) { return new Pose2d(applyTranslation(pose.getTranslation()), applyRotation(pose.getRotation())); } - private static boolean shouldFlip() { + private static Pose3d applyPose(Pose3d pose3d) { + return new Pose3d( + applyTranslation(pose3d.getTranslation()), applyRotation(pose3d.getRotation())); + } + + public static boolean shouldFlip() { var currentAllianceOpt = DriverStation.getAlliance(); return currentAllianceOpt.isPresent() && currentAllianceOpt.get() == DriverStation.Alliance.Red; } @@ -43,20 +52,28 @@ public static double apply(double x) { return shouldFlip() ? applyX(x) : x; } - public static Translation2d apply(Translation2d translation) { - return shouldFlip() ? applyTranslation(translation) : translation; + public static Translation2d apply(Translation2d translation2d) { + return shouldFlip() ? applyTranslation(translation2d) : translation2d; } - public static Rotation2d apply(Rotation2d rotation) { - return shouldFlip() ? applyRotation(rotation) : rotation; + public static Translation3d apply(Translation3d translation3d) { + return shouldFlip() ? applyTranslation(translation3d) : translation3d; } - public static Pose2d apply(Pose2d pose) { - return shouldFlip() ? applyPose(pose) : pose; + public static Rotation2d apply(Rotation2d rotation2d) { + return shouldFlip() ? applyRotation(rotation2d) : rotation2d; } - public static Translation3d apply(Translation3d translation3d) { - return shouldFlip() ? applyTranslation(translation3d) : translation3d; + public static Rotation3d apply(Rotation3d rotation3d) { + return shouldFlip() ? applyRotation(rotation3d) : rotation3d; + } + + public static Pose2d apply(Pose2d pose2d) { + return shouldFlip() ? applyPose(pose2d) : pose2d; + } + + public static Pose3d apply(Pose3d pose3d) { + return shouldFlip() ? applyPose(pose3d) : pose3d; } public static class AllianceRelative { @@ -84,16 +101,24 @@ public static AllianceRelative from(Rotation2d value) { return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); } + public static AllianceRelative from(Rotation3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); + } + public static AllianceRelative from(Translation2d value) { return new AllianceRelative<>(value, AllianceFlipUtil::applyTranslation); } + public static AllianceRelative from(Translation3d 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); + public static AllianceRelative from(Pose3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyPose); } } } diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-25.2.2.json similarity index 92% rename from vendordeps/Phoenix6-frc2025-latest.json rename to vendordeps/Phoenix6-25.2.2.json index 820c61a..39ae6c5 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-25.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-latest.json", + "fileName": "Phoenix6-25.2.2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, From de577e53203b8b230ad9d0794b68207aba6775e7 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:47:57 -0500 Subject: [PATCH 21/73] fix akit version mismatch --- vendordeps/AdvantageKit.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa81b2f..79bdf3e 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.1.0", + "version": "4.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.1.0" + "version": "4.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.1.0", + "version": "4.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ From 68e75f96ca0b1cbb53b0039c66e3a05ac3f9bedd Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 19 Feb 2025 23:34:57 -0500 Subject: [PATCH 22/73] Add intake subsystem --- src/main/java/frc/robot/RobotContainer.java | 11 ++- .../robot/subsystems/intake/IntakeBase.java | 30 +++++++ .../subsystems/intake/IntakeConstants.java | 11 +++ .../frc/robot/subsystems/intake/IntakeIO.java | 24 ++++++ .../robot/subsystems/intake/IntakeIOSim.java | 41 ++++++++++ .../subsystems/intake/IntakeIOSpark.java | 82 +++++++++++++++++++ 6 files changed, 197 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeBase.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 480a909..78aff3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,10 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.*; -import frc.robot.util.AlertsUtil; +import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.intake.IntakeIOSpark; import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -17,6 +20,7 @@ public class RobotContainer { // Subsystems private final DriveBase driveBase; + private final IntakeBase intakeBase; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -34,6 +38,7 @@ public RobotContainer() { new ModuleIOSpark(1), new ModuleIOSpark(2), new ModuleIOSpark(3)); + intakeBase = new IntakeBase(new IntakeIOSpark()); } case SIM -> { driveBase = @@ -43,6 +48,8 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + intakeBase = new IntakeBase(new IntakeIOSim()); + } default -> { driveBase = @@ -52,6 +59,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + intakeBase = new IntakeBase(new IntakeIO() {}); } } @@ -65,7 +73,6 @@ public RobotContainer() { autoChooser.addOption( "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); } - } configureButtonBindings(); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeBase.java b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java new file mode 100644 index 0000000..dcca0a9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class IntakeBase extends SubsystemBase { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + private final Alert disconnected = + new Alert("Intake motor disconnected!", Alert.AlertType.kWarning); + + public IntakeBase(IntakeIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + disconnected.set(!inputs.connected); + } + + public Command runRoller(double inputVolts) { + return startEnd(() -> io.runVolts(inputVolts), io::stop); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..69f14fb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.intake; + +class IntakeConstants { + public static final boolean intakeInverted = true; + public static final double intakeMoI = 0.025; + + public static final double intakeGearing = 2.0; + + public static final double intakePositionConversionFactor = 2 * Math.PI / intakeGearing; + public static final double intakeVelocityConversionFactor = intakePositionConversionFactor / 60.0; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..a4fde97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + class IntakeIOInputs { + public boolean connected = false; + + public double positionRads; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + } + + default void updateInputs(IntakeIOInputs inputs) {} + + default void runVolts(double output) {} + + default void stop() {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..1d128d1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class IntakeIOSim implements IntakeIO { + private static final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private static final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(intakeMotorModel, intakeMoI, intakeGearing), + intakeMotorModel); + + private double appliedVoltage = 0.0; + + @Override + public void updateInputs(IntakeIOInputs inputs) { + sim.update(Constants.kLoopPeriodSecs); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java new file mode 100644 index 0000000..7996140 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -0,0 +1,82 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.filter.Debouncer; + +public class IntakeIOSpark implements IntakeIO { + private final SparkBase spark; + private final RelativeEncoder encoder; + + private final Debouncer connectedDebouncer = new Debouncer(.5); + + public IntakeIOSpark() { + spark = new SparkMax(12, SparkLowLevel.MotorType.kBrushless); + encoder = spark.getEncoder(); + + var config = new SparkMaxConfig(); + config + .inverted(intakeInverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(40, 50) + .voltageCompensation(12.0); + + config + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + config + .encoder + .positionConversionFactor(intakePositionConversionFactor) + .velocityConversionFactor(intakeVelocityConversionFactor) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + spark.configure( + config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadsPerSec = encoder.getVelocity(); + inputs.appliedVoltage = spark.getAppliedOutput() * spark.getBusVoltage(); + inputs.currentAmps = spark.getOutputCurrent(); + inputs.tempCelsius = spark.getMotorTemperature(); + + inputs.connected = connectedDebouncer.calculate(!spark.hasActiveFault()); + } + + @Override + public void runVolts(double output) { + spark.setVoltage(output); + } + + @Override + public void stop() { + spark.stopMotor(); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); + + spark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} From ca2f600f234c53cbbfd7de39552818c127d4f15f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 22 Feb 2025 00:53:08 -0500 Subject: [PATCH 23/73] Update robot constants to match bot --- .../robot/subsystems/drive/DriveConstants.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4222ed9..0f12398 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -38,8 +38,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(2) .driveMotorId(3) - .encoderChannel(2) - .encoderOffset(Rotation2d.fromRadians(0.16028737150729522)) + .encoderChannel(0) + .encoderOffset(Rotation2d.fromRadians(1.7182357115138978).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -48,8 +48,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(4) .driveMotorId(5) - .encoderChannel(3) - .encoderOffset(Rotation2d.fromRadians(-0.1422097592800296)) + .encoderChannel(1) + .encoderOffset(Rotation2d.fromRadians(-1.4361935561244243).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -58,8 +58,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(6) .driveMotorId(7) - .encoderChannel(1) - .encoderOffset(Rotation2d.fromRadians(-3.009554996093968)) + .encoderChannel(2) + .encoderOffset(Rotation2d.fromRadians(0.9998617084472845).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -68,8 +68,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(8) .driveMotorId(9) - .encoderChannel(0) - .encoderOffset(Rotation2d.fromRadians(2.559973505647124)) + .encoderChannel(3) + .encoderOffset(Rotation2d.fromRadians(-1.7285578862473199).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) From 6adf211c5e34a0c35c5a679c734caf39afb0f72e Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 22 Feb 2025 01:05:32 -0500 Subject: [PATCH 24/73] Fix DT bug --- src/main/java/frc/robot/commands/DriveCommands.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 5588836..3d9d4b7 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -64,7 +64,6 @@ public static Command joystickDrive( rotation = rotation.rotateBy(Rotation2d.kPi); } speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds driveBase.runVelocity(speeds); From e8fbd8094026f56fd72af4489b30a31a5e2d366c Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 22 Feb 2025 01:28:04 -0500 Subject: [PATCH 25/73] Update Intake --- .../subsystems/intake/IntakeConstants.java | 10 +++++----- .../robot/subsystems/intake/IntakeIOSim.java | 5 ++++- .../robot/subsystems/intake/IntakeIOSpark.java | 18 +++++++++--------- 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 69f14fb..b4b4fd8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,11 +1,11 @@ package frc.robot.subsystems.intake; class IntakeConstants { - public static final boolean intakeInverted = true; - public static final double intakeMoI = 0.025; + public static final boolean inverted = true; + public static final double moi = 0.025; - public static final double intakeGearing = 2.0; + public static final double gearing = 2.0; - public static final double intakePositionConversionFactor = 2 * Math.PI / intakeGearing; - public static final double intakeVelocityConversionFactor = intakePositionConversionFactor / 60.0; + public static final double positionConversionFactor = 2 * Math.PI / gearing; + public static final double velocityConversionFactor = positionConversionFactor / 60.0; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 1d128d1..46c6e23 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -12,7 +12,10 @@ public class IntakeIOSim implements IntakeIO { private static final DCMotor intakeMotorModel = DCMotor.getNEO(1); private static final DCMotorSim sim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(intakeMotorModel, intakeMoI, intakeGearing), + LinearSystemId.createDCMotorSystem(intakeMotorModel, + moi, + gearing + ), intakeMotorModel); private double appliedVoltage = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java index 7996140..b350c67 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -17,16 +17,23 @@ public class IntakeIOSpark implements IntakeIO { private final Debouncer connectedDebouncer = new Debouncer(.5); public IntakeIOSpark() { - spark = new SparkMax(12, SparkLowLevel.MotorType.kBrushless); + spark = new SparkMax(11, SparkLowLevel.MotorType.kBrushless); encoder = spark.getEncoder(); var config = new SparkMaxConfig(); config - .inverted(intakeInverted) + .inverted(inverted) .idleMode(IdleMode.kBrake) .smartCurrentLimit(40, 50) .voltageCompensation(12.0); + config + .encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(velocityConversionFactor) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + config .signals .primaryEncoderPositionAlwaysOn(true) @@ -37,13 +44,6 @@ public IntakeIOSpark() { .busVoltagePeriodMs(20) .outputCurrentPeriodMs(20); - config - .encoder - .positionConversionFactor(intakePositionConversionFactor) - .velocityConversionFactor(intakeVelocityConversionFactor) - .uvwMeasurementPeriod(10) - .uvwAverageDepth(2); - spark.configure( config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); } From 43435348a07e05f7cc8096adecdd54e234ab25e3 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 22 Feb 2025 10:39:38 -0500 Subject: [PATCH 26/73] explicitly set motor temp signals --- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 6 ++++-- .../java/frc/robot/subsystems/intake/IntakeIOSpark.java | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 9b7bd76..89daabd 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -84,7 +84,8 @@ public ModuleIOSpark(int index) { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); driveSpark.configure( driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -116,7 +117,8 @@ public ModuleIOSpark(int index) { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); turnSpark.configure(turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java index b350c67..cfcbd14 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -42,7 +42,8 @@ public IntakeIOSpark() { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); spark.configure( config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); From 96f6eae34148a6acd41b60d069dcd7a052245bd9 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 22 Feb 2025 10:40:15 -0500 Subject: [PATCH 27/73] dont make conversion factors variables --- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 6 +----- src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java | 6 +----- .../java/frc/robot/subsystems/intake/IntakeIOSpark.java | 4 ++-- 3 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index b4b4fd8..5fe42dd 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -2,10 +2,6 @@ class IntakeConstants { public static final boolean inverted = true; - public static final double moi = 0.025; - + public static final double moi = 0.025; // TODO public static final double gearing = 2.0; - - public static final double positionConversionFactor = 2 * Math.PI / gearing; - public static final double velocityConversionFactor = positionConversionFactor / 60.0; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 46c6e23..37ade16 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -12,11 +12,7 @@ public class IntakeIOSim implements IntakeIO { private static final DCMotor intakeMotorModel = DCMotor.getNEO(1); private static final DCMotorSim sim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(intakeMotorModel, - moi, - gearing - ), - intakeMotorModel); + LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); private double appliedVoltage = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java index cfcbd14..eca148d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -29,8 +29,8 @@ public IntakeIOSpark() { config .encoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(velocityConversionFactor) + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); From fb125076708266e1c948eca930a01f8695ea8e15 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 23 Feb 2025 13:58:29 -0500 Subject: [PATCH 28/73] Update FieldConstants.java --- src/main/java/frc/robot/FieldConstants.java | 195 +++++++++----------- 1 file changed, 87 insertions(+), 108 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index aad1e00..9a3e7b2 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,25 +1,32 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFieldLayout; 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; +import java.io.IOException; +import java.util.*; +import lombok.Getter; /** * 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 AprilTagFieldLayout fieldLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + + public static final double fieldLength = + AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldLength(); + public static final double fieldWidth = + AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldWidth(); 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)); + new Pose2d( + AprilTagLayoutType.OFFICIAL.getFieldLayout().getTagPose(16).get().getX(), + 0, + Rotation2d.fromDegrees(90)); } public static class Barge { @@ -36,73 +43,76 @@ public static class Barge { } 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 double stationLength = Units.inchesToMeters(79.750); public static final Pose2d rightCenterFace = new Pose2d( Units.inchesToMeters(33.526), Units.inchesToMeters(25.824), Rotation2d.fromDegrees(144.011 - 90)); + public static final Pose2d leftCenterFace = + new Pose2d( + rightCenterFace.getX(), + fieldWidth - rightCenterFace.getY(), + Rotation2d.fromRadians(-rightCenterFace.getRotation().getRadians())); + } + + public enum ReefLevel { + L1(Units.inchesToMeters(25.0), 0), + L2(Units.inchesToMeters(31.875 - Math.cos(Math.toRadians(35.0)) * 0.625), -35), + L3(Units.inchesToMeters(47.625 - Math.cos(Math.toRadians(35.0)) * 0.625), -35), + L4(Units.inchesToMeters(72), -90); + + public final double height; + public final double pitch; + + ReefLevel(double height, double pitch) { + this.height = height; + this.pitch = pitch; // Degrees + } + + public static ReefLevel fromLevel(int level) { + return Arrays.stream(values()) + .filter(height -> height.ordinal() == level) + .findFirst() + .orElse(L4); + } } public static class Reef { + public static final double faceLength = Units.inchesToMeters(36.792600); public static final Translation2d center = - new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + new Translation2d(Units.inchesToMeters(176.746), fieldWidth / 2.0); 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 = + public static final List> branchPositions = new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise + public static final List> branchPositions2d = new ArrayList<>(); 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)); + var aprilTagLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + centerFaces[0] = aprilTagLayout.getTagPose(18).get().toPose2d(); + centerFaces[1] = aprilTagLayout.getTagPose(19).get().toPose2d(); + centerFaces[2] = aprilTagLayout.getTagPose(20).get().toPose2d(); + centerFaces[3] = aprilTagLayout.getTagPose(21).get().toPose2d(); + centerFaces[4] = aprilTagLayout.getTagPose(22).get().toPose2d(); + centerFaces[5] = aprilTagLayout.getTagPose(17).get().toPose2d(); // Initialize branch positions for (int face = 0; face < 6; face++) { - Map fillRight = new HashMap<>(); - Map fillLeft = new HashMap<>(); - for (var level : ReefHeight.values()) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + Map fillRight2d = new HashMap<>(); + Map fillLeft2d = new HashMap<>(); + for (var level : ReefLevel.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, + var rightBranchPose = new Pose3d( new Translation3d( poseDirection @@ -115,9 +125,8 @@ public static class Reef { new Rotation3d( 0, Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); - fillLeft.put( - level, + poseDirection.getRotation().getRadians())); + var leftBranchPose = new Pose3d( new Translation3d( poseDirection @@ -130,74 +139,44 @@ public static class Reef { new Rotation3d( 0, Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); + poseDirection.getRotation().getRadians())); + + fillRight.put(level, rightBranchPose); + fillLeft.put(level, leftBranchPose); + fillRight2d.put(level, rightBranchPose.toPose2d()); + fillLeft2d.put(level, leftBranchPose.toPose2d()); } - branchPositions.add((face * 2) + 1, fillRight); - branchPositions.add((face * 2) + 2, fillLeft); + branchPositions.add(fillRight); + branchPositions.add(fillLeft); + branchPositions2d.add(fillRight2d); + branchPositions2d.add(fillLeft2d); } } } 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 double separation = Units.inchesToMeters(72.0); public static final Pose2d middleIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); + new Pose2d(Units.inchesToMeters(48), fieldWidth / 2.0, new Rotation2d()); + public static final Pose2d leftIceCream = + new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() + separation, new Rotation2d()); public static final Pose2d rightIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); + new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() - separation, 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); + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2025-reefscape-welded.json"); - ReefHeight(double height, double pitch) { - this.height = height; - this.pitch = pitch; // in degrees - } + private final AprilTagFieldLayout fieldLayout; - public final double height; - public final double pitch; + private AprilTagLayoutType(String file) { + try { + fieldLayout = AprilTagFieldLayout.loadFromResource(file); + } catch (IOException exception) { + throw new RuntimeException("Failed to load AprilTagLayoutType: " + file, exception); + } + } } - - // 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; - // } } From ba6e2991d808c364586857695d9867a73bdd8b99 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 24 Feb 2025 05:03:14 -0500 Subject: [PATCH 29/73] Update EqualsUtil.java --- src/main/java/frc/robot/util/EqualsUtil.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java index a267032..9dacb20 100644 --- a/src/main/java/frc/robot/util/EqualsUtil.java +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -4,7 +4,7 @@ public class EqualsUtil { public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); + return Math.abs(a - b) <= epsilon; } public static boolean epsilonEquals(double a, double b) { From 1baa58d8dd96d97f443ec8aab682cd5d8d5466bf Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 24 Feb 2025 11:09:27 -0500 Subject: [PATCH 30/73] Add updatable debouncer --- .../robot/subsystems/drive/ModuleIOSpark.java | 2 +- .../subsystems/intake/IntakeIOSpark.java | 2 +- src/main/java/frc/robot/util/Debouncer.java | 90 +++++++++++++++++++ 3 files changed, 92 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/util/Debouncer.java diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 89daabd..43870cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -16,10 +16,10 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.AnalogEncoder; import frc.robot.Constants; +import frc.robot.util.Debouncer; import java.util.Queue; public class ModuleIOSpark implements ModuleIO { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java index eca148d..7ed2a2e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -8,7 +8,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.filter.Debouncer; +import frc.robot.util.Debouncer; public class IntakeIOSpark implements IntakeIO { private final SparkBase spark; diff --git a/src/main/java/frc/robot/util/Debouncer.java b/src/main/java/frc/robot/util/Debouncer.java new file mode 100644 index 0000000..657a83a --- /dev/null +++ b/src/main/java/frc/robot/util/Debouncer.java @@ -0,0 +1,90 @@ +package frc.robot.util; + +import edu.wpi.first.math.MathSharedStore; + +/** + * A simple debounce filter for boolean streams. Requires that the boolean change value from + * baseline for a specified period of time before the filtered value changes. + */ +public class Debouncer { + /** Type of debouncing to perform. */ + public enum DebounceType { + /** Rising edge. */ + kRising, + /** Falling edge. */ + kFalling, + /** Both rising and falling edges. */ + kBoth + } + + private double m_debounceTimeSeconds; + private final DebounceType m_debounceType; + private boolean m_baseline; + + private double m_prevTimeSeconds; + + /** + * Creates a new Debouncer. + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + * @param type Which type of state change the debouncing will be performed on. + */ + public Debouncer(double debounceTime, DebounceType type) { + m_debounceTimeSeconds = debounceTime; + m_debounceType = type; + + resetTimer(); + + m_baseline = + switch (m_debounceType) { + case kBoth, kRising -> false; + case kFalling -> true; + }; + } + + /** + * Creates a new Debouncer. Baseline value defaulted to "false." + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + */ + public Debouncer(double debounceTime) { + this(debounceTime, DebounceType.kRising); + } + + private void resetTimer() { + m_prevTimeSeconds = MathSharedStore.getTimestamp(); + } + + private boolean hasElapsed() { + return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds; + } + + /** + * Applies the debouncer to the input stream. + * + * @param input The current value of the input stream. + * @return The debounced value of the input stream. + */ + public boolean calculate(boolean input) { + if (input == m_baseline) { + resetTimer(); + } + + if (hasElapsed()) { + if (m_debounceType == DebounceType.kBoth) { + m_baseline = input; + resetTimer(); + } + return input; + } else { + return m_baseline; + } + } + + public void setDebounceTime(double debounceTime) { + m_debounceTimeSeconds = debounceTime; + resetTimer(); + } +} From bd348ee7efa5ff7adddc9e6eb5648bf8f1c8448f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 24 Feb 2025 14:36:40 -0500 Subject: [PATCH 31/73] Make sim models non static avoids building creating these objects when running as non-sim --- src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java | 4 ++-- src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 2b670b3..d40eab4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -11,8 +11,8 @@ import java.util.Queue; public class ModuleIOSim implements ModuleIO { - private static final DCMotor driveMotorModel = DCMotor.getNEO(1); - private static final DCMotor turnMotorModel = DCMotor.getNEO(1); + private final DCMotor driveMotorModel = DCMotor.getNEO(1); + private final DCMotor turnMotorModel = DCMotor.getNEO(1); private final DCMotorSim driveSim = new DCMotorSim( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 37ade16..a31f234 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,8 +9,8 @@ import frc.robot.Constants; public class IntakeIOSim implements IntakeIO { - private static final DCMotor intakeMotorModel = DCMotor.getNEO(1); - private static final DCMotorSim sim = + private final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private final DCMotorSim sim = new DCMotorSim( LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); From 19e39761c03d84aab7256b660042e199dd804225 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 24 Feb 2025 14:36:49 -0500 Subject: [PATCH 32/73] Implement DispenserIO --- .../superstructure/DispenserIO.java | 26 ++++++ .../superstructure/DispenserIOSim.java | 40 +++++++++ .../superstructure/DispenserIOSpark.java | 90 +++++++++++++++++++ 3 files changed, 156 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java b/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java new file mode 100644 index 0000000..c86eae9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.superstructure; + +import org.littletonrobotics.junction.AutoLog; + +public interface DispenserIO { + @AutoLog + class DispenserIOInputs { + public boolean connected = false; + + public double positionRads; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + + public boolean rearBeamBreakBroken = false; + } + + default void updateInputs(DispenserIOInputs inputs) {} + + default void runVolts(double output) {} + + default void stop() {} + + // default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java b/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java new file mode 100644 index 0000000..f7d721f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.superstructure; + +import static frc.robot.subsystems.superstructure.SuperstructureConstants.Dispenser.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class DispenserIOSim implements DispenserIO { + private final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); + + private double appliedVoltage = 0.0; + + @Override + public void updateInputs(DispenserIOInputs inputs) { + sim.update(Constants.kLoopPeriodSecs); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java b/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java new file mode 100644 index 0000000..523103d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.superstructure; + +import static frc.robot.subsystems.superstructure.SuperstructureConstants.Dispenser.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.util.Debouncer; + +public class DispenserIOSpark implements DispenserIO { + private final SparkBase spark; + private final RelativeEncoder encoder; + + private final Debouncer connectedDebouncer = new Debouncer(.5); + + // End Dispenser beam break + private final DigitalInput rearBeamBreak = new DigitalInput(0); + + public DispenserIOSpark() { + spark = new SparkMax(14, SparkLowLevel.MotorType.kBrushless); + encoder = spark.getEncoder(); + + var config = new SparkMaxConfig(); + config + .inverted(inverted) + .idleMode(SparkBaseConfig.IdleMode.kBrake) + .smartCurrentLimit(40, 50) + .voltageCompensation(12.0); + + config + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + config + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + spark.configure( + config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(DispenserIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadsPerSec = encoder.getVelocity(); + inputs.appliedVoltage = spark.getAppliedOutput() * spark.getBusVoltage(); + inputs.currentAmps = spark.getOutputCurrent(); + inputs.tempCelsius = spark.getMotorTemperature(); + + inputs.rearBeamBreakBroken = !rearBeamBreak.get(); + + inputs.connected = connectedDebouncer.calculate(!spark.hasActiveFault()); + } + + @Override + public void runVolts(double output) { + spark.setVoltage(output); + } + + @Override + public void stop() { + spark.stopMotor(); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode( + enabled ? SparkBaseConfig.IdleMode.kBrake : SparkBaseConfig.IdleMode.kCoast); + + spark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} From ba09067684158f1859c2290eb650eb8f567363c2 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 24 Feb 2025 14:36:57 -0500 Subject: [PATCH 33/73] Implement ElevatorIO --- .../subsystems/superstructure/ElevatorIO.java | 31 ++++ .../superstructure/ElevatorIOSim.java | 74 +++++++++ .../superstructure/ElevatorIOSpark.java | 143 ++++++++++++++++++ 3 files changed, 248 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java new file mode 100644 index 0000000..7b68624 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.superstructure; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + public class ElevatorIOInputs { + public boolean leaderConnected = false; + public boolean followerConnected = false; + + public double positionRads = 0.0; + public double velocityRadPerSec = 0.0; + public double[] appliedVolts = new double[] {}; + public double[] currentAmps = new double[] {}; + public double[] tempCelsius = new double[] {}; + } + + default void updateInputs(ElevatorIOInputs inputs) {} + + default void runOpenLoop(double output) {} + + default void runPosition(double positionRads, double feedforwardVolts) {} + + default void stop() {} + + default void resetOrigin() {} + + default void setPID(double kP, double kI, double kD) {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java new file mode 100644 index 0000000..5718cb8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.superstructure; + +import static frc.robot.subsystems.superstructure.SuperstructureConstants.Elevator.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import frc.robot.Constants; + +public class ElevatorIOSim implements ElevatorIO { + private final DCMotor elevatorMotorModel = DCMotor.getNEO(2); + private final ElevatorSim sim = + new ElevatorSim( + elevatorMotorModel, + SuperstructureConstants.Elevator.gearing, + carriageMassKg + stagesMassKg, + drumRadius, + 0, + maxTravel, + true, + 0, + 0.01, + 0.0); + + private final PIDController controller = new PIDController(0, 0, 0); + + private double appliedVolts = 0.0; + private boolean closedLoop = false; + private double feedforward = 0.0; + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + if (!closedLoop) { + controller.reset(); + } else { + appliedVolts = controller.calculate(sim.getPositionMeters()) + feedforward; + } + + sim.setInputVoltage(appliedVolts); + sim.update(Constants.kLoopPeriodSecs); + + inputs.leaderConnected = true; + inputs.followerConnected = true; + + inputs.positionRads = sim.getPositionMeters() / drumRadius; + inputs.velocityRadPerSec = sim.getVelocityMetersPerSecond() / drumRadius; + + inputs.appliedVolts = new double[] {appliedVolts}; + inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + } + + @Override + public void runOpenLoop(double output) { + closedLoop = false; + appliedVolts = output; + } + + @Override + public void runPosition(double positionRads, double feedforwardVolts) { + closedLoop = true; + controller.setSetpoint(positionRads); + feedforward = feedforwardVolts; + } + + @Override + public void stop() { + runOpenLoop(0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java new file mode 100644 index 0000000..fc436eb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java @@ -0,0 +1,143 @@ +package frc.robot.subsystems.superstructure; + +import static frc.robot.subsystems.superstructure.SuperstructureConstants.Elevator.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.*; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.util.Debouncer; + +public class ElevatorIOSpark implements ElevatorIO { + private final SparkBase leaderSpark; + private final SparkBase followerSpark; + + private final RelativeEncoder encoder; + + private final SparkClosedLoopController controller; + + private final Debouncer leaderConnectedDebouncer = new Debouncer(0.5); + private final Debouncer followerConnectedDebouncer = new Debouncer(0.5); + + public ElevatorIOSpark() { + leaderSpark = new SparkMax(12, SparkLowLevel.MotorType.kBrushless); + followerSpark = new SparkMax(13, SparkLowLevel.MotorType.kBrushless); + + encoder = leaderSpark.getEncoder(); + controller = leaderSpark.getClosedLoopController(); + + var leaderConfig = new SparkMaxConfig(); + leaderConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(60).voltageCompensation(12.0); + leaderConfig + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + leaderConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pidf(0.0, 0.0, 0.0, 0.0); + leaderConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + leaderSpark.configure( + leaderConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + + var followerConfig = new SparkMaxConfig(); + followerConfig + .follow(leaderSpark, true) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(60) + .voltageCompensation(12.0); + + leaderConfig + .signals + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + followerSpark.configure( + followerConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadPerSec = encoder.getVelocity(); + + inputs.appliedVolts = + new double[] { + leaderSpark.getAppliedOutput() * leaderSpark.getAppliedOutput(), + followerSpark.getAppliedOutput() * followerSpark.getBusVoltage() + }; + inputs.currentAmps = + new double[] {leaderSpark.getOutputCurrent(), followerSpark.getOutputCurrent()}; + inputs.tempCelsius = + new double[] {leaderSpark.getMotorTemperature(), followerSpark.getMotorTemperature()}; + + inputs.leaderConnected = leaderConnectedDebouncer.calculate(!leaderSpark.hasActiveFault()); + inputs.followerConnected = + followerConnectedDebouncer.calculate(!followerSpark.hasActiveFault()); + } + + @Override + public void runOpenLoop(double output) { + leaderSpark.setVoltage(output); + } + + @Override + public void runPosition(double positionRads, double feedforwardVolts) { + controller.setReference( + positionRads, + SparkBase.ControlType.kPosition, + ClosedLoopSlot.kSlot1, + feedforwardVolts, + SparkClosedLoopController.ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + leaderSpark.stopMotor(); + } + + @Override + public void resetOrigin() { + encoder.setPosition(0.0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + var PIDConfig = new SparkMaxConfig(); + PIDConfig.closedLoop.pid(kP, kI, kD); + + leaderSpark.configure( + PIDConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode( + enabled ? SparkBaseConfig.IdleMode.kBrake : SparkBaseConfig.IdleMode.kCoast); + + leaderSpark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} From 9f9de79cf2f5d6650b46a4b92adedfda7012d27f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 25 Feb 2025 03:29:28 -0500 Subject: [PATCH 34/73] Update IO --- .../superstructure/DispenserIO.java | 2 +- .../superstructure/ElevatorIOSpark.java | 20 +++++++++++++------ 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java b/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java index c86eae9..8b6d442 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java +++ b/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java @@ -22,5 +22,5 @@ default void runVolts(double output) {} default void stop() {} - // default void setBrakeMode(boolean enabled) {} + default void setBrakeMode(boolean enabled) {} } diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java index fc436eb..95df4e8 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java @@ -4,7 +4,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.*; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; @@ -36,7 +36,11 @@ public ElevatorIOSpark() { .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); - leaderConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pidf(0.0, 0.0, 0.0, 0.0); + + leaderConfig + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) + .pidf(0.0, 0.0, 0.0, 0.0); leaderConfig .signals .primaryEncoderPositionAlwaysOn(true) @@ -55,12 +59,12 @@ public ElevatorIOSpark() { var followerConfig = new SparkMaxConfig(); followerConfig - .follow(leaderSpark, true) .idleMode(IdleMode.kBrake) .smartCurrentLimit(60) - .voltageCompensation(12.0); + .voltageCompensation(12.0) + .follow(leaderSpark, true); - leaderConfig + followerConfig .signals .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) @@ -80,7 +84,7 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.appliedVolts = new double[] { - leaderSpark.getAppliedOutput() * leaderSpark.getAppliedOutput(), + leaderSpark.getAppliedOutput() * leaderSpark.getBusVoltage(), followerSpark.getAppliedOutput() * followerSpark.getBusVoltage() }; inputs.currentAmps = @@ -139,5 +143,9 @@ public void setBrakeMode(boolean enabled) { brakeModeConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters); + followerSpark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); } } From ee57f87138c8c7ace29a1366c6b6c1770a7e8484 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 25 Feb 2025 22:03:17 -0500 Subject: [PATCH 35/73] WIP --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 69 +++-- .../java/frc/robot/commands/AutoRoutine.java | 3 + .../java/frc/robot/commands/DriveToPose.java | 50 +++ .../frc/robot/commands/DriveTrajectory.java | 8 + .../frc/robot/commands/IntakeCommands.java | 15 + .../subsystems/dispenser/DispenserBase.java | 70 +++++ .../dispenser/DispenserConstants.java | 7 + .../DispenserIO.java | 2 +- .../DispenserIOSim.java | 4 +- .../DispenserIOSpark.java | 8 +- .../subsystems/elevator/ElevatorBase.java | 287 ++++++++++++++++++ .../elevator/ElevatorConstants.java | 20 ++ .../ElevatorIO.java | 2 +- .../ElevatorIOSim.java | 6 +- .../ElevatorIOSpark.java | 13 +- .../subsystems/elevator/ElevatorState.java | 32 ++ .../elevator/ElevatorVisualizer.java | 11 + src/main/java/frc/robot/util/AlertsUtil.java | 137 ++++++++- .../frc/robot/util/LoggedTunableNumber.java | 8 +- .../util/trajectory/DriveTrajectories.java | 10 + 21 files changed, 723 insertions(+), 41 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoRoutine.java create mode 100644 src/main/java/frc/robot/commands/DriveToPose.java create mode 100644 src/main/java/frc/robot/commands/DriveTrajectory.java create mode 100644 src/main/java/frc/robot/commands/IntakeCommands.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java rename src/main/java/frc/robot/subsystems/{superstructure => dispenser}/DispenserIO.java (92%) rename src/main/java/frc/robot/subsystems/{superstructure => dispenser}/DispenserIOSim.java (89%) rename src/main/java/frc/robot/subsystems/{superstructure => dispenser}/DispenserIOSpark.java (91%) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java rename src/main/java/frc/robot/subsystems/{superstructure => elevator}/ElevatorIO.java (94%) rename src/main/java/frc/robot/subsystems/{superstructure => elevator}/ElevatorIOSim.java (90%) rename src/main/java/frc/robot/subsystems/{superstructure => elevator}/ElevatorIOSpark.java (90%) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorState.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java create mode 100644 src/main/java/frc/robot/util/trajectory/DriveTrajectories.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7de34c6..2697903 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,8 @@ public class Constants { public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running public static final boolean ENABLE_LOGGING = true; + // Disable LEDs, will reduce software and electrical overhead but disable hardware alerts + public static final boolean ENABLE_LEDs = false; public static final double kLoopPeriodSecs = 0.02; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78aff3b..45c6b98 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,17 +1,20 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.dispenser.DispenserIO; +import frc.robot.subsystems.dispenser.DispenserIOSim; +import frc.robot.subsystems.dispenser.DispenserIOSpark; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.elevator.*; import frc.robot.subsystems.intake.IntakeBase; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSpark; -import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { @@ -21,6 +24,8 @@ public class RobotContainer { // Subsystems private final DriveBase driveBase; private final IntakeBase intakeBase; + private final ElevatorBase elevatorBase; + private final DispenserBase dispenserBase; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -39,6 +44,8 @@ public RobotContainer() { new ModuleIOSpark(2), new ModuleIOSpark(3)); intakeBase = new IntakeBase(new IntakeIOSpark()); + elevatorBase = new ElevatorBase(new ElevatorIOSpark()); + dispenserBase = new DispenserBase(new DispenserIOSpark()); } case SIM -> { driveBase = @@ -49,7 +56,8 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); intakeBase = new IntakeBase(new IntakeIOSim()); - + elevatorBase = new ElevatorBase(new ElevatorIOSim()); + dispenserBase = new DispenserBase(new DispenserIOSim()); } default -> { driveBase = @@ -60,6 +68,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); intakeBase = new IntakeBase(new IntakeIO() {}); + elevatorBase = new ElevatorBase(new ElevatorIO() {}); + dispenserBase = new DispenserBase(new DispenserIO() {}); } } @@ -72,6 +82,19 @@ public RobotContainer() { "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); autoChooser.addOption( "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); + + // autoChooser.addOption( + // "Elevator Dynamic Forward", + // superstructureBase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // autoChooser.addOption( + // "Elevator Dynamic Reverse", + // superstructureBase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + // autoChooser.addOption( + // "Elevator Quasi Forward", + // superstructureBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // autoChooser.addOption( + // "Elevator Quasi Reverse", + // superstructureBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } configureButtonBindings(); @@ -96,22 +119,34 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> Rotation2d.kZero)); - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); + controller.back().onTrue(elevatorBase.homingSequence()); - // Reset gyro to 0° when B button is pressed + controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); + controller + .povLeft() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); + controller.povUp().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL))); controller - .b() - .onTrue( - Commands.runOnce( - () -> - PoseEstimator.getInstance() - .resetPose( - new Pose2d( - PoseEstimator.getInstance().getEstimatedPose().getTranslation(), - AllianceFlipUtil.apply(new Rotation2d()))), - driveBase) - .ignoringDisable(true)); + .povRight() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL))); + + // // Switch to X pattern when X button is pressed + // controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); + + // // Reset gyro to 0° when B button is pressed + // controller + // .b() + // .onTrue( + // Commands.runOnce( + // () -> + // PoseEstimator.getInstance() + // .resetPose( + // new Pose2d( + // + // PoseEstimator.getInstance().getEstimatedPose().getTranslation(), + // AllianceFlipUtil.apply(new Rotation2d()))), + // driveBase) + // .ignoringDisable(true)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AutoRoutine.java b/src/main/java/frc/robot/commands/AutoRoutine.java new file mode 100644 index 0000000..f515ed2 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoRoutine.java @@ -0,0 +1,3 @@ +package frc.robot.commands; + +public class AutoRoutine {} diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 0000000..8914e2f --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,50 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveBase; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.Supplier; + +public class DriveToPose extends Command { + private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("DriveToPose/DrivekP"); + private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("DriveToPose/DrivekD"); + private static final LoggedTunableNumber thetakP = new LoggedTunableNumber("DriveToPose/ThetakP"); + private static final LoggedTunableNumber thetakD = new LoggedTunableNumber("DriveToPose/ThetakD"); + private static final LoggedTunableNumber driveMaxVelocity = + new LoggedTunableNumber("DriveToPose/DriveMaxVelocity"); + private static final LoggedTunableNumber driveMaxVelocitySlow = + new LoggedTunableNumber("DriveToPose/DriveMaxVelocitySlow"); + private static final LoggedTunableNumber driveMaxAcceleration = + new LoggedTunableNumber("DriveToPose/DriveMaxAcceleration"); + private static final LoggedTunableNumber thetaMaxVelocity = + new LoggedTunableNumber("DriveToPose/ThetaMaxVelocity"); + private static final LoggedTunableNumber thetaMaxAcceleration = + new LoggedTunableNumber("DriveToPose/ThetaMaxAcceleration"); + private static final LoggedTunableNumber driveTolerance = + new LoggedTunableNumber("DriveToPose/DriveTolerance"); + private static final LoggedTunableNumber thetaTolerance = + new LoggedTunableNumber("DriveToPose/ThetaTolerance"); + private static final LoggedTunableNumber ffMinRadius = + new LoggedTunableNumber("DriveToPose/FFMinRadius"); + private static final LoggedTunableNumber ffMaxRadius = + new LoggedTunableNumber("DriveToPose/FFMaxRadius"); + + static { + drivekP.initDefault(0.75); + drivekD.initDefault(0.0); + thetakP.initDefault(4.0); + thetakD.initDefault(0.0); + driveMaxVelocity.initDefault(3.8); + driveMaxAcceleration.initDefault(3.0); + thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0)); + thetaMaxAcceleration.initDefault(8.0); + driveTolerance.initDefault(0.01); + thetaTolerance.initDefault(Units.degreesToRadians(1.0)); + ffMinRadius.initDefault(0.1); + ffMaxRadius.initDefault(0.15); + } + + public DriveToPose(DriveBase drive, Supplier target) {} +} diff --git a/src/main/java/frc/robot/commands/DriveTrajectory.java b/src/main/java/frc/robot/commands/DriveTrajectory.java new file mode 100644 index 0000000..11a161c --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveTrajectory.java @@ -0,0 +1,8 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveBase; + +public class DriveTrajectory extends Command { + public DriveTrajectory(DriveBase drive) {} +} diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..f773de7 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,15 @@ +package frc.robot.commands; + +import frc.robot.util.LoggedTunableNumber; + +public class IntakeCommands { + public static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Intake/HopperIntakeVolts", 4.0); + + // public static Command intake(SuperstructureBase superstructure, IntakeBase hopper) { + // // return + // // + // superstructure.runGoal(SuperstructureState.INTAKE).alongWith(Commands.waitUntil(superstructure::atGoal).andThen(hopper.runRoller(intakeVolts.get()))); + // return Commands.none(); + // } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java new file mode 100644 index 0000000..3d8e51a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -0,0 +1,70 @@ +package frc.robot.subsystems.dispenser; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.Debouncer; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.Logger; + +public class DispenserBase extends SubsystemBase { + private static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Dispenser/IntakeVolts", 4.0); + private static final LoggedTunableNumber ejectVolts = + new LoggedTunableNumber("Dispenser/EjectVolts", 4.0); + + private static final LoggedTunableNumber holdingCoralPeriod = + new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); + + private static final LoggedTunableNumber ejectPeriod = + new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.65); + + private final DispenserIO io; + private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); + + @Setter private boolean isEStopped; + @Getter private boolean hasCoral; + + private final Debouncer holdingCoralDebouncer = new Debouncer(0); + + private final Alert disconnected = + new Alert("Dispenser motor disconnected!", Alert.AlertType.kWarning); + + @Setter private double coralVolts = 0.0; + + public DispenserBase(DispenserIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Dispenser", inputs); + + disconnected.set(!inputs.connected); + + if (holdingCoralPeriod.hasChanged()) { + holdingCoralDebouncer.setDebounceTime(holdingCoralPeriod.get()); + } + + // Update if holding coral. + hasCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); + + // Run Coral + if (!isEStopped) { + io.runVolts(coralVolts); + } else { + io.stop(); + } + } + + public Command intakeTillHolding() { + return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> hasCoral); + } + + public Command eject() { + return startEnd(() -> io.runVolts(ejectVolts.get()), io::stop).withTimeout(ejectPeriod.get()); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java new file mode 100644 index 0000000..df11973 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.dispenser; + +class DispenserConstants { + public static final boolean inverted = true; + public static final double moi = 0.025; // TODO + public static final double gearing = 34.0 / 24.0; +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java similarity index 92% rename from src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java rename to src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java index 8b6d442..ff8b74a 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.dispenser; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java similarity index 89% rename from src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java rename to src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java index f7d721f..efcf3a5 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java @@ -1,6 +1,6 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.dispenser; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Dispenser.*; +import static frc.robot.subsystems.dispenser.DispenserConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java similarity index 91% rename from src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java rename to src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java index 523103d..8a689b6 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java @@ -1,10 +1,10 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.dispenser; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Dispenser.*; +import static frc.robot.subsystems.dispenser.DispenserConstants.*; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -21,7 +21,7 @@ public class DispenserIOSpark implements DispenserIO { private final DigitalInput rearBeamBreak = new DigitalInput(0); public DispenserIOSpark() { - spark = new SparkMax(14, SparkLowLevel.MotorType.kBrushless); + spark = new SparkMax(14, MotorType.kBrushless); encoder = spark.getEncoder(); var config = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java new file mode 100644 index 0000000..05c8b99 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -0,0 +1,287 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.util.Debouncer; +import frc.robot.util.EqualsUtil; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class ElevatorBase extends SubsystemBase { + // Tunable numbers + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD"); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); + private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/kA"); + + private static final LoggedTunableNumber maxVelocityMetersPerSec = + new LoggedTunableNumber("Elevator/MaxVelocityMetersPerSec", 2.0); + private static final LoggedTunableNumber maxAccelerationMetersPerSec2 = + new LoggedTunableNumber("Elevator/MaxAccelerationMetersPerSec2", 10); + + private static final LoggedTunableNumber homingVolts = + new LoggedTunableNumber("Elevator/HomingVolts", -2.0); + private static final LoggedTunableNumber homingTimeSecs = + new LoggedTunableNumber("Elevator/HomingTimeSecs", 0.25); + private static final LoggedTunableNumber homingVelocityThresh = + new LoggedTunableNumber("Elevator/HomingVelocityThresh", 5.0); + + private static final LoggedTunableNumber toleranceMeters = + new LoggedTunableNumber("Elevator/ToleranceMeters", 0.2); + + static { + switch (Constants.getRobot()) { + case COMPBOT -> { + kP.initDefault(0); // TODO + kD.initDefault(0); // TODO + kS.initDefault(0.34691); // TODO + kG.initDefault(0.71532); // TODO + kA.initDefault(0.0061913); // TODO + } + case SIMBOT -> { + kP.initDefault(0); // TODO + kD.initDefault(0); // TODO + kS.initDefault(0); // TODO + kG.initDefault(0); // TODO + kA.initDefault(0); // TODO + } + } + } + + private final ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + private final Alert motorDisconnectedAlert = + new Alert("Elevator leader motor disconnected!", Alert.AlertType.kWarning); + private final Alert followerDisconnectedAlert = + new Alert("Elevator follower motor disconnected!", Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Goal") + @Getter + @Setter + private ElevatorState goal = ElevatorState.START; + + private TrapezoidProfile profile; + private State setpoint = new State(); + + private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.0, 0.0); + + private boolean profileDisabled = false; + @Setter private boolean eStopped = false; + + @AutoLogOutput(key = "Elevator/Homed") + @Getter + private boolean homed = false; + + private final Debouncer toleranceDebouncer = new Debouncer(0.25, Debouncer.DebounceType.kRising); + private final Alert outOfTolleranceAlert = + new Alert( + "Elevator emergency disabled due to high position error. Rehome the elevator to reset.", + Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Profile/AtGoal") + @Getter + private boolean atGoal = false; + + private final ElevatorVisualizer measuredVisualizer = new ElevatorVisualizer("Measured"); + private final ElevatorVisualizer setpointVisualizer = new ElevatorVisualizer("Setpoint"); + + private final SysIdRoutine sysId; + + public ElevatorBase(ElevatorIO io) { + this.io = io; + + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(.1).per(Second), + Volts.of(4), + Seconds.of( + 45), // Effectively disable the timeout and allow the Command factories to set + // them + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> io.runOpenLoop(voltage.in(Volts)), + null, // No log consumer, since data is recorded by AdvantageKit + this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + + motorDisconnectedAlert.set(!inputs.leaderConnected); + followerDisconnectedAlert.set(!inputs.followerConnected); + + // Update tunable numbers + if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { + io.setPID(kP.get(), 0.0, kD.get()); + } + if (kS.hasChanged(hashCode()) || kG.hasChanged(hashCode()) || kA.hasChanged(hashCode())) { + feedforward.setKs(kS.get()); + feedforward.setKg(kG.get()); + feedforward.setKa(kA.get()); + } + + if (maxVelocityMetersPerSec.hasChanged(hashCode()) + || maxAccelerationMetersPerSec2.hasChanged(hashCode())) { + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + } + + // Run profile + final boolean shouldRunProfile = + !profileDisabled && homed && !eStopped && DriverStation.isEnabled(); + + Logger.recordOutput("test1", !profileDisabled); + Logger.recordOutput("test2", !eStopped); + + Logger.recordOutput("Elevator/RunningProfile", shouldRunProfile); + + // // Check if out of tolerance + // boolean outOfTolerance = + // Math.abs(getPositionMeters() - setpoint.position) > toleranceMeters.get(); + // boolean shouldEStop = toleranceDebouncer.calculate(outOfTolerance && shouldRunProfile); + // + // outOfTolleranceAlert.set(shouldEStop); + // if (shouldEStop) { + // eStopped = true; + // } + + if (shouldRunProfile) { + // Clamp goal + var goalState = + new State( + MathUtil.clamp(goal.getElevatorHeightMeters().getAsDouble(), 0.0, maxTravel), 0); + + double previousVelocity = setpoint.velocity; + setpoint = profile.calculate(Constants.kLoopPeriodSecs, setpoint, goalState); + if (setpoint.position < 0.0 || setpoint.position > maxTravel) { + setpoint = new State(MathUtil.clamp(setpoint.position, 0.0, maxTravel), 0.0); + } + + io.runPosition( + setpoint.position / drumRadius / numStages, + feedforward.calculateWithVelocities(setpoint.velocity, previousVelocity)); + + // Check at goal + atGoal = + EqualsUtil.epsilonEquals(setpoint.position, goalState.position) + && EqualsUtil.epsilonEquals(setpoint.velocity, goalState.velocity); + + // Stop if at the bottom + if (atGoal && EqualsUtil.epsilonEquals(setpoint.position, 0.0)) { + io.stop(); + } + + // Log state + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", setpoint.position); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", setpoint.velocity); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", goalState.position); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", goalState.velocity); + } else { + if (DriverStation.isDisabled()) { + goal = ElevatorState.STOW; + } + + // Reset setpoint + setpoint = new State(getPositionMeters(), 0.0); + + // Clear logs + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", 0.0); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", 0.0); + } + + if (eStopped) { + io.stop(); + } + + Logger.recordOutput( + "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); + + measuredVisualizer.update(getPositionMeters()); + setpointVisualizer.update(setpoint.position); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.quasistatic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.dynamic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + public Command homingSequence() { + var homingDebouncer = new Debouncer(homingTimeSecs.get()); + return Commands.startRun( + () -> { + profileDisabled = true; + homed = false; + homingDebouncer.calculate(false); + }, + () -> { + io.runOpenLoop(homingVolts.get()); + homed = + homingDebouncer.calculate( + Math.abs(inputs.velocityRadPerSec) <= homingVelocityThresh.get()); + }) + .until(() -> homed) + .andThen( + () -> { + io.resetOrigin(); + homed = true; + }) + .finallyDo(() -> profileDisabled = false); + } + + @AutoLogOutput(key = "Elevator/MeasuredHeightMeters") + public double getPositionMeters() { + return inputs.positionRads * drumRadius * numStages; + } + + public double getGoalMeters() { + return goal.getElevatorHeightMeters().getAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..e237195 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class ElevatorConstants { + // Pitch from Floor to Elevator + public static final Rotation2d elevatorPitch = Rotation2d.fromDegrees(84.5); + // Pitch from Elevator to Dispenser + public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(73.0); + + public static final double drumRadius = Units.inchesToMeters(1.751 / 2.0); + public static final double gearing = 3.0; + public static final int numStages = 2; + + public static final double maxTravel = Units.inchesToMeters(30.0); // TODO + + public static final double carriageMassKg = Units.lbsToKilograms(6.0); + public static final double stagesMassKg = Units.lbsToKilograms(12.0); +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java similarity index 94% rename from src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 7b68624..aceb856 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.elevator; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java similarity index 90% rename from src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 5718cb8..e84b6a8 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,6 +1,6 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.elevator; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Elevator.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -12,7 +12,7 @@ public class ElevatorIOSim implements ElevatorIO { private final ElevatorSim sim = new ElevatorSim( elevatorMotorModel, - SuperstructureConstants.Elevator.gearing, + gearing, carriageMassKg + stagesMassKg, drumRadius, 0, diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java similarity index 90% rename from src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java index 95df4e8..17e62c1 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -1,11 +1,11 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.elevator; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Elevator.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.util.Debouncer; @@ -22,8 +22,8 @@ public class ElevatorIOSpark implements ElevatorIO { private final Debouncer followerConnectedDebouncer = new Debouncer(0.5); public ElevatorIOSpark() { - leaderSpark = new SparkMax(12, SparkLowLevel.MotorType.kBrushless); - followerSpark = new SparkMax(13, SparkLowLevel.MotorType.kBrushless); + leaderSpark = new SparkMax(12, MotorType.kBrushless); + followerSpark = new SparkMax(13, MotorType.kBrushless); encoder = leaderSpark.getEncoder(); controller = leaderSpark.getClosedLoopController(); @@ -136,8 +136,7 @@ public void setPID(double kP, double kI, double kD) { @Override public void setBrakeMode(boolean enabled) { var brakeModeConfig = new SparkMaxConfig(); - brakeModeConfig.idleMode( - enabled ? SparkBaseConfig.IdleMode.kBrake : SparkBaseConfig.IdleMode.kCoast); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); leaderSpark.configure( brakeModeConfig, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java new file mode 100644 index 0000000..48a0652 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.util.Units; +import frc.robot.FieldConstants.ReefLevel; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +@Getter +@RequiredArgsConstructor +public enum ElevatorState { + START(() -> 0.0), + STOW("Stow", 0.0), + INTAKE("Intake", 0.0), + L1_CORAL(ReefLevel.L1, Units.inchesToMeters(0.0)), + L2_CORAL(ReefLevel.L2, Units.inchesToMeters(0.0)), + L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0)); + + private final DoubleSupplier elevatorHeightMeters; + + ElevatorState(String name, double defaultValue) { + elevatorHeightMeters = new LoggedTunableNumber("Elevator/Presets/" + name, defaultValue); + } + + ElevatorState(ReefLevel reefLevel, double defaultOffset) { + var offsetTunable = + new LoggedTunableNumber( + String.format("Elevator/Presets/%s Offset", reefLevel), defaultOffset); + elevatorHeightMeters = () -> reefLevel.height + offsetTunable.get(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..0196eb9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.elevator; + +public class ElevatorVisualizer { + private final String name; + + public ElevatorVisualizer(String name) { + this.name = name; + } + + public void update(double elevatorPositionMeters) {} +} diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java index 0f51f72..4e1e513 100644 --- a/src/main/java/frc/robot/util/AlertsUtil.java +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -1,13 +1,23 @@ package frc.robot.util; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +// LED Alerts +// endgameAlert +// autoScoring (Reef Side denotes side, color denotes level) +// superstructureEstopped +// lowBatteryAlert +// visionDisconnected +// following trajectory +// go to pose +// feederstation alert (alert feeder station player about being OTW) + +// TODO indicate robot initializing in LEDs + public class AlertsUtil { + private static final int numLeds = 120; private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; private static AlertsUtil instance; @@ -24,18 +34,32 @@ public static AlertsUtil getInstance() { private final Alert canErrorAlert = new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); private final Debouncer canErrorDebouncer = new Debouncer(0.5); - private final Alert lowBatteryVoltageAlert = new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); // Program Alerts private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); + private final Alert ledsDisabledAlert = + new Alert("LEDs disabled by program override", AlertType.kInfo); + + private AddressableLED leds; + private AddressableLEDBuffer ledBuffer; private AlertsUtil() { if (Constants.TUNING_MODE) { tuningModeAlert.set(true); } + + if (Constants.ENABLE_LEDs) { + leds = new AddressableLED(1); + ledBuffer = new AddressableLEDBuffer(numLeds); + + leds.setLength(ledBuffer.getLength()); + leds.start(); + } else { + ledsDisabledAlert.set(true); + } } public void periodic() { @@ -50,5 +74,108 @@ public void periodic() { lowBatteryVoltageAlert.set( batteryVoltageDebouncer.calculate( RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + + // Update Program Alerts + // TODO + + // Update Hardware Alerts + if (leds == null) return; + + // // Disable LEDs if battery voltage is too low + // if(ledVoltageDebouncer.calculate( + // RobotController.getBatteryVoltage() <= LED_DISABLE_VOLTAGE_THRESHOLD)) { + // ledsDisabledAlert.set(true); + // ledLowVoltageDisabledAlert.set(true); + // + // leds.stop(); + // leds.close(); + // + // leds = null; + // ledBuffer = null; + // + // return; + } + + // private Color solid(Section section, Color color) { + // if (color != null) { + // for (int i = section.start(); i < section.end(); i++) { + // ledBuffer.setLED(i, color); + // } + // } + // return color; + // } + // + // private Color strobe(Section section, Color c1, Color c2, double duration) { + // boolean c1On = ((Timer.getTimestamp() % duration) / duration) > 0.5; + // return solid(section, c1On ? c1 : c2); + // } + // + // private Color breath(Section section, Color c1, Color c2, double duration, double timestamp) { + // double x = ((timestamp % duration) / duration) * 2.0 * Math.PI; + // double ratio = (Math.sin(x) + 1.0) / 2.0; + // double red = (c1.red * (1 - ratio)) + (c2.red * ratio); + // double green = (c1.green * (1 - ratio)) + (c2.green * ratio); + // double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio); + // var color = new Color(red, green, blue); + // solid(section, color); + // return color; + // } + // + // private Color breath(Section section, Color c1, Color c2, double duration) { + // return breath(section, c1, c2, duration, Timer.getTimestamp()); + // } + // + // private void rainbow(Section section, double cycleLength, double duration) { + // double x = (1 - ((Timer.getTimestamp() / duration) % 1.0)) * 180.0; + // double xDiffPerLed = 180.0 / cycleLength; + // for (int i = section.end() - 1; i >= section.start(); i--) { + // x += xDiffPerLed; + // x %= 180.0; + // ledBuffer.setHSV(i, (int) x, 255, 255); + // } + // } + // + // private void wave(Section section, Color c1, Color c2, double cycleLength, double duration) { + // double x = (1 - ((Timer.getTimestamp() % duration) / duration)) * 2.0 * Math.PI; + // double xDiffPerLed = (2.0 * Math.PI) / cycleLength; + // for (int i = section.end() - 1; i >= section.start(); i--) { + // x += xDiffPerLed; + // double ratio = (Math.pow(Math.sin(x), waveExponent) + 1.0) / 2.0; + // if (Double.isNaN(ratio)) { + // ratio = (-Math.pow(Math.sin(x + Math.PI), waveExponent) + 1.0) / 2.0; + // } + // if (Double.isNaN(ratio)) { + // ratio = 0.5; + // } + // double red = (c1.red * (1 - ratio)) + (c2.red * ratio); + // double green = (c1.green * (1 - ratio)) + (c2.green * ratio); + // double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio); + // ledBuffer.setLED(i, new Color(red, green, blue)); + // } + // } + // + // private void stripes(Section section, List colors, int stripeLength, double duration) { + // int offset = (int) (Timer.getTimestamp() % duration / duration * stripeLength * + // colors.size()); + // for (int i = section.end() - 1; i >= section.start(); i--) { + // int colorIndex = + // (int) (Math.floor((double) (i - offset) / stripeLength) + colors.size()) % + // colors.size(); + // colorIndex = colors.size() - 1 - colorIndex; + // ledBuffer.setLED(i, colors.get(colorIndex)); + // } + // } + + // LEDs have a base mode / pattern / effect + // alerts will update and override specific sections if something is active + // apply the buffer + + public static class HardwareIndicatedAlert extends Alert { + public HardwareIndicatedAlert( + String text, AlertType type, int priority /*TODO handle LED stuff*/) { + super(text, type); + } } + + private static record Section(int start, int end) {} } diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 71e4c88..2f67543 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -4,13 +4,14 @@ import java.util.Arrays; import java.util.HashMap; import java.util.Map; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. */ -public class LoggedTunableNumber { +public class LoggedTunableNumber implements DoubleSupplier { private static final String tableKey = "TunableNumbers"; private final String key; @@ -131,4 +132,9 @@ public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumb action.run(); } } + + @Override + public double getAsDouble() { + return get(); + } } diff --git a/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java b/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java new file mode 100644 index 0000000..316b2da --- /dev/null +++ b/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java @@ -0,0 +1,10 @@ +package frc.robot.util.trajectory; + +public class DriveTrajectories { + // Let Feederpose be some middle point between all feeder station nodes + // Let face_#_pose be some point between both left and right reef stations BUT not flush with the + // reef + + // known non-OTF trajectories + // feederpose to face_n_pose +} From 07083ba1d80b32fe6304f25afe028c50cd5f4395 Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Wed, 26 Feb 2025 01:36:12 -0500 Subject: [PATCH 36/73] fixes --- .../frc/robot/subsystems/elevator/ElevatorBase.java | 10 +++++----- .../robot/subsystems/elevator/ElevatorConstants.java | 5 ++++- .../frc/robot/subsystems/elevator/ElevatorIOSpark.java | 2 +- .../frc/robot/subsystems/elevator/ElevatorState.java | 8 +++++++- 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 05c8b99..c39b680 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -48,11 +48,11 @@ public class ElevatorBase extends SubsystemBase { static { switch (Constants.getRobot()) { case COMPBOT -> { - kP.initDefault(0); // TODO - kD.initDefault(0); // TODO - kS.initDefault(0.34691); // TODO - kG.initDefault(0.71532); // TODO - kA.initDefault(0.0061913); // TODO + kP.initDefault(0.3); // TODO + kD.initDefault(0.25); // TODO + kS.initDefault(0); // TODO + kG.initDefault(1.05); // TODO + kA.initDefault(0.0); // TODO } case SIMBOT -> { kP.initDefault(0); // TODO diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index e237195..85c509a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -9,11 +9,14 @@ public class ElevatorConstants { // Pitch from Elevator to Dispenser public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(73.0); + public static final double originToBaseHeightMeters = Units.inchesToMeters(5.149922); + public static final double drumRadius = Units.inchesToMeters(1.751 / 2.0); public static final double gearing = 3.0; public static final int numStages = 2; - public static final double maxTravel = Units.inchesToMeters(30.0); // TODO + // public static final double maxTravel = Units.inchesToMeters(42.244094); // TODO + public static final double maxTravel = Units.inchesToMeters(42); // TODO public static final double carriageMassKg = Units.lbsToKilograms(6.0); public static final double stagesMassKg = Units.lbsToKilograms(12.0); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java index 17e62c1..f028ec0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -107,7 +107,7 @@ public void runPosition(double positionRads, double feedforwardVolts) { controller.setReference( positionRads, SparkBase.ControlType.kPosition, - ClosedLoopSlot.kSlot1, + ClosedLoopSlot.kSlot0, feedforwardVolts, SparkClosedLoopController.ArbFFUnits.kVoltage); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index 48a0652..5fa8ed6 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.elevator; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + import edu.wpi.first.math.util.Units; import frc.robot.FieldConstants.ReefLevel; import frc.robot.util.LoggedTunableNumber; @@ -27,6 +29,10 @@ public enum ElevatorState { var offsetTunable = new LoggedTunableNumber( String.format("Elevator/Presets/%s Offset", reefLevel), defaultOffset); - elevatorHeightMeters = () -> reefLevel.height + offsetTunable.get(); + + elevatorHeightMeters = + () -> + (reefLevel.height - originToBaseHeightMeters) / elevatorPitch.getSin() + + offsetTunable.get(); } } From d267a5f5fa61a4fc23719ef03079e075d88fb78c Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 01:54:57 -0500 Subject: [PATCH 37/73] add elevator and end effector subsystems --- .../subsystems/dispenser/DispenserBase.java | 62 ++++ .../dispenser/DispenserConstants.java | 7 + .../DispenserIO.java | 2 +- .../DispenserIOSim.java | 4 +- .../DispenserIOSpark.java | 8 +- .../subsystems/elevator/ElevatorBase.java | 283 ++++++++++++++++++ .../elevator/ElevatorConstants.java | 23 ++ .../ElevatorIO.java | 2 +- .../ElevatorIOSim.java | 6 +- .../ElevatorIOSpark.java | 15 +- .../subsystems/elevator/ElevatorState.java | 37 +++ .../frc/robot/util/LoggedTunableNumber.java | 8 +- 12 files changed, 437 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java rename src/main/java/frc/robot/subsystems/{superstructure => dispenser}/DispenserIO.java (92%) rename src/main/java/frc/robot/subsystems/{superstructure => dispenser}/DispenserIOSim.java (89%) rename src/main/java/frc/robot/subsystems/{superstructure => dispenser}/DispenserIOSpark.java (91%) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java rename src/main/java/frc/robot/subsystems/{superstructure => elevator}/ElevatorIO.java (94%) rename src/main/java/frc/robot/subsystems/{superstructure => elevator}/ElevatorIOSim.java (90%) rename src/main/java/frc/robot/subsystems/{superstructure => elevator}/ElevatorIOSpark.java (90%) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorState.java diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java new file mode 100644 index 0000000..f312008 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.dispenser; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.Debouncer; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.Logger; + +public class DispenserBase extends SubsystemBase { + private static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Dispenser/IntakeVolts", 6.0); + private static final LoggedTunableNumber ejectVolts = + new LoggedTunableNumber("Dispenser/EjectVolts", 2.0); + + private static final LoggedTunableNumber holdingCoralPeriod = + new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); + + private static final LoggedTunableNumber ejectPeriod = + new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.65); + + private final DispenserIO io; + private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); + + @Getter private boolean hasCoral; + + private final Debouncer holdingCoralDebouncer = new Debouncer(0); + + private final Alert disconnected = + new Alert("Dispenser motor disconnected!", Alert.AlertType.kWarning); + + @Setter private double coralVolts = 0.0; + + public DispenserBase(DispenserIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Dispenser", inputs); + + disconnected.set(!inputs.connected); + + if (holdingCoralPeriod.hasChanged()) { + holdingCoralDebouncer.setDebounceTime(holdingCoralPeriod.get()); + } + + // Update if holding coral. + hasCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); + } + + public Command intakeTillHolding() { + return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> hasCoral); + } + + public Command eject() { + return startEnd(() -> io.runVolts(ejectVolts.get()), io::stop).withTimeout(ejectPeriod.get()); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java new file mode 100644 index 0000000..df11973 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.dispenser; + +class DispenserConstants { + public static final boolean inverted = true; + public static final double moi = 0.025; // TODO + public static final double gearing = 34.0 / 24.0; +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java similarity index 92% rename from src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java rename to src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java index 8b6d442..ff8b74a 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIO.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.dispenser; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java similarity index 89% rename from src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java rename to src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java index f7d721f..efcf3a5 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSim.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java @@ -1,6 +1,6 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.dispenser; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Dispenser.*; +import static frc.robot.subsystems.dispenser.DispenserConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java similarity index 91% rename from src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java rename to src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java index 523103d..8a689b6 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/DispenserIOSpark.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java @@ -1,10 +1,10 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.dispenser; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Dispenser.*; +import static frc.robot.subsystems.dispenser.DispenserConstants.*; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -21,7 +21,7 @@ public class DispenserIOSpark implements DispenserIO { private final DigitalInput rearBeamBreak = new DigitalInput(0); public DispenserIOSpark() { - spark = new SparkMax(14, SparkLowLevel.MotorType.kBrushless); + spark = new SparkMax(14, MotorType.kBrushless); encoder = spark.getEncoder(); var config = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java new file mode 100644 index 0000000..35040ff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -0,0 +1,283 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.util.Debouncer; +import frc.robot.util.EqualsUtil; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class ElevatorBase extends SubsystemBase { + // Tunable numbers + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD"); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); + private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/kA"); + + private static final LoggedTunableNumber maxVelocityMetersPerSec = + new LoggedTunableNumber("Elevator/MaxVelocityMetersPerSec", 2.0); + private static final LoggedTunableNumber maxAccelerationMetersPerSec2 = + new LoggedTunableNumber("Elevator/MaxAccelerationMetersPerSec2", 10); + + private static final LoggedTunableNumber homingVolts = + new LoggedTunableNumber("Elevator/HomingVolts", -2.0); + private static final LoggedTunableNumber homingTimeSecs = + new LoggedTunableNumber("Elevator/HomingTimeSecs", 0.25); + private static final LoggedTunableNumber homingVelocityThresh = + new LoggedTunableNumber("Elevator/HomingVelocityThresh", 5.0); + + private static final LoggedTunableNumber toleranceMeters = + new LoggedTunableNumber("Elevator/ToleranceMeters", 0.2); + + static { + switch (Constants.getRobot()) { + case COMPBOT -> { + kP.initDefault(0.3); + kD.initDefault(0.25); + kS.initDefault(0); + kG.initDefault(1.05); + kA.initDefault(0.0); + } + case SIMBOT -> { + kP.initDefault(0); // TODO + kD.initDefault(0); // TODO + kS.initDefault(0); // TODO + kG.initDefault(0); // TODO + kA.initDefault(0); // TODO + } + } + } + + private final ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + private final Alert motorDisconnectedAlert = + new Alert("Elevator leader motor disconnected!", Alert.AlertType.kWarning); + private final Alert followerDisconnectedAlert = + new Alert("Elevator follower motor disconnected!", Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Goal") + @Getter + @Setter + private ElevatorState goal = ElevatorState.START; + + private TrapezoidProfile profile; + private State setpoint = new State(); + + private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.0, 0.0); + + private boolean profileDisabled = false; + @Setter private boolean eStopped = false; + + @AutoLogOutput(key = "Elevator/Homed") + @Getter + private boolean homed = false; + + private final Debouncer toleranceDebouncer = new Debouncer(0.25, Debouncer.DebounceType.kRising); + private final Alert outOfTolleranceAlert = + new Alert( + "Elevator emergency disabled due to high position error. Rehome the elevator to reset.", + Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Profile/AtGoal") + @Getter + private boolean atGoal = false; + + private final ElevatorVisualizer measuredVisualizer = new ElevatorVisualizer("Measured"); + private final ElevatorVisualizer setpointVisualizer = new ElevatorVisualizer("Setpoint"); + + private final SysIdRoutine sysId; + + public ElevatorBase(ElevatorIO io) { + this.io = io; + + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(.1).per(Second), + Volts.of(4), + Seconds.of( + 45), // Effectively disable the timeout and allow the Command factories to set + // them + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> io.runOpenLoop(voltage.in(Volts)), + null, // No log consumer, since data is recorded by AdvantageKit + this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + + motorDisconnectedAlert.set(!inputs.leaderConnected); + followerDisconnectedAlert.set(!inputs.followerConnected); + + // Update tunable numbers + if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { + io.setPID(kP.get(), 0.0, kD.get()); + } + if (kS.hasChanged(hashCode()) || kG.hasChanged(hashCode()) || kA.hasChanged(hashCode())) { + feedforward.setKs(kS.get()); + feedforward.setKg(kG.get()); + feedforward.setKa(kA.get()); + } + + if (maxVelocityMetersPerSec.hasChanged(hashCode()) + || maxAccelerationMetersPerSec2.hasChanged(hashCode())) { + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + } + + // Run profile + final boolean shouldRunProfile = + !profileDisabled && homed && !eStopped && DriverStation.isEnabled(); + Logger.recordOutput("Elevator/RunningProfile", shouldRunProfile); + + // // Check if out of tolerance + // boolean outOfTolerance = + // Math.abs(getPositionMeters() - setpoint.position) > toleranceMeters.get(); + // boolean shouldEStop = toleranceDebouncer.calculate(outOfTolerance && shouldRunProfile); + // + // outOfTolleranceAlert.set(shouldEStop); + // if (shouldEStop) { + // eStopped = true; + // } + + if (shouldRunProfile) { + // Clamp goal + var goalState = + new State( + MathUtil.clamp(goal.getElevatorHeightMeters().getAsDouble(), 0.0, maxTravel), 0); + + double previousVelocity = setpoint.velocity; + setpoint = profile.calculate(Constants.kLoopPeriodSecs, setpoint, goalState); + if (setpoint.position < 0.0 || setpoint.position > maxTravel) { + setpoint = new State(MathUtil.clamp(setpoint.position, 0.0, maxTravel), 0.0); + } + + io.runPosition( + setpoint.position / drumRadius / numStages, + feedforward.calculateWithVelocities(setpoint.velocity, previousVelocity)); + + // Check at goal + atGoal = + EqualsUtil.epsilonEquals(setpoint.position, goalState.position) + && EqualsUtil.epsilonEquals(setpoint.velocity, goalState.velocity); + + // Stop if at the bottom + if (atGoal && EqualsUtil.epsilonEquals(setpoint.position, 0.0)) { + io.stop(); + } + + // Log state + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", setpoint.position); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", setpoint.velocity); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", goalState.position); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", goalState.velocity); + } else { + if (DriverStation.isDisabled()) { + goal = ElevatorState.STOW; + } + + // Reset setpoint + setpoint = new State(getPositionMeters(), 0.0); + + // Clear logs + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", 0.0); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", 0.0); + } + + if (eStopped) { + io.stop(); + } + + Logger.recordOutput( + "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); + + measuredVisualizer.update(getPositionMeters()); + setpointVisualizer.update(setpoint.position); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.quasistatic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.dynamic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + public Command homingSequence() { + var homingDebouncer = new Debouncer(homingTimeSecs.get()); + return Commands.startRun( + () -> { + profileDisabled = true; + homed = false; + homingDebouncer.calculate(false); + }, + () -> { + io.runOpenLoop(homingVolts.get()); + homed = + homingDebouncer.calculate( + Math.abs(inputs.velocityRadPerSec) <= homingVelocityThresh.get()); + }) + .until(() -> homed) + .andThen( + () -> { + io.resetOrigin(); + homed = true; + }) + .finallyDo(() -> profileDisabled = false); + } + + @AutoLogOutput(key = "Elevator/MeasuredHeightMeters") + public double getPositionMeters() { + return inputs.positionRads * drumRadius * numStages; + } + + public double getGoalMeters() { + return goal.getElevatorHeightMeters().getAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..85c509a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class ElevatorConstants { + // Pitch from Floor to Elevator + public static final Rotation2d elevatorPitch = Rotation2d.fromDegrees(84.5); + // Pitch from Elevator to Dispenser + public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(73.0); + + public static final double originToBaseHeightMeters = Units.inchesToMeters(5.149922); + + public static final double drumRadius = Units.inchesToMeters(1.751 / 2.0); + public static final double gearing = 3.0; + public static final int numStages = 2; + + // public static final double maxTravel = Units.inchesToMeters(42.244094); // TODO + public static final double maxTravel = Units.inchesToMeters(42); // TODO + + public static final double carriageMassKg = Units.lbsToKilograms(6.0); + public static final double stagesMassKg = Units.lbsToKilograms(12.0); +} diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java similarity index 94% rename from src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 7b68624..aceb856 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.elevator; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java similarity index 90% rename from src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 5718cb8..e84b6a8 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -1,6 +1,6 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.elevator; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Elevator.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -12,7 +12,7 @@ public class ElevatorIOSim implements ElevatorIO { private final ElevatorSim sim = new ElevatorSim( elevatorMotorModel, - SuperstructureConstants.Elevator.gearing, + gearing, carriageMassKg + stagesMassKg, drumRadius, 0, diff --git a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java similarity index 90% rename from src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java index 95df4e8..f028ec0 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -1,11 +1,11 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.subsystems.elevator; -import static frc.robot.subsystems.superstructure.SuperstructureConstants.Elevator.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.util.Debouncer; @@ -22,8 +22,8 @@ public class ElevatorIOSpark implements ElevatorIO { private final Debouncer followerConnectedDebouncer = new Debouncer(0.5); public ElevatorIOSpark() { - leaderSpark = new SparkMax(12, SparkLowLevel.MotorType.kBrushless); - followerSpark = new SparkMax(13, SparkLowLevel.MotorType.kBrushless); + leaderSpark = new SparkMax(12, MotorType.kBrushless); + followerSpark = new SparkMax(13, MotorType.kBrushless); encoder = leaderSpark.getEncoder(); controller = leaderSpark.getClosedLoopController(); @@ -107,7 +107,7 @@ public void runPosition(double positionRads, double feedforwardVolts) { controller.setReference( positionRads, SparkBase.ControlType.kPosition, - ClosedLoopSlot.kSlot1, + ClosedLoopSlot.kSlot0, feedforwardVolts, SparkClosedLoopController.ArbFFUnits.kVoltage); } @@ -136,8 +136,7 @@ public void setPID(double kP, double kI, double kD) { @Override public void setBrakeMode(boolean enabled) { var brakeModeConfig = new SparkMaxConfig(); - brakeModeConfig.idleMode( - enabled ? SparkBaseConfig.IdleMode.kBrake : SparkBaseConfig.IdleMode.kCoast); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); leaderSpark.configure( brakeModeConfig, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java new file mode 100644 index 0000000..a24d6f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.util.Units; +import frc.robot.FieldConstants.ReefLevel; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +@Getter +@RequiredArgsConstructor +public enum ElevatorState { + START(() -> 0.0), + STOW("Stow", 0.0), + INTAKE("Intake", 0.0), + L1_CORAL(ReefLevel.L1, Units.inchesToMeters(0.0)), + L2_CORAL(ReefLevel.L2, Units.inchesToMeters(1)), + L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0)); + + private final DoubleSupplier elevatorHeightMeters; + + ElevatorState(String name, double defaultValue) { + elevatorHeightMeters = new LoggedTunableNumber("Elevator/Presets/" + name, defaultValue); + } + + ElevatorState(ReefLevel reefLevel, double defaultOffset) { + var offsetTunable = + new LoggedTunableNumber( + String.format("Elevator/Presets/%s Offset", reefLevel), defaultOffset); + elevatorHeightMeters = + () -> + (reefLevel.height + offsetTunable.get() - originToBaseHeightMeters) + / elevatorPitch.getSin(); + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 71e4c88..2f67543 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -4,13 +4,14 @@ import java.util.Arrays; import java.util.HashMap; import java.util.Map; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. */ -public class LoggedTunableNumber { +public class LoggedTunableNumber implements DoubleSupplier { private static final String tableKey = "TunableNumbers"; private final String key; @@ -131,4 +132,9 @@ public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumb action.run(); } } + + @Override + public double getAsDouble() { + return get(); + } } From d1f8beb647341078b591bf9d92889cd945de9e86 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 10:24:33 -0500 Subject: [PATCH 38/73] add sprint --- .../frc/robot/commands/DriveCommands.java | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 3d9d4b7..1bad7eb 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -10,18 +10,21 @@ import frc.robot.PoseEstimator; import frc.robot.subsystems.drive.DriveBase; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class DriveCommands { // Drive private static final double DEADBAND = 0.1; - private static final LoggedNetworkNumber LINEAR_VELOCITY_SCALAR = - new LoggedNetworkNumber("TeleopDrive/LinearVelocityScalar", 1.0); - private static final LoggedNetworkNumber ANGULAR_VELOCITY_SCALAR = - new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 0.7); + private static final LoggedTunableNumber teleopLinearScalar = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 0.5); + private static final LoggedTunableNumber teleopLinearScalarSprint = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 1.0); + private static final LoggedTunableNumber angularScalar = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 1.0); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; @@ -35,7 +38,8 @@ public static Command joystickDrive( DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { + DoubleSupplier omegaSupplier, + BooleanSupplier sprintSupplier) { return Commands.run( () -> { // Apply deadband @@ -49,8 +53,11 @@ public static Command joystickDrive( omega = Math.copySign(Math.pow(omega, 2), omega); // Generate robot relative speeds - double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); - double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get(); + double linearVelocityScalar = + sprintSupplier.getAsBoolean() + ? teleopLinearScalarSprint.get() + : teleopLinearScalar.get(); + double angularVelocityScalar = angularScalar.get(); var speeds = new ChassisSpeeds( @@ -80,7 +87,8 @@ public static Command joystickDriveAtAngle( DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - Supplier rotationSupplier) { + Supplier rotationSupplier, + BooleanSupplier sprintSupplier) { ProfiledPIDController angleController = new ProfiledPIDController( ANGLE_KP, @@ -106,7 +114,10 @@ public static Command joystickDriveAtAngle( rotation.getRadians(), rotationSupplier.get().getRadians()); // Generate robot relative speeds - double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); + double linearVelocityScalar = + sprintSupplier.getAsBoolean() + ? teleopLinearScalarSprint.get() + : teleopLinearScalar.get(); var speeds = new ChassisSpeeds( x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, From 1d3b19195f0965de2da490d954342cacf353d2dc Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 10:24:53 -0500 Subject: [PATCH 39/73] add proto auto intake --- .../frc/robot/commands/IntakeCommands.java | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 src/main/java/frc/robot/commands/IntakeCommands.java diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..2a21a15 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,23 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.elevator.ElevatorBase; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.util.LoggedTunableNumber; + +public class IntakeCommands { + public static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Intake/HopperIntakeVolts", 6.0); + + public static Command intake(ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { + return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) + .alongWith( + Commands.waitUntil(elevator::isAtGoal) + .andThen( + Commands.deadline( + dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())))); + } +} From e7fadb78556a97f651eceaa2934ee1f3684f20f4 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 11:20:15 -0500 Subject: [PATCH 40/73] schedule home command if not homed on teleop init --- .../java/frc/robot/subsystems/elevator/ElevatorBase.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 35040ff..8225b3e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -221,6 +221,11 @@ public void periodic() { Logger.recordOutput( "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); + // If not homed, schedule that command + if (!homed) { + homingSequence().schedule(); + } + measuredVisualizer.update(getPositionMeters()); setpointVisualizer.update(setpoint.position); } From 6cfc8e339566071b0dfe1b55291828f5523012ca Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 14:57:45 -0500 Subject: [PATCH 41/73] Update LoggedTunableNumber.java --- src/main/java/frc/robot/util/LoggedTunableNumber.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 2f67543..30e0232 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -12,7 +12,7 @@ * value not in dashboard. */ public class LoggedTunableNumber implements DoubleSupplier { - private static final String tableKey = "TunableNumbers"; + private static final String tableKey = "/TunableNumbers"; private final String key; private Double defaultValue = null; From b32b9fc75f2d4b8d5533fadc176c701eec4eff0d Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 18:48:48 -0500 Subject: [PATCH 42/73] Update RobotContainer.java --- src/main/java/frc/robot/RobotContainer.java | 100 ++++++++++++++++++-- 1 file changed, 92 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78aff3b..f135a68 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,17 +2,28 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.commands.IntakeCommands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.dispenser.DispenserIO; +import frc.robot.subsystems.dispenser.DispenserIOSim; +import frc.robot.subsystems.dispenser.DispenserIOSpark; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.elevator.*; import frc.robot.subsystems.intake.IntakeBase; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSpark; import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class RobotContainer { // Load PoseEstimator class @@ -21,6 +32,8 @@ public class RobotContainer { // Subsystems private final DriveBase driveBase; private final IntakeBase intakeBase; + private final ElevatorBase elevatorBase; + private final DispenserBase dispenserBase; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -28,6 +41,11 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + private final LoggedNetworkNumber endgameAlert1 = + new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #1", 30.0); + private final LoggedNetworkNumber endgameAlert2 = + new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0); + public RobotContainer() { switch (Constants.getMode()) { case REAL -> { @@ -39,6 +57,8 @@ public RobotContainer() { new ModuleIOSpark(2), new ModuleIOSpark(3)); intakeBase = new IntakeBase(new IntakeIOSpark()); + elevatorBase = new ElevatorBase(new ElevatorIOSpark()); + dispenserBase = new DispenserBase(new DispenserIOSpark()); } case SIM -> { driveBase = @@ -49,7 +69,8 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); intakeBase = new IntakeBase(new IntakeIOSim()); - + elevatorBase = new ElevatorBase(new ElevatorIOSim()); + dispenserBase = new DispenserBase(new DispenserIOSim()); } default -> { driveBase = @@ -60,6 +81,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); intakeBase = new IntakeBase(new IntakeIO() {}); + elevatorBase = new ElevatorBase(new ElevatorIO() {}); + dispenserBase = new DispenserBase(new DispenserIO() {}); } } @@ -72,6 +95,18 @@ public RobotContainer() { "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); autoChooser.addOption( "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); + autoChooser.addOption( + "Elevator Dynamic Forward", + elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kForward, 0.5)); + autoChooser.addOption( + "Elevator Dynamic Reverse", + elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kReverse, 0.25)); + autoChooser.addOption( + "Elevator Quasi Forward", + elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward, 25)); + autoChooser.addOption( + "Elevator Quasi Reverse", + elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse, 3)); } configureButtonBindings(); @@ -84,9 +119,10 @@ private void configureButtonBindings() { driveBase, () -> -controller.getLeftY(), () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -controller.getRightX(), + controller.leftStick())); - // Lock to 0° when A button is held + // TODO Lock to Feeder Station when held controller .a() .whileTrue( @@ -94,14 +130,39 @@ private void configureButtonBindings() { driveBase, () -> -controller.getLeftY(), () -> -controller.getLeftX(), - () -> Rotation2d.kZero)); + () -> Rotation2d.kZero, + controller.leftStick())); - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); + // Stow + controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); + // Intake + controller.b().onTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); + // L1 + controller + .povLeft() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); + // L2 + controller.povUp().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL))); + // L3 + controller + .povRight() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL))); + // Dispense + controller.rightTrigger().onTrue(dispenserBase.eject()); - // Reset gyro to 0° when B button is pressed + // Home Elevator + controller.back().debounce(0.5).onTrue(elevatorBase.homingSequence()); + + // Auto Align (Left or Right) + // TODO + + // Human Player Alert (Strobe LEDs) + // TODO + + // Reset Gyro controller - .b() + .start() + .and(controller.back()) .onTrue( Commands.runOnce( () -> @@ -112,6 +173,29 @@ private void configureButtonBindings() { AllianceFlipUtil.apply(new Rotation2d()))), driveBase) .ignoringDisable(true)); + + // Endgame + new Trigger( + () -> + DriverStation.isTeleopEnabled() + && DriverStation.getMatchTime() > 0 + && DriverStation.getMatchTime() <= Math.round(endgameAlert1.get())) + .onTrue( + Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0)) + .withTimeout(0.5)); + + new Trigger( + () -> + DriverStation.isTeleopEnabled() + && DriverStation.getMatchTime() > 0 + && DriverStation.getMatchTime() <= Math.round(endgameAlert2.get())) + .onTrue( + Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0)) + .withTimeout(0.5)); } public Command getAutonomousCommand() { From 2e7b277a430c9b1cfb39e9c44657eda154c0ca3f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 26 Feb 2025 18:48:56 -0500 Subject: [PATCH 43/73] Create ElevatorVisualizer.java --- .../robot/subsystems/elevator/ElevatorVisualizer.java | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..0196eb9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.elevator; + +public class ElevatorVisualizer { + private final String name; + + public ElevatorVisualizer(String name) { + this.name = name; + } + + public void update(double elevatorPositionMeters) {} +} From 32aca1e42958e7faccf81f6fc759329ae5b35b0b Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu, 27 Feb 2025 01:05:11 -0500 Subject: [PATCH 44/73] tuning stuff --- src/main/java/frc/robot/FieldConstants.java | 6 +++ src/main/java/frc/robot/RobotContainer.java | 47 ++++++++++++++----- src/main/java/frc/robot/basement-layout.json | 45 ++++++++++++++++++ .../subsystems/dispenser/DispenserBase.java | 16 +++---- .../frc/robot/subsystems/drive/DriveBase.java | 33 ++++++++++++- .../frc/robot/subsystems/drive/Module.java | 20 +++++--- .../subsystems/elevator/ElevatorBase.java | 6 +-- .../subsystems/elevator/ElevatorState.java | 2 +- 8 files changed, 142 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc/robot/basement-layout.json diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 9a3e7b2..6bc65c4 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,9 +1,11 @@ package frc.robot; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import java.io.IOException; +import java.nio.file.Path; import java.util.*; import lombok.Getter; @@ -14,6 +16,10 @@ public class FieldConstants { public static AprilTagFieldLayout fieldLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + static { + System.out.println(Path.of(AprilTagFields.k2025ReefscapeWelded.toString())); + } + public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldLength(); public static final double fieldWidth = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ccdf07d..32fc491 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; @@ -20,6 +21,7 @@ import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSpark; +import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; @@ -105,6 +107,15 @@ public RobotContainer() { autoChooser.addOption( "Elevator Quasi Reverse", elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse, 3)); + + autoChooser.addOption( + "Drive Dynamic Forward", driveBase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Dynamic Reverse", driveBase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive Quasi Forward", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Quasi Reverse", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } configureButtonBindings(); @@ -115,26 +126,35 @@ private void configureButtonBindings() { driveBase.setDefaultCommand( DriveCommands.joystickDrive( driveBase, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX(), + () -> + controller.y().getAsBoolean() + ? -controller.getLeftY() * 0.7 + : -controller.getLeftY(), + () -> + controller.y().getAsBoolean() + ? -controller.getLeftX() * 0.7 + : -controller.getLeftX(), + () -> + controller.y().getAsBoolean() + ? -controller.getRightX() * 0.7 + : -controller.getRightX(), controller.leftStick())); // TODO Lock to Feeder Station when held - controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - driveBase, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> Rotation2d.kZero, - controller.leftStick())); + // controller + // .y() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // driveBase, + // () -> -controller.getLeftY(), + // () -> -controller.getLeftX(), + // () -> Rotation2d.kZero, + // controller.leftStick())); // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); // Intake - controller.b().onTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); + controller.x().onTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); // L1 controller .povLeft() @@ -161,6 +181,7 @@ private void configureButtonBindings() { controller .start() .and(controller.back()) + .debounce(0.5) .onTrue( Commands.runOnce( () -> diff --git a/src/main/java/frc/robot/basement-layout.json b/src/main/java/frc/robot/basement-layout.json new file mode 100644 index 0000000..daaf4c1 --- /dev/null +++ b/src/main/java/frc/robot/basement-layout.json @@ -0,0 +1,45 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.697198, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.052 + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java index 474d140..7bfbfa0 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -13,7 +13,7 @@ public class DispenserBase extends SubsystemBase { private static final LoggedTunableNumber intakeVolts = new LoggedTunableNumber("Dispenser/IntakeVolts", 6.0); private static final LoggedTunableNumber ejectVolts = - new LoggedTunableNumber("Dispenser/EjectVolts", 2.0); + new LoggedTunableNumber("Dispenser/EjectVolts", 1.3); private static final LoggedTunableNumber holdingCoralPeriod = new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); @@ -32,7 +32,7 @@ public class DispenserBase extends SubsystemBase { private final Alert disconnected = new Alert("Dispenser motor disconnected!", Alert.AlertType.kWarning); - @Setter private double coralVolts = 0.0; + // @Setter private double coralVolts = 0.0; public DispenserBase(DispenserIO io) { this.io = io; @@ -52,12 +52,12 @@ public void periodic() { // Update if holding coral. hasCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); - // Run Coral - if (!isEStopped) { - io.runVolts(coralVolts); - } else { - io.stop(); - } + // // Run Coral + // if (!isEStopped) { + // io.runVolts(coralVolts); + // } else { + // io.stop(); + // } } public Command intakeTillHolding() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index d46304f..1d970a8 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -15,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.PoseEstimator; import frc.robot.util.LoggedTunableNumber; @@ -29,8 +32,8 @@ public class DriveBase extends SubsystemBase { // Characterization - private static final double FF_START_DELAY = 2.0; // Secs - private static final double FF_RAMP_RATE = 0.85; // Volts/Sec + private static final double FF_START_DELAY = 2; // Secs + private static final double FF_RAMP_RATE = 3.5; // Volts/Sec private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 @@ -73,6 +76,16 @@ public class DriveBase extends SubsystemBase { @AutoLogOutput(key = "Drive/BrakeModeEnabled") private boolean BRAKE_MODE = true; + private final SysIdRoutine sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + public DriveBase( GyroIO gyroIO, ModuleIO flModuleIO, @@ -206,6 +219,8 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsUnoptimized", setpointStatesUnoptimized); Logger.recordOutput("SwerveStates/Setpoints", setpointStates); Logger.recordOutput("SwerveChassisSpeeds/Setpoints", currentSetpoint.chassisSpeeds()); + Logger.recordOutput( + "SwerveChassisSpeeds/SetpointsUnoptimized", currentSetpoint.chassisSpeeds()); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -213,6 +228,8 @@ public void runVelocity(ChassisSpeeds speeds) { } } + // CAN CONNECTOR ON CANID 6 MUST BE REPLACED BEFORE COMP + /** Runs the drive in a straight(ish) line with the specified drive output. */ public void runCharacterization(double output) { CLOSED_LOOP_MODE = false; @@ -419,6 +436,18 @@ public Command wheelRadiusCharacterization() { }))); } + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + private static class WheelRadiusCharacterizationState { double[] positions = new double[4]; Rotation2d lastAngle = new Rotation2d(); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index c51306d..375e3cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -26,12 +26,18 @@ class Module { static { switch (Constants.getRobot()) { case COMPBOT -> { - drivekS.initDefault(0.19700); - drivekV.initDefault(0.12941); - drivekP.initDefault(0.005); - drivekD.initDefault(0.0); - turnkP.initDefault(2.0); - turnkD.initDefault(0.05); + // drivekS.initDefault(0.113190); + // drivekV.initDefault(0.841640); + // drivekP.initDefault(0.01); + // drivekD.initDefault(0); + // turnkP.initDefault(2); + // turnkD.initDefault(0); + drivekS.initDefault(0.0); + drivekV.initDefault(0.0); + drivekP.initDefault(0.0); + drivekD.initDefault(0); + turnkP.initDefault(0); + turnkD.initDefault(0); } default -> { drivekS.initDefault(0.113190); @@ -95,6 +101,8 @@ public void periodic() { /** Runs the module with the specified setpoint state. */ public void runSetpoint(SwerveModuleState state) { + // state.optimize(getAngle()); + // state.cosineScale(getAngle()); m_io.runDriveVelocity(state.speedMetersPerSecond / DriveConstants.wheelRadius); m_io.runTurnPosition(state.angle); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 8225b3e..48334be 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -222,9 +222,9 @@ public void periodic() { "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); // If not homed, schedule that command - if (!homed) { - homingSequence().schedule(); - } + // if (!homed && !homingSequence().isScheduled()) { + // homingSequence().schedule(); + // } measuredVisualizer.update(getPositionMeters()); setpointVisualizer.update(setpoint.position); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index a24d6f1..f10f24c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -15,7 +15,7 @@ public enum ElevatorState { START(() -> 0.0), STOW("Stow", 0.0), INTAKE("Intake", 0.0), - L1_CORAL(ReefLevel.L1, Units.inchesToMeters(0.0)), + L1_CORAL(ReefLevel.L1, Units.inchesToMeters(7)), L2_CORAL(ReefLevel.L2, Units.inchesToMeters(1)), L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0)); From a6ac1fe2c2a0712859b2c78dadd7086bae7401b2 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 10:49:23 -0500 Subject: [PATCH 45/73] fix merge --- src/main/java/frc/robot/FieldConstants.java | 6 --- src/main/java/frc/robot/basement-layout.json | 45 -------------------- 2 files changed, 51 deletions(-) delete mode 100644 src/main/java/frc/robot/basement-layout.json diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 6bc65c4..9a3e7b2 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,11 +1,9 @@ package frc.robot; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import java.io.IOException; -import java.nio.file.Path; import java.util.*; import lombok.Getter; @@ -16,10 +14,6 @@ public class FieldConstants { public static AprilTagFieldLayout fieldLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); - static { - System.out.println(Path.of(AprilTagFields.k2025ReefscapeWelded.toString())); - } - public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldLength(); public static final double fieldWidth = diff --git a/src/main/java/frc/robot/basement-layout.json b/src/main/java/frc/robot/basement-layout.json deleted file mode 100644 index daaf4c1..0000000 --- a/src/main/java/frc/robot/basement-layout.json +++ /dev/null @@ -1,45 +0,0 @@ -{ - "tags": [ - { - "ID": 1, - "pose": { - "translation": { - "x": 16.697198, - "y": 0.65532, - "z": 1.4859 - }, - "rotation": { - "quaternion": { - "W": 0.4539904997395468, - "X": 0.0, - "Y": 0.0, - "Z": 0.8910065241883678 - } - } - } - }, - - { - "ID": 6, - "pose": { - "translation": { - "x": 13.474446, - "y": 3.3063179999999996, - "z": 0.308102 - }, - "rotation": { - "quaternion": { - "W": -0.8660254037844387, - "X": -0.0, - "Y": 0.0, - "Z": 0.49999999999999994 - } - } - } - } - ], - "field": { - "length": 17.548, - "width": 8.052 - } - } \ No newline at end of file From eda4d4bdaa315a1e4c2bb4ad9718232aa2974e22 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 10:50:05 -0500 Subject: [PATCH 46/73] Fix characterization DT bug --- .../frc/robot/subsystems/drive/DriveBase.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 23 ++++++------------- 2 files changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index 1d970a8..459c43d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -291,7 +291,7 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + /** Returns the average velocity of the modules in rad/sec. */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 375e3cf..eeee2a2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -3,7 +3,6 @@ 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 edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import frc.robot.Constants; @@ -26,18 +25,12 @@ class Module { static { switch (Constants.getRobot()) { case COMPBOT -> { - // drivekS.initDefault(0.113190); - // drivekV.initDefault(0.841640); - // drivekP.initDefault(0.01); - // drivekD.initDefault(0); - // turnkP.initDefault(2); - // turnkD.initDefault(0); - drivekS.initDefault(0.0); - drivekV.initDefault(0.0); + drivekS.initDefault(0.69641); + drivekV.initDefault(0.12647); drivekP.initDefault(0.0); - drivekD.initDefault(0); - turnkP.initDefault(0); - turnkD.initDefault(0); + drivekD.initDefault(0.0); + turnkP.initDefault(1.5); + turnkD.initDefault(0.0); } default -> { drivekS.initDefault(0.113190); @@ -101,8 +94,6 @@ public void periodic() { /** Runs the module with the specified setpoint state. */ public void runSetpoint(SwerveModuleState state) { - // state.optimize(getAngle()); - // state.cosineScale(getAngle()); m_io.runDriveVelocity(state.speedMetersPerSecond / DriveConstants.wheelRadius); m_io.runTurnPosition(state.angle); } @@ -149,9 +140,9 @@ public double getWheelRadiusCharacterizationPosition() { return m_inputs.drivePositionRad; } - /** Returns the module velocity in rotations/sec (Phoenix native units). */ + /** Returns the module velocity in rad/sec. */ public double getFFCharacterizationVelocity() { - return Units.radiansToRotations(m_inputs.driveVelocityRadPerSec); + return m_inputs.driveVelocityRadPerSec; } /* Sets brake mode to {@code enabled} */ From 24e9f2b4ccb65aa2641f5b542847f92649a702d5 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 10:50:57 -0500 Subject: [PATCH 47/73] Auto hone elevator on start --- .../java/frc/robot/subsystems/elevator/ElevatorBase.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 48334be..0719f83 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -222,9 +222,9 @@ public void periodic() { "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); // If not homed, schedule that command - // if (!homed && !homingSequence().isScheduled()) { - // homingSequence().schedule(); - // } + if (!homed && !profileDisabled) { + homingSequence().schedule(); + } measuredVisualizer.update(getPositionMeters()); setpointVisualizer.update(setpoint.position); From e9c169485efbf1c1873457f1c69f7b7c3f43d1da Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 10:51:20 -0500 Subject: [PATCH 48/73] idk if this matters --- src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 43870cd..24414cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -89,6 +89,7 @@ public ModuleIOSpark(int index) { driveSpark.configure( driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + driveEncoder.setPosition(0.0); // Configure Turn var turnConfig = new SparkMaxConfig(); From de0014ccd9ec2329c01e64120c62610973842a77 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 12:40:28 -0500 Subject: [PATCH 49/73] Make slow mode toggleable and cleanup changes from Thursday --- src/main/java/frc/robot/RobotContainer.java | 33 +++++-------------- .../frc/robot/commands/DriveCommands.java | 24 +++++++------- 2 files changed, 21 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32fc491..6d66e1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -46,6 +46,8 @@ public class RobotContainer { private final LoggedNetworkNumber endgameAlert2 = new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0); + private boolean slowModeEnabled; + public RobotContainer() { switch (Constants.getMode()) { case REAL -> { @@ -122,34 +124,17 @@ public RobotContainer() { } private void configureButtonBindings() { + // Make slow mode toggleable + controller.y().toggleOnTrue(Commands.runOnce(() -> slowModeEnabled = !slowModeEnabled)); + // Default command, normal field-relative drive driveBase.setDefaultCommand( DriveCommands.joystickDrive( driveBase, - () -> - controller.y().getAsBoolean() - ? -controller.getLeftY() * 0.7 - : -controller.getLeftY(), - () -> - controller.y().getAsBoolean() - ? -controller.getLeftX() * 0.7 - : -controller.getLeftX(), - () -> - controller.y().getAsBoolean() - ? -controller.getRightX() * 0.7 - : -controller.getRightX(), - controller.leftStick())); - - // TODO Lock to Feeder Station when held - // controller - // .y() - // .whileTrue( - // DriveCommands.joystickDriveAtAngle( - // driveBase, - // () -> -controller.getLeftY(), - // () -> -controller.getLeftX(), - // () -> Rotation2d.kZero, - // controller.leftStick())); + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX(), + () -> slowModeEnabled)); // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1bad7eb..e6b2f97 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -20,11 +20,11 @@ public class DriveCommands { private static final double DEADBAND = 0.1; private static final LoggedTunableNumber teleopLinearScalar = - new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 0.5); - private static final LoggedTunableNumber teleopLinearScalarSprint = - new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 1.0); - private static final LoggedTunableNumber angularScalar = - new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 1.0); + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0); + private static final LoggedTunableNumber teleopLinearScalarSlowMode = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 0.5); + private static final LoggedTunableNumber teleopAngularScalar = + new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; @@ -39,7 +39,7 @@ public static Command joystickDrive( DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier, - BooleanSupplier sprintSupplier) { + BooleanSupplier slowSupplier) { return Commands.run( () -> { // Apply deadband @@ -54,10 +54,10 @@ public static Command joystickDrive( // Generate robot relative speeds double linearVelocityScalar = - sprintSupplier.getAsBoolean() - ? teleopLinearScalarSprint.get() + slowSupplier.getAsBoolean() + ? teleopLinearScalarSlowMode.get() : teleopLinearScalar.get(); - double angularVelocityScalar = angularScalar.get(); + double angularVelocityScalar = teleopAngularScalar.get(); var speeds = new ChassisSpeeds( @@ -88,7 +88,7 @@ public static Command joystickDriveAtAngle( DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier rotationSupplier, - BooleanSupplier sprintSupplier) { + BooleanSupplier slowSupplier) { ProfiledPIDController angleController = new ProfiledPIDController( ANGLE_KP, @@ -115,8 +115,8 @@ public static Command joystickDriveAtAngle( // Generate robot relative speeds double linearVelocityScalar = - sprintSupplier.getAsBoolean() - ? teleopLinearScalarSprint.get() + slowSupplier.getAsBoolean() + ? teleopLinearScalarSlowMode.get() : teleopLinearScalar.get(); var speeds = new ChassisSpeeds( From f17c27284704a73ea111ba047bf25cbb7c4eb561 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 12:41:19 -0500 Subject: [PATCH 50/73] Make intake toggleable --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- src/main/java/frc/robot/commands/IntakeCommands.java | 10 +++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6d66e1b..29e6620 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -138,8 +138,6 @@ private void configureButtonBindings() { // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); - // Intake - controller.x().onTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); // L1 controller .povLeft() @@ -150,6 +148,10 @@ private void configureButtonBindings() { controller .povRight() .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL))); + + // Intake + controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); + // Dispense controller.rightTrigger().onTrue(dispenserBase.eject()); diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 2a21a15..15760a5 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -10,14 +10,18 @@ public class IntakeCommands { public static final LoggedTunableNumber intakeVolts = - new LoggedTunableNumber("Intake/HopperIntakeVolts", 6.0); + new LoggedTunableNumber("Intake/HopperIntakeVolts", 5.5); + public static final LoggedTunableNumber intakeTimeout = + new LoggedTunableNumber("Intake/IntakeTimeoutSecs", 5.0); public static Command intake(ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) - .alongWith( + .andThen( Commands.waitUntil(elevator::isAtGoal) .andThen( Commands.deadline( - dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())))); + dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())) + .withTimeout(intakeTimeout.get()))) + .finallyDo(() -> elevator.setGoal(ElevatorState.STOW)); } } From 3d7c324107b5835ec39c64155be03a194fdcd2b6 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 12:41:47 -0500 Subject: [PATCH 51/73] stop elevator from honing on gyro reset --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29e6620..9cb9adf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -156,7 +156,11 @@ private void configureButtonBindings() { controller.rightTrigger().onTrue(dispenserBase.eject()); // Home Elevator - controller.back().debounce(0.5).onTrue(elevatorBase.homingSequence()); + controller + .back() + .and(controller.start().negate()) + .debounce(0.5) + .onTrue(elevatorBase.homingSequence()); // Auto Align (Left or Right) // TODO From 5b7bf78cee0ade7d5a032ee35edad0ce24e4360e Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 12:42:38 -0500 Subject: [PATCH 52/73] Rename holdingCoral to better match lombok --- .../subsystems/dispenser/DispenserBase.java | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java index 7bfbfa0..597a0d8 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -6,7 +6,6 @@ import frc.robot.util.Debouncer; import frc.robot.util.LoggedTunableNumber; import lombok.Getter; -import lombok.Setter; import org.littletonrobotics.junction.Logger; public class DispenserBase extends SubsystemBase { @@ -24,16 +23,13 @@ public class DispenserBase extends SubsystemBase { private final DispenserIO io; private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); - @Setter private boolean isEStopped; - @Getter private boolean hasCoral; + @Getter private boolean holdingCoral; private final Debouncer holdingCoralDebouncer = new Debouncer(0); private final Alert disconnected = new Alert("Dispenser motor disconnected!", Alert.AlertType.kWarning); - // @Setter private double coralVolts = 0.0; - public DispenserBase(DispenserIO io) { this.io = io; } @@ -50,18 +46,11 @@ public void periodic() { } // Update if holding coral. - hasCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); - - // // Run Coral - // if (!isEStopped) { - // io.runVolts(coralVolts); - // } else { - // io.stop(); - // } + holdingCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); } public Command intakeTillHolding() { - return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> hasCoral); + return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> holdingCoral); } public Command eject() { From fd13f3f313e0a6fa053e1f084e78f7a4697b2580 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 12:44:13 -0500 Subject: [PATCH 53/73] update dispenser eject to be faster also goes back to stow automatically --- src/main/java/frc/robot/RobotContainer.java | 8 +++++++- .../frc/robot/subsystems/dispenser/DispenserBase.java | 4 ++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9cb9adf..01c3506 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -153,7 +153,13 @@ private void configureButtonBindings() { controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); // Dispense - controller.rightTrigger().onTrue(dispenserBase.eject()); + controller + .rightTrigger() + .and(controller.leftTrigger().negate()) + .onTrue( + dispenserBase + .eject() + .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW)))); // Home Elevator controller diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java index 597a0d8..5e74580 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -12,13 +12,13 @@ public class DispenserBase extends SubsystemBase { private static final LoggedTunableNumber intakeVolts = new LoggedTunableNumber("Dispenser/IntakeVolts", 6.0); private static final LoggedTunableNumber ejectVolts = - new LoggedTunableNumber("Dispenser/EjectVolts", 1.3); + new LoggedTunableNumber("Dispenser/EjectVolts", 4.0); private static final LoggedTunableNumber holdingCoralPeriod = new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); private static final LoggedTunableNumber ejectPeriod = - new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.65); + new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.35); private final DispenserIO io; private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); From 773704f71a039f1825a2f3807e5faf9dd4e607f9 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 12:44:29 -0500 Subject: [PATCH 54/73] add reserialize command --- src/main/java/frc/robot/RobotContainer.java | 7 +++++++ src/main/java/frc/robot/commands/IntakeCommands.java | 9 +++++++++ .../frc/robot/subsystems/dispenser/DispenserBase.java | 4 ++++ 3 files changed, 20 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01c3506..2fcc6c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -161,6 +161,13 @@ private void configureButtonBindings() { .eject() .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW)))); + // Reserialize + controller + .leftTrigger() + .and(controller.rightTrigger()) + .debounce(0.25) + .onTrue(IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase)); + // Home Elevator controller .back() diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 15760a5..15c6a4f 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -24,4 +24,13 @@ public static Command intake(ElevatorBase elevator, IntakeBase intake, Dispenser .withTimeout(intakeTimeout.get()))) .finallyDo(() -> elevator.setGoal(ElevatorState.STOW)); } + + public static Command reserialize( + ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { + return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) + .andThen( + Commands.waitUntil(elevator::isAtGoal) + .andThen(dispenser.runRollers(-2.0).until(() -> !dispenser.isHoldingCoral())) + .andThen(IntakeCommands.intake(elevator, intake, dispenser))); + } } diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java index 5e74580..eb761d6 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -49,6 +49,10 @@ public void periodic() { holdingCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); } + public Command runRollers(double inputVolts) { + return startEnd(() -> io.runVolts(inputVolts), io::stop); + } + public Command intakeTillHolding() { return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> holdingCoral); } From bafb1a78ef946029dd5de3a5e355630d44eb0c1f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 28 Feb 2025 14:55:34 -0500 Subject: [PATCH 55/73] Add elevator and dispenser. fix drive bugs. commit 773704f71a039f1825a2f3807e5faf9dd4e607f9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:44:29 2025 -0500 add reserialize command commit fd13f3f313e0a6fa053e1f084e78f7a4697b2580 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:44:13 2025 -0500 update dispenser eject to be faster also goes back to stow automatically commit 5b7bf78cee0ade7d5a032ee35edad0ce24e4360e Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:42:38 2025 -0500 Rename holdingCoral to better match lombok commit 3d7c324107b5835ec39c64155be03a194fdcd2b6 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:41:47 2025 -0500 stop elevator from honing on gyro reset commit f17c27284704a73ea111ba047bf25cbb7c4eb561 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:41:19 2025 -0500 Make intake toggleable commit de0014ccd9ec2329c01e64120c62610973842a77 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:40:28 2025 -0500 Make slow mode toggleable and cleanup changes from Thursday commit e9c169485efbf1c1873457f1c69f7b7c3f43d1da Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:51:20 2025 -0500 idk if this matters commit 24e9f2b4ccb65aa2641f5b542847f92649a702d5 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:50:57 2025 -0500 Auto hone elevator on start commit eda4d4bdaa315a1e4c2bb4ad9718232aa2974e22 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:50:05 2025 -0500 Fix characterization DT bug commit a6ac1fe2c2a0712859b2c78dadd7086bae7401b2 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:49:23 2025 -0500 fix merge commit 32aca1e42958e7faccf81f6fc759329ae5b35b0b Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu Feb 27 01:05:11 2025 -0500 tuning stuff commit 1b9467e64e8533dff54591e8612d4cad12a4711d Merge: 07083ba 2e7b277 Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Wed Feb 26 18:55:24 2025 -0500 Merge branch 'sriman-dev' of https://github.com/Talon540Programming/Reefscape2025 into sriman-dev commit 2e7b277a430c9b1cfb39e9c44657eda154c0ca3f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 18:48:56 2025 -0500 Create ElevatorVisualizer.java commit b32b9fc75f2d4b8d5533fadc176c701eec4eff0d Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 18:48:48 2025 -0500 Update RobotContainer.java commit 6cfc8e339566071b0dfe1b55291828f5523012ca Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 14:57:45 2025 -0500 Update LoggedTunableNumber.java commit e7fadb78556a97f651eceaa2934ee1f3684f20f4 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 11:20:15 2025 -0500 schedule home command if not homed on teleop init commit 1d3b19195f0965de2da490d954342cacf353d2dc Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 10:24:53 2025 -0500 add proto auto intake commit d1f8beb647341078b591bf9d92889cd945de9e86 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 10:24:33 2025 -0500 add sprint commit d267a5f5fa61a4fc23719ef03079e075d88fb78c Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 01:54:57 2025 -0500 add elevator and end effector subsystems commit 07083ba1d80b32fe6304f25afe028c50cd5f4395 Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Wed Feb 26 01:36:12 2025 -0500 fixes commit ee57f87138c8c7ace29a1366c6b6c1770a7e8484 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Feb 25 22:03:17 2025 -0500 WIP commit 9f9de79cf2f5d6650b46a4b92adedfda7012d27f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Feb 25 03:29:28 2025 -0500 Update IO commit ba09067684158f1859c2290eb650eb8f567363c2 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:57 2025 -0500 Implement ElevatorIO commit 19e39761c03d84aab7256b660042e199dd804225 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:49 2025 -0500 Implement DispenserIO commit bd348ee7efa5ff7adddc9e6eb5648bf8f1c8448f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:40 2025 -0500 Make sim models non static avoids building creating these objects when running as non-sim commit 1baa58d8dd96d97f443ec8aab682cd5d8d5466bf Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 11:09:27 2025 -0500 Add updatable debouncer commit ba6e2991d808c364586857695d9867a73bdd8b99 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 05:03:14 2025 -0500 Update EqualsUtil.java commit fb125076708266e1c948eca930a01f8695ea8e15 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 23 13:58:29 2025 -0500 Update FieldConstants.java commit 96f6eae34148a6acd41b60d069dcd7a052245bd9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 10:40:15 2025 -0500 dont make conversion factors variables commit 43435348a07e05f7cc8096adecdd54e234ab25e3 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 10:39:38 2025 -0500 explicitly set motor temp signals commit e8fbd8094026f56fd72af4489b30a31a5e2d366c Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 01:28:04 2025 -0500 Update Intake commit 6adf211c5e34a0c35c5a679c734caf39afb0f72e Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 01:05:32 2025 -0500 Fix DT bug commit ca2f600f234c53cbbfd7de39552818c127d4f15f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 00:53:08 2025 -0500 Update robot constants to match bot commit 68e75f96ca0b1cbb53b0039c66e3a05ac3f9bedd Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 19 23:34:57 2025 -0500 Add intake subsystem commit de577e53203b8b230ad9d0794b68207aba6775e7 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 19 21:47:57 2025 -0500 fix akit version mismatch commit 2a1068cc911a360e906143ae3fe51a5c9e20aa4c Merge: 420fa3d 3be2661 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 16 15:45:12 2025 -0500 Merge branch 'main' into sriman-dev commit 420fa3d8329e495d1a9422680ba392a58594e3c8 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 16 15:38:07 2025 -0500 Update WPILIB to 2025.3.1 commit 5b6968a286336017765f9eb07022e52eedd2e6b0 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu Feb 13 01:44:37 2025 -0500 Fix red alliance drive commands bug commit 295299829ca2ac25090cc684e1a29941dba6a40c Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 15 01:43:00 2025 -0500 Shorten name for drive temp commit b2626e8188e48faf889353291b823806ebb62044 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu Feb 13 00:06:04 2025 -0500 Refactor code structure to not over-expose subsystem specific stuff commit 2af259b8051e899a7fd4f0881f469a1e9c042b21 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 23:47:30 2025 -0500 Update Phoenix6 to 2025.2.2 commit 2ae942fab2a6df947bbd95de3acb054f21613be7 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 23:44:07 2025 -0500 Refactor abstract alerts into util class to later roll into LEDs commit 876046fc313910b509223089d7cc4a642dcb343d Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 23:42:36 2025 -0500 Rename RobotState to PoseEstimator Because only vision and drive will interact with pose (and purely position) and no other robot state is tracked, it makes more sense for this to be renammed to reflect that. commit e5c48434c24987d890808075435f263c189dbc6a Merge: f7606cf 9325f68 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:08:31 2025 -0500 Merge branch 'main' into sriman-dev commit f7606cf1df2482dd259d1795aac8e2c650479f3f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:07:08 2025 -0500 Inline tuning mode alert commit addcdcebe1579fa6625bd3a10573b4d6e567c1f4 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:04:53 2025 -0500 Formatting fixes commit 1b089c2e48d60b91746ec9027c54081b24d39cfe Merge: 37cd26f 93e655d Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:03:18 2025 -0500 Merge branch 'main' into sriman-dev commit 37cd26f8f16c0ff68b443e1e34f1b537ac1502c9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:55:27 2025 -0500 Log drive motor temps commit 15e8caf8a87b3117208c2d49c3bfa086356575df Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:43:07 2025 -0500 Update sim characterization constants to be more accurate, stops a massive overshooting for some reason commit 803498a62e7d6a01598a80db76b12c4ea44af728 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:42:46 2025 -0500 Cleanup dashboard setting stuff commit 1cc2430a80c8e3bc42f9ffda1be27114f37ac568 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:42:25 2025 -0500 Cleanup typing, make errors more obvious commit 98c071538f2695fa8bae86db29b2520814843909 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 5 13:21:38 2025 -0500 Fix Modules Falsely Reporting as Disconnected commit f9babf3b43a48e78657bb152028209e1788a6230 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 1 09:57:53 2025 -0500 clean commit 16c4eba5a4e66e6019a69086de7db2b8c3653f43 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Jan 29 12:44:39 2025 -0500 Increase voltage warning on battery and add CAN error alert commit 71368d180fc9aec15e0f869f715eb33a7497bdcf Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Jan 18 04:02:47 2025 -0500 Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI Co-Authored-By: Talon540-root <122660543+Talon540-root@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/FieldConstants.java | 195 ++++++------ src/main/java/frc/robot/RobotContainer.java | 148 +++++++-- .../java/frc/robot/commands/AutoRoutine.java | 3 + .../frc/robot/commands/DriveCommands.java | 32 +- .../java/frc/robot/commands/DriveToPose.java | 50 ++++ .../frc/robot/commands/DriveTrajectory.java | 8 + .../frc/robot/commands/IntakeCommands.java | 36 +++ .../subsystems/dispenser/DispenserBase.java | 63 ++++ .../dispenser/DispenserConstants.java | 7 + .../subsystems/dispenser/DispenserIO.java | 26 ++ .../subsystems/dispenser/DispenserIOSim.java | 40 +++ .../dispenser/DispenserIOSpark.java | 90 ++++++ .../frc/robot/subsystems/drive/DriveBase.java | 35 ++- .../subsystems/drive/DriveConstants.java | 16 +- .../frc/robot/subsystems/drive/Module.java | 15 +- .../robot/subsystems/drive/ModuleIOSim.java | 4 +- .../robot/subsystems/drive/ModuleIOSpark.java | 9 +- .../subsystems/elevator/ElevatorBase.java | 282 ++++++++++++++++++ .../elevator/ElevatorConstants.java | 23 ++ .../robot/subsystems/elevator/ElevatorIO.java | 31 ++ .../subsystems/elevator/ElevatorIOSim.java | 74 +++++ .../subsystems/elevator/ElevatorIOSpark.java | 150 ++++++++++ .../subsystems/elevator/ElevatorState.java | 37 +++ .../robot/subsystems/intake/IntakeBase.java | 30 ++ .../subsystems/intake/IntakeConstants.java | 7 + .../frc/robot/subsystems/intake/IntakeIO.java | 24 ++ .../robot/subsystems/intake/IntakeIOSim.java | 40 +++ .../subsystems/intake/IntakeIOSpark.java | 83 ++++++ src/main/java/frc/robot/util/AlertsUtil.java | 137 ++++++++- src/main/java/frc/robot/util/Debouncer.java | 90 ++++++ src/main/java/frc/robot/util/EqualsUtil.java | 2 +- .../frc/robot/util/LoggedTunableNumber.java | 10 +- .../util/trajectory/DriveTrajectories.java | 10 + vendordeps/AdvantageKit.json | 6 +- 35 files changed, 1644 insertions(+), 171 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoRoutine.java create mode 100644 src/main/java/frc/robot/commands/DriveToPose.java create mode 100644 src/main/java/frc/robot/commands/DriveTrajectory.java create mode 100644 src/main/java/frc/robot/commands/IntakeCommands.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorState.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeBase.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java create mode 100644 src/main/java/frc/robot/util/Debouncer.java create mode 100644 src/main/java/frc/robot/util/trajectory/DriveTrajectories.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7de34c6..2697903 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,8 @@ public class Constants { public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running public static final boolean ENABLE_LOGGING = true; + // Disable LEDs, will reduce software and electrical overhead but disable hardware alerts + public static final boolean ENABLE_LEDs = false; public static final double kLoopPeriodSecs = 0.02; diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index aad1e00..9a3e7b2 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,25 +1,32 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFieldLayout; 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; +import java.io.IOException; +import java.util.*; +import lombok.Getter; /** * 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 AprilTagFieldLayout fieldLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + + public static final double fieldLength = + AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldLength(); + public static final double fieldWidth = + AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldWidth(); 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)); + new Pose2d( + AprilTagLayoutType.OFFICIAL.getFieldLayout().getTagPose(16).get().getX(), + 0, + Rotation2d.fromDegrees(90)); } public static class Barge { @@ -36,73 +43,76 @@ public static class Barge { } 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 double stationLength = Units.inchesToMeters(79.750); public static final Pose2d rightCenterFace = new Pose2d( Units.inchesToMeters(33.526), Units.inchesToMeters(25.824), Rotation2d.fromDegrees(144.011 - 90)); + public static final Pose2d leftCenterFace = + new Pose2d( + rightCenterFace.getX(), + fieldWidth - rightCenterFace.getY(), + Rotation2d.fromRadians(-rightCenterFace.getRotation().getRadians())); + } + + public enum ReefLevel { + L1(Units.inchesToMeters(25.0), 0), + L2(Units.inchesToMeters(31.875 - Math.cos(Math.toRadians(35.0)) * 0.625), -35), + L3(Units.inchesToMeters(47.625 - Math.cos(Math.toRadians(35.0)) * 0.625), -35), + L4(Units.inchesToMeters(72), -90); + + public final double height; + public final double pitch; + + ReefLevel(double height, double pitch) { + this.height = height; + this.pitch = pitch; // Degrees + } + + public static ReefLevel fromLevel(int level) { + return Arrays.stream(values()) + .filter(height -> height.ordinal() == level) + .findFirst() + .orElse(L4); + } } public static class Reef { + public static final double faceLength = Units.inchesToMeters(36.792600); public static final Translation2d center = - new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + new Translation2d(Units.inchesToMeters(176.746), fieldWidth / 2.0); 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 = + public static final List> branchPositions = new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise + public static final List> branchPositions2d = new ArrayList<>(); 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)); + var aprilTagLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + centerFaces[0] = aprilTagLayout.getTagPose(18).get().toPose2d(); + centerFaces[1] = aprilTagLayout.getTagPose(19).get().toPose2d(); + centerFaces[2] = aprilTagLayout.getTagPose(20).get().toPose2d(); + centerFaces[3] = aprilTagLayout.getTagPose(21).get().toPose2d(); + centerFaces[4] = aprilTagLayout.getTagPose(22).get().toPose2d(); + centerFaces[5] = aprilTagLayout.getTagPose(17).get().toPose2d(); // Initialize branch positions for (int face = 0; face < 6; face++) { - Map fillRight = new HashMap<>(); - Map fillLeft = new HashMap<>(); - for (var level : ReefHeight.values()) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + Map fillRight2d = new HashMap<>(); + Map fillLeft2d = new HashMap<>(); + for (var level : ReefLevel.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, + var rightBranchPose = new Pose3d( new Translation3d( poseDirection @@ -115,9 +125,8 @@ public static class Reef { new Rotation3d( 0, Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); - fillLeft.put( - level, + poseDirection.getRotation().getRadians())); + var leftBranchPose = new Pose3d( new Translation3d( poseDirection @@ -130,74 +139,44 @@ public static class Reef { new Rotation3d( 0, Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); + poseDirection.getRotation().getRadians())); + + fillRight.put(level, rightBranchPose); + fillLeft.put(level, leftBranchPose); + fillRight2d.put(level, rightBranchPose.toPose2d()); + fillLeft2d.put(level, leftBranchPose.toPose2d()); } - branchPositions.add((face * 2) + 1, fillRight); - branchPositions.add((face * 2) + 2, fillLeft); + branchPositions.add(fillRight); + branchPositions.add(fillLeft); + branchPositions2d.add(fillRight2d); + branchPositions2d.add(fillLeft2d); } } } 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 double separation = Units.inchesToMeters(72.0); public static final Pose2d middleIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); + new Pose2d(Units.inchesToMeters(48), fieldWidth / 2.0, new Rotation2d()); + public static final Pose2d leftIceCream = + new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() + separation, new Rotation2d()); public static final Pose2d rightIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); + new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() - separation, 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); + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2025-reefscape-welded.json"); - ReefHeight(double height, double pitch) { - this.height = height; - this.pitch = pitch; // in degrees - } + private final AprilTagFieldLayout fieldLayout; - public final double height; - public final double pitch; + private AprilTagLayoutType(String file) { + try { + fieldLayout = AprilTagFieldLayout.loadFromResource(file); + } catch (IOException exception) { + throw new RuntimeException("Failed to load AprilTagLayoutType: " + file, exception); + } + } } - - // 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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 480a909..2fcc6c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,14 +2,28 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.commands.IntakeCommands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.dispenser.DispenserIO; +import frc.robot.subsystems.dispenser.DispenserIOSim; +import frc.robot.subsystems.dispenser.DispenserIOSpark; import frc.robot.subsystems.drive.*; -import frc.robot.util.AlertsUtil; +import frc.robot.subsystems.elevator.*; +import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.intake.IntakeIOSpark; import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class RobotContainer { // Load PoseEstimator class @@ -17,6 +31,9 @@ public class RobotContainer { // Subsystems private final DriveBase driveBase; + private final IntakeBase intakeBase; + private final ElevatorBase elevatorBase; + private final DispenserBase dispenserBase; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -24,6 +41,13 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + private final LoggedNetworkNumber endgameAlert1 = + new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #1", 30.0); + private final LoggedNetworkNumber endgameAlert2 = + new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0); + + private boolean slowModeEnabled; + public RobotContainer() { switch (Constants.getMode()) { case REAL -> { @@ -34,6 +58,9 @@ public RobotContainer() { new ModuleIOSpark(1), new ModuleIOSpark(2), new ModuleIOSpark(3)); + intakeBase = new IntakeBase(new IntakeIOSpark()); + elevatorBase = new ElevatorBase(new ElevatorIOSpark()); + dispenserBase = new DispenserBase(new DispenserIOSpark()); } case SIM -> { driveBase = @@ -43,6 +70,9 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + intakeBase = new IntakeBase(new IntakeIOSim()); + elevatorBase = new ElevatorBase(new ElevatorIOSim()); + dispenserBase = new DispenserBase(new DispenserIOSim()); } default -> { driveBase = @@ -52,6 +82,9 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + intakeBase = new IntakeBase(new IntakeIO() {}); + elevatorBase = new ElevatorBase(new ElevatorIO() {}); + dispenserBase = new DispenserBase(new DispenserIO() {}); } } @@ -64,37 +97,95 @@ public RobotContainer() { "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); autoChooser.addOption( "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); - } + autoChooser.addOption( + "Elevator Dynamic Forward", + elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kForward, 0.5)); + autoChooser.addOption( + "Elevator Dynamic Reverse", + elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kReverse, 0.25)); + autoChooser.addOption( + "Elevator Quasi Forward", + elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward, 25)); + autoChooser.addOption( + "Elevator Quasi Reverse", + elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse, 3)); + + autoChooser.addOption( + "Drive Dynamic Forward", driveBase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Dynamic Reverse", driveBase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive Quasi Forward", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Quasi Reverse", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } configureButtonBindings(); } private void configureButtonBindings() { + // Make slow mode toggleable + controller.y().toggleOnTrue(Commands.runOnce(() -> slowModeEnabled = !slowModeEnabled)); + // Default command, normal field-relative drive driveBase.setDefaultCommand( DriveCommands.joystickDrive( driveBase, () -> -controller.getLeftY(), () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -controller.getRightX(), + () -> slowModeEnabled)); - // Lock to 0° when A button is held + // Stow + controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); + // L1 controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - driveBase, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> Rotation2d.kZero)); - - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); - - // Reset gyro to 0° when B button is pressed + .povLeft() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); + // L2 + controller.povUp().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL))); + // L3 controller - .b() + .povRight() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL))); + + // Intake + controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); + + // Dispense + controller + .rightTrigger() + .and(controller.leftTrigger().negate()) + .onTrue( + dispenserBase + .eject() + .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW)))); + + // Reserialize + controller + .leftTrigger() + .and(controller.rightTrigger()) + .debounce(0.25) + .onTrue(IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase)); + + // Home Elevator + controller + .back() + .and(controller.start().negate()) + .debounce(0.5) + .onTrue(elevatorBase.homingSequence()); + + // Auto Align (Left or Right) + // TODO + + // Human Player Alert (Strobe LEDs) + // TODO + + // Reset Gyro + controller + .start() + .and(controller.back()) + .debounce(0.5) .onTrue( Commands.runOnce( () -> @@ -105,6 +196,29 @@ private void configureButtonBindings() { AllianceFlipUtil.apply(new Rotation2d()))), driveBase) .ignoringDisable(true)); + + // Endgame + new Trigger( + () -> + DriverStation.isTeleopEnabled() + && DriverStation.getMatchTime() > 0 + && DriverStation.getMatchTime() <= Math.round(endgameAlert1.get())) + .onTrue( + Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0)) + .withTimeout(0.5)); + + new Trigger( + () -> + DriverStation.isTeleopEnabled() + && DriverStation.getMatchTime() > 0 + && DriverStation.getMatchTime() <= Math.round(endgameAlert2.get())) + .onTrue( + Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0)) + .withTimeout(0.5)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AutoRoutine.java b/src/main/java/frc/robot/commands/AutoRoutine.java new file mode 100644 index 0000000..f515ed2 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoRoutine.java @@ -0,0 +1,3 @@ +package frc.robot.commands; + +public class AutoRoutine {} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 5588836..e6b2f97 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -10,18 +10,21 @@ import frc.robot.PoseEstimator; import frc.robot.subsystems.drive.DriveBase; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class DriveCommands { // Drive private static final double DEADBAND = 0.1; - private static final LoggedNetworkNumber LINEAR_VELOCITY_SCALAR = - new LoggedNetworkNumber("TeleopDrive/LinearVelocityScalar", 1.0); - private static final LoggedNetworkNumber ANGULAR_VELOCITY_SCALAR = - new LoggedNetworkNumber("TeleopDrive/AngularVelocityScalar", 0.7); + private static final LoggedTunableNumber teleopLinearScalar = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0); + private static final LoggedTunableNumber teleopLinearScalarSlowMode = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 0.5); + private static final LoggedTunableNumber teleopAngularScalar = + new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; @@ -35,7 +38,8 @@ public static Command joystickDrive( DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { + DoubleSupplier omegaSupplier, + BooleanSupplier slowSupplier) { return Commands.run( () -> { // Apply deadband @@ -49,8 +53,11 @@ public static Command joystickDrive( omega = Math.copySign(Math.pow(omega, 2), omega); // Generate robot relative speeds - double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); - double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get(); + double linearVelocityScalar = + slowSupplier.getAsBoolean() + ? teleopLinearScalarSlowMode.get() + : teleopLinearScalar.get(); + double angularVelocityScalar = teleopAngularScalar.get(); var speeds = new ChassisSpeeds( @@ -64,7 +71,6 @@ public static Command joystickDrive( rotation = rotation.rotateBy(Rotation2d.kPi); } speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds driveBase.runVelocity(speeds); @@ -81,7 +87,8 @@ public static Command joystickDriveAtAngle( DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - Supplier rotationSupplier) { + Supplier rotationSupplier, + BooleanSupplier slowSupplier) { ProfiledPIDController angleController = new ProfiledPIDController( ANGLE_KP, @@ -107,7 +114,10 @@ public static Command joystickDriveAtAngle( rotation.getRadians(), rotationSupplier.get().getRadians()); // Generate robot relative speeds - double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); + double linearVelocityScalar = + slowSupplier.getAsBoolean() + ? teleopLinearScalarSlowMode.get() + : teleopLinearScalar.get(); var speeds = new ChassisSpeeds( x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 0000000..8914e2f --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,50 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveBase; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.Supplier; + +public class DriveToPose extends Command { + private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("DriveToPose/DrivekP"); + private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("DriveToPose/DrivekD"); + private static final LoggedTunableNumber thetakP = new LoggedTunableNumber("DriveToPose/ThetakP"); + private static final LoggedTunableNumber thetakD = new LoggedTunableNumber("DriveToPose/ThetakD"); + private static final LoggedTunableNumber driveMaxVelocity = + new LoggedTunableNumber("DriveToPose/DriveMaxVelocity"); + private static final LoggedTunableNumber driveMaxVelocitySlow = + new LoggedTunableNumber("DriveToPose/DriveMaxVelocitySlow"); + private static final LoggedTunableNumber driveMaxAcceleration = + new LoggedTunableNumber("DriveToPose/DriveMaxAcceleration"); + private static final LoggedTunableNumber thetaMaxVelocity = + new LoggedTunableNumber("DriveToPose/ThetaMaxVelocity"); + private static final LoggedTunableNumber thetaMaxAcceleration = + new LoggedTunableNumber("DriveToPose/ThetaMaxAcceleration"); + private static final LoggedTunableNumber driveTolerance = + new LoggedTunableNumber("DriveToPose/DriveTolerance"); + private static final LoggedTunableNumber thetaTolerance = + new LoggedTunableNumber("DriveToPose/ThetaTolerance"); + private static final LoggedTunableNumber ffMinRadius = + new LoggedTunableNumber("DriveToPose/FFMinRadius"); + private static final LoggedTunableNumber ffMaxRadius = + new LoggedTunableNumber("DriveToPose/FFMaxRadius"); + + static { + drivekP.initDefault(0.75); + drivekD.initDefault(0.0); + thetakP.initDefault(4.0); + thetakD.initDefault(0.0); + driveMaxVelocity.initDefault(3.8); + driveMaxAcceleration.initDefault(3.0); + thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0)); + thetaMaxAcceleration.initDefault(8.0); + driveTolerance.initDefault(0.01); + thetaTolerance.initDefault(Units.degreesToRadians(1.0)); + ffMinRadius.initDefault(0.1); + ffMaxRadius.initDefault(0.15); + } + + public DriveToPose(DriveBase drive, Supplier target) {} +} diff --git a/src/main/java/frc/robot/commands/DriveTrajectory.java b/src/main/java/frc/robot/commands/DriveTrajectory.java new file mode 100644 index 0000000..11a161c --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveTrajectory.java @@ -0,0 +1,8 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveBase; + +public class DriveTrajectory extends Command { + public DriveTrajectory(DriveBase drive) {} +} diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..15c6a4f --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.elevator.ElevatorBase; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.util.LoggedTunableNumber; + +public class IntakeCommands { + public static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Intake/HopperIntakeVolts", 5.5); + public static final LoggedTunableNumber intakeTimeout = + new LoggedTunableNumber("Intake/IntakeTimeoutSecs", 5.0); + + public static Command intake(ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { + return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) + .andThen( + Commands.waitUntil(elevator::isAtGoal) + .andThen( + Commands.deadline( + dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())) + .withTimeout(intakeTimeout.get()))) + .finallyDo(() -> elevator.setGoal(ElevatorState.STOW)); + } + + public static Command reserialize( + ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { + return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) + .andThen( + Commands.waitUntil(elevator::isAtGoal) + .andThen(dispenser.runRollers(-2.0).until(() -> !dispenser.isHoldingCoral())) + .andThen(IntakeCommands.intake(elevator, intake, dispenser))); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java new file mode 100644 index 0000000..eb761d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.dispenser; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.Debouncer; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import org.littletonrobotics.junction.Logger; + +public class DispenserBase extends SubsystemBase { + private static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Dispenser/IntakeVolts", 6.0); + private static final LoggedTunableNumber ejectVolts = + new LoggedTunableNumber("Dispenser/EjectVolts", 4.0); + + private static final LoggedTunableNumber holdingCoralPeriod = + new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); + + private static final LoggedTunableNumber ejectPeriod = + new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.35); + + private final DispenserIO io; + private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); + + @Getter private boolean holdingCoral; + + private final Debouncer holdingCoralDebouncer = new Debouncer(0); + + private final Alert disconnected = + new Alert("Dispenser motor disconnected!", Alert.AlertType.kWarning); + + public DispenserBase(DispenserIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Dispenser", inputs); + + disconnected.set(!inputs.connected); + + if (holdingCoralPeriod.hasChanged()) { + holdingCoralDebouncer.setDebounceTime(holdingCoralPeriod.get()); + } + + // Update if holding coral. + holdingCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); + } + + public Command runRollers(double inputVolts) { + return startEnd(() -> io.runVolts(inputVolts), io::stop); + } + + public Command intakeTillHolding() { + return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> holdingCoral); + } + + public Command eject() { + return startEnd(() -> io.runVolts(ejectVolts.get()), io::stop).withTimeout(ejectPeriod.get()); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java new file mode 100644 index 0000000..df11973 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.dispenser; + +class DispenserConstants { + public static final boolean inverted = true; + public static final double moi = 0.025; // TODO + public static final double gearing = 34.0 / 24.0; +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java new file mode 100644 index 0000000..ff8b74a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.dispenser; + +import org.littletonrobotics.junction.AutoLog; + +public interface DispenserIO { + @AutoLog + class DispenserIOInputs { + public boolean connected = false; + + public double positionRads; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + + public boolean rearBeamBreakBroken = false; + } + + default void updateInputs(DispenserIOInputs inputs) {} + + default void runVolts(double output) {} + + default void stop() {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java new file mode 100644 index 0000000..efcf3a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.dispenser; + +import static frc.robot.subsystems.dispenser.DispenserConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class DispenserIOSim implements DispenserIO { + private final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); + + private double appliedVoltage = 0.0; + + @Override + public void updateInputs(DispenserIOInputs inputs) { + sim.update(Constants.kLoopPeriodSecs); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java new file mode 100644 index 0000000..8a689b6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.dispenser; + +import static frc.robot.subsystems.dispenser.DispenserConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.util.Debouncer; + +public class DispenserIOSpark implements DispenserIO { + private final SparkBase spark; + private final RelativeEncoder encoder; + + private final Debouncer connectedDebouncer = new Debouncer(.5); + + // End Dispenser beam break + private final DigitalInput rearBeamBreak = new DigitalInput(0); + + public DispenserIOSpark() { + spark = new SparkMax(14, MotorType.kBrushless); + encoder = spark.getEncoder(); + + var config = new SparkMaxConfig(); + config + .inverted(inverted) + .idleMode(SparkBaseConfig.IdleMode.kBrake) + .smartCurrentLimit(40, 50) + .voltageCompensation(12.0); + + config + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + config + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + spark.configure( + config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(DispenserIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadsPerSec = encoder.getVelocity(); + inputs.appliedVoltage = spark.getAppliedOutput() * spark.getBusVoltage(); + inputs.currentAmps = spark.getOutputCurrent(); + inputs.tempCelsius = spark.getMotorTemperature(); + + inputs.rearBeamBreakBroken = !rearBeamBreak.get(); + + inputs.connected = connectedDebouncer.calculate(!spark.hasActiveFault()); + } + + @Override + public void runVolts(double output) { + spark.setVoltage(output); + } + + @Override + public void stop() { + spark.stopMotor(); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode( + enabled ? SparkBaseConfig.IdleMode.kBrake : SparkBaseConfig.IdleMode.kCoast); + + spark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index d46304f..459c43d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -15,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.PoseEstimator; import frc.robot.util.LoggedTunableNumber; @@ -29,8 +32,8 @@ public class DriveBase extends SubsystemBase { // Characterization - private static final double FF_START_DELAY = 2.0; // Secs - private static final double FF_RAMP_RATE = 0.85; // Volts/Sec + private static final double FF_START_DELAY = 2; // Secs + private static final double FF_RAMP_RATE = 3.5; // Volts/Sec private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 @@ -73,6 +76,16 @@ public class DriveBase extends SubsystemBase { @AutoLogOutput(key = "Drive/BrakeModeEnabled") private boolean BRAKE_MODE = true; + private final SysIdRoutine sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + public DriveBase( GyroIO gyroIO, ModuleIO flModuleIO, @@ -206,6 +219,8 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsUnoptimized", setpointStatesUnoptimized); Logger.recordOutput("SwerveStates/Setpoints", setpointStates); Logger.recordOutput("SwerveChassisSpeeds/Setpoints", currentSetpoint.chassisSpeeds()); + Logger.recordOutput( + "SwerveChassisSpeeds/SetpointsUnoptimized", currentSetpoint.chassisSpeeds()); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -213,6 +228,8 @@ public void runVelocity(ChassisSpeeds speeds) { } } + // CAN CONNECTOR ON CANID 6 MUST BE REPLACED BEFORE COMP + /** Runs the drive in a straight(ish) line with the specified drive output. */ public void runCharacterization(double output) { CLOSED_LOOP_MODE = false; @@ -274,7 +291,7 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + /** Returns the average velocity of the modules in rad/sec. */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { @@ -419,6 +436,18 @@ public Command wheelRadiusCharacterization() { }))); } + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + private static class WheelRadiusCharacterizationState { double[] positions = new double[4]; Rotation2d lastAngle = new Rotation2d(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4222ed9..0f12398 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -38,8 +38,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(2) .driveMotorId(3) - .encoderChannel(2) - .encoderOffset(Rotation2d.fromRadians(0.16028737150729522)) + .encoderChannel(0) + .encoderOffset(Rotation2d.fromRadians(1.7182357115138978).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -48,8 +48,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(4) .driveMotorId(5) - .encoderChannel(3) - .encoderOffset(Rotation2d.fromRadians(-0.1422097592800296)) + .encoderChannel(1) + .encoderOffset(Rotation2d.fromRadians(-1.4361935561244243).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -58,8 +58,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(6) .driveMotorId(7) - .encoderChannel(1) - .encoderOffset(Rotation2d.fromRadians(-3.009554996093968)) + .encoderChannel(2) + .encoderOffset(Rotation2d.fromRadians(0.9998617084472845).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -68,8 +68,8 @@ class DriveConstants { ModuleConfig.builder() .turnMotorId(8) .driveMotorId(9) - .encoderChannel(0) - .encoderOffset(Rotation2d.fromRadians(2.559973505647124)) + .encoderChannel(3) + .encoderOffset(Rotation2d.fromRadians(-1.7285578862473199).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index c51306d..eeee2a2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -3,7 +3,6 @@ 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 edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import frc.robot.Constants; @@ -26,12 +25,12 @@ class Module { static { switch (Constants.getRobot()) { case COMPBOT -> { - drivekS.initDefault(0.19700); - drivekV.initDefault(0.12941); - drivekP.initDefault(0.005); + drivekS.initDefault(0.69641); + drivekV.initDefault(0.12647); + drivekP.initDefault(0.0); drivekD.initDefault(0.0); - turnkP.initDefault(2.0); - turnkD.initDefault(0.05); + turnkP.initDefault(1.5); + turnkD.initDefault(0.0); } default -> { drivekS.initDefault(0.113190); @@ -141,9 +140,9 @@ public double getWheelRadiusCharacterizationPosition() { return m_inputs.drivePositionRad; } - /** Returns the module velocity in rotations/sec (Phoenix native units). */ + /** Returns the module velocity in rad/sec. */ public double getFFCharacterizationVelocity() { - return Units.radiansToRotations(m_inputs.driveVelocityRadPerSec); + return m_inputs.driveVelocityRadPerSec; } /* Sets brake mode to {@code enabled} */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 2b670b3..d40eab4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -11,8 +11,8 @@ import java.util.Queue; public class ModuleIOSim implements ModuleIO { - private static final DCMotor driveMotorModel = DCMotor.getNEO(1); - private static final DCMotor turnMotorModel = DCMotor.getNEO(1); + private final DCMotor driveMotorModel = DCMotor.getNEO(1); + private final DCMotor turnMotorModel = DCMotor.getNEO(1); private final DCMotorSim driveSim = new DCMotorSim( diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 9b7bd76..24414cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -16,10 +16,10 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.AnalogEncoder; import frc.robot.Constants; +import frc.robot.util.Debouncer; import java.util.Queue; public class ModuleIOSpark implements ModuleIO { @@ -84,10 +84,12 @@ public ModuleIOSpark(int index) { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); driveSpark.configure( driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + driveEncoder.setPosition(0.0); // Configure Turn var turnConfig = new SparkMaxConfig(); @@ -116,7 +118,8 @@ public ModuleIOSpark(int index) { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); turnSpark.configure(turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java new file mode 100644 index 0000000..5f0eddc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -0,0 +1,282 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.util.Debouncer; +import frc.robot.util.EqualsUtil; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class ElevatorBase extends SubsystemBase { + // Tunable numbers + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD"); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); + private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/kA"); + + private static final LoggedTunableNumber maxVelocityMetersPerSec = + new LoggedTunableNumber("Elevator/MaxVelocityMetersPerSec", 2.0); + private static final LoggedTunableNumber maxAccelerationMetersPerSec2 = + new LoggedTunableNumber("Elevator/MaxAccelerationMetersPerSec2", 10); + + private static final LoggedTunableNumber homingVolts = + new LoggedTunableNumber("Elevator/HomingVolts", -2.0); + private static final LoggedTunableNumber homingTimeSecs = + new LoggedTunableNumber("Elevator/HomingTimeSecs", 0.25); + private static final LoggedTunableNumber homingVelocityThresh = + new LoggedTunableNumber("Elevator/HomingVelocityThresh", 5.0); + + private static final LoggedTunableNumber toleranceMeters = + new LoggedTunableNumber("Elevator/ToleranceMeters", 0.2); + + static { + switch (Constants.getRobot()) { + case COMPBOT -> { + kP.initDefault(0.3); + kD.initDefault(0.25); + kS.initDefault(0); + kG.initDefault(1.05); + kA.initDefault(0.0); + } + case SIMBOT -> { + kP.initDefault(0); // TODO + kD.initDefault(0); // TODO + kS.initDefault(0); // TODO + kG.initDefault(0); // TODO + kA.initDefault(0); // TODO + } + } + } + + private final ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + private final Alert motorDisconnectedAlert = + new Alert("Elevator leader motor disconnected!", Alert.AlertType.kWarning); + private final Alert followerDisconnectedAlert = + new Alert("Elevator follower motor disconnected!", Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Goal") + @Getter + @Setter + private ElevatorState goal = ElevatorState.START; + + private TrapezoidProfile profile; + private State setpoint = new State(); + + private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.0, 0.0); + + private boolean profileDisabled = false; + @Setter private boolean eStopped = false; + + @AutoLogOutput(key = "Elevator/Homed") + @Getter + private boolean homed = false; + + private final Debouncer toleranceDebouncer = new Debouncer(0.25, Debouncer.DebounceType.kRising); + private final Alert outOfTolleranceAlert = + new Alert( + "Elevator emergency disabled due to high position error. Rehome the elevator to reset.", + Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Profile/AtGoal") + @Getter + private boolean atGoal = false; + + private final SysIdRoutine sysId; + + public ElevatorBase(ElevatorIO io) { + this.io = io; + + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(.1).per(Second), + Volts.of(4), + Seconds.of( + 45), // Effectively disable the timeout and allow the Command factories to set + // them + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> io.runOpenLoop(voltage.in(Volts)), + null, // No log consumer, since data is recorded by AdvantageKit + this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + + motorDisconnectedAlert.set(!inputs.leaderConnected); + followerDisconnectedAlert.set(!inputs.followerConnected); + + // Update tunable numbers + if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { + io.setPID(kP.get(), 0.0, kD.get()); + } + if (kS.hasChanged(hashCode()) || kG.hasChanged(hashCode()) || kA.hasChanged(hashCode())) { + feedforward.setKs(kS.get()); + feedforward.setKg(kG.get()); + feedforward.setKa(kA.get()); + } + + if (maxVelocityMetersPerSec.hasChanged(hashCode()) + || maxAccelerationMetersPerSec2.hasChanged(hashCode())) { + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + } + + // Run profile + final boolean shouldRunProfile = + !profileDisabled && homed && !eStopped && DriverStation.isEnabled(); + Logger.recordOutput("Elevator/RunningProfile", shouldRunProfile); + + // // Check if out of tolerance + // boolean outOfTolerance = + // Math.abs(getPositionMeters() - setpoint.position) > toleranceMeters.get(); + // boolean shouldEStop = toleranceDebouncer.calculate(outOfTolerance && shouldRunProfile); + // + // outOfTolleranceAlert.set(shouldEStop); + // if (shouldEStop) { + // eStopped = true; + // } + + if (shouldRunProfile) { + // Clamp goal + var goalState = + new State( + MathUtil.clamp(goal.getElevatorHeightMeters().getAsDouble(), 0.0, maxTravel), 0); + + double previousVelocity = setpoint.velocity; + setpoint = profile.calculate(Constants.kLoopPeriodSecs, setpoint, goalState); + if (setpoint.position < 0.0 || setpoint.position > maxTravel) { + setpoint = new State(MathUtil.clamp(setpoint.position, 0.0, maxTravel), 0.0); + } + + io.runPosition( + setpoint.position / drumRadius / numStages, + feedforward.calculateWithVelocities(setpoint.velocity, previousVelocity)); + + // Check at goal + atGoal = + EqualsUtil.epsilonEquals(setpoint.position, goalState.position) + && EqualsUtil.epsilonEquals(setpoint.velocity, goalState.velocity); + + // Stop if at the bottom + if (atGoal && EqualsUtil.epsilonEquals(setpoint.position, 0.0)) { + io.stop(); + } + + // Log state + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", setpoint.position); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", setpoint.velocity); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", goalState.position); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", goalState.velocity); + } else { + if (DriverStation.isDisabled()) { + goal = ElevatorState.STOW; + } + + // Reset setpoint + setpoint = new State(getPositionMeters(), 0.0); + + // Clear logs + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", 0.0); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", 0.0); + } + + if (eStopped) { + io.stop(); + } + + Logger.recordOutput( + "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); + + // If not homed, schedule that command + if (!homed && !profileDisabled) { + homingSequence().schedule(); + } + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.quasistatic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.dynamic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + public Command homingSequence() { + var homingDebouncer = new Debouncer(homingTimeSecs.get()); + return Commands.startRun( + () -> { + profileDisabled = true; + homed = false; + homingDebouncer.calculate(false); + }, + () -> { + io.runOpenLoop(homingVolts.get()); + homed = + homingDebouncer.calculate( + Math.abs(inputs.velocityRadPerSec) <= homingVelocityThresh.get()); + }) + .until(() -> homed) + .andThen( + () -> { + io.resetOrigin(); + homed = true; + }) + .finallyDo(() -> profileDisabled = false); + } + + @AutoLogOutput(key = "Elevator/MeasuredHeightMeters") + public double getPositionMeters() { + return inputs.positionRads * drumRadius * numStages; + } + + public double getGoalMeters() { + return goal.getElevatorHeightMeters().getAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..85c509a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class ElevatorConstants { + // Pitch from Floor to Elevator + public static final Rotation2d elevatorPitch = Rotation2d.fromDegrees(84.5); + // Pitch from Elevator to Dispenser + public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(73.0); + + public static final double originToBaseHeightMeters = Units.inchesToMeters(5.149922); + + public static final double drumRadius = Units.inchesToMeters(1.751 / 2.0); + public static final double gearing = 3.0; + public static final int numStages = 2; + + // public static final double maxTravel = Units.inchesToMeters(42.244094); // TODO + public static final double maxTravel = Units.inchesToMeters(42); // TODO + + public static final double carriageMassKg = Units.lbsToKilograms(6.0); + public static final double stagesMassKg = Units.lbsToKilograms(12.0); +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..aceb856 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + public class ElevatorIOInputs { + public boolean leaderConnected = false; + public boolean followerConnected = false; + + public double positionRads = 0.0; + public double velocityRadPerSec = 0.0; + public double[] appliedVolts = new double[] {}; + public double[] currentAmps = new double[] {}; + public double[] tempCelsius = new double[] {}; + } + + default void updateInputs(ElevatorIOInputs inputs) {} + + default void runOpenLoop(double output) {} + + default void runPosition(double positionRads, double feedforwardVolts) {} + + default void stop() {} + + default void resetOrigin() {} + + default void setPID(double kP, double kI, double kD) {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java new file mode 100644 index 0000000..e84b6a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import frc.robot.Constants; + +public class ElevatorIOSim implements ElevatorIO { + private final DCMotor elevatorMotorModel = DCMotor.getNEO(2); + private final ElevatorSim sim = + new ElevatorSim( + elevatorMotorModel, + gearing, + carriageMassKg + stagesMassKg, + drumRadius, + 0, + maxTravel, + true, + 0, + 0.01, + 0.0); + + private final PIDController controller = new PIDController(0, 0, 0); + + private double appliedVolts = 0.0; + private boolean closedLoop = false; + private double feedforward = 0.0; + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + if (!closedLoop) { + controller.reset(); + } else { + appliedVolts = controller.calculate(sim.getPositionMeters()) + feedforward; + } + + sim.setInputVoltage(appliedVolts); + sim.update(Constants.kLoopPeriodSecs); + + inputs.leaderConnected = true; + inputs.followerConnected = true; + + inputs.positionRads = sim.getPositionMeters() / drumRadius; + inputs.velocityRadPerSec = sim.getVelocityMetersPerSecond() / drumRadius; + + inputs.appliedVolts = new double[] {appliedVolts}; + inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + } + + @Override + public void runOpenLoop(double output) { + closedLoop = false; + appliedVolts = output; + } + + @Override + public void runPosition(double positionRads, double feedforwardVolts) { + closedLoop = true; + controller.setSetpoint(positionRads); + feedforward = feedforwardVolts; + } + + @Override + public void stop() { + runOpenLoop(0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java new file mode 100644 index 0000000..f028ec0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -0,0 +1,150 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.util.Debouncer; + +public class ElevatorIOSpark implements ElevatorIO { + private final SparkBase leaderSpark; + private final SparkBase followerSpark; + + private final RelativeEncoder encoder; + + private final SparkClosedLoopController controller; + + private final Debouncer leaderConnectedDebouncer = new Debouncer(0.5); + private final Debouncer followerConnectedDebouncer = new Debouncer(0.5); + + public ElevatorIOSpark() { + leaderSpark = new SparkMax(12, MotorType.kBrushless); + followerSpark = new SparkMax(13, MotorType.kBrushless); + + encoder = leaderSpark.getEncoder(); + controller = leaderSpark.getClosedLoopController(); + + var leaderConfig = new SparkMaxConfig(); + leaderConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(60).voltageCompensation(12.0); + leaderConfig + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + leaderConfig + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) + .pidf(0.0, 0.0, 0.0, 0.0); + leaderConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + leaderSpark.configure( + leaderConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + + var followerConfig = new SparkMaxConfig(); + followerConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(60) + .voltageCompensation(12.0) + .follow(leaderSpark, true); + + followerConfig + .signals + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + followerSpark.configure( + followerConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadPerSec = encoder.getVelocity(); + + inputs.appliedVolts = + new double[] { + leaderSpark.getAppliedOutput() * leaderSpark.getBusVoltage(), + followerSpark.getAppliedOutput() * followerSpark.getBusVoltage() + }; + inputs.currentAmps = + new double[] {leaderSpark.getOutputCurrent(), followerSpark.getOutputCurrent()}; + inputs.tempCelsius = + new double[] {leaderSpark.getMotorTemperature(), followerSpark.getMotorTemperature()}; + + inputs.leaderConnected = leaderConnectedDebouncer.calculate(!leaderSpark.hasActiveFault()); + inputs.followerConnected = + followerConnectedDebouncer.calculate(!followerSpark.hasActiveFault()); + } + + @Override + public void runOpenLoop(double output) { + leaderSpark.setVoltage(output); + } + + @Override + public void runPosition(double positionRads, double feedforwardVolts) { + controller.setReference( + positionRads, + SparkBase.ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedforwardVolts, + SparkClosedLoopController.ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + leaderSpark.stopMotor(); + } + + @Override + public void resetOrigin() { + encoder.setPosition(0.0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + var PIDConfig = new SparkMaxConfig(); + PIDConfig.closedLoop.pid(kP, kI, kD); + + leaderSpark.configure( + PIDConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); + + leaderSpark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + followerSpark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java new file mode 100644 index 0000000..f10f24c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.util.Units; +import frc.robot.FieldConstants.ReefLevel; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +@Getter +@RequiredArgsConstructor +public enum ElevatorState { + START(() -> 0.0), + STOW("Stow", 0.0), + INTAKE("Intake", 0.0), + L1_CORAL(ReefLevel.L1, Units.inchesToMeters(7)), + L2_CORAL(ReefLevel.L2, Units.inchesToMeters(1)), + L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0)); + + private final DoubleSupplier elevatorHeightMeters; + + ElevatorState(String name, double defaultValue) { + elevatorHeightMeters = new LoggedTunableNumber("Elevator/Presets/" + name, defaultValue); + } + + ElevatorState(ReefLevel reefLevel, double defaultOffset) { + var offsetTunable = + new LoggedTunableNumber( + String.format("Elevator/Presets/%s Offset", reefLevel), defaultOffset); + elevatorHeightMeters = + () -> + (reefLevel.height + offsetTunable.get() - originToBaseHeightMeters) + / elevatorPitch.getSin(); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeBase.java b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java new file mode 100644 index 0000000..dcca0a9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class IntakeBase extends SubsystemBase { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + private final Alert disconnected = + new Alert("Intake motor disconnected!", Alert.AlertType.kWarning); + + public IntakeBase(IntakeIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + disconnected.set(!inputs.connected); + } + + public Command runRoller(double inputVolts) { + return startEnd(() -> io.runVolts(inputVolts), io::stop); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..5fe42dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.intake; + +class IntakeConstants { + public static final boolean inverted = true; + public static final double moi = 0.025; // TODO + public static final double gearing = 2.0; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..a4fde97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + class IntakeIOInputs { + public boolean connected = false; + + public double positionRads; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + } + + default void updateInputs(IntakeIOInputs inputs) {} + + default void runVolts(double output) {} + + default void stop() {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..a31f234 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class IntakeIOSim implements IntakeIO { + private final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); + + private double appliedVoltage = 0.0; + + @Override + public void updateInputs(IntakeIOInputs inputs) { + sim.update(Constants.kLoopPeriodSecs); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java new file mode 100644 index 0000000..7ed2a2e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -0,0 +1,83 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.util.Debouncer; + +public class IntakeIOSpark implements IntakeIO { + private final SparkBase spark; + private final RelativeEncoder encoder; + + private final Debouncer connectedDebouncer = new Debouncer(.5); + + public IntakeIOSpark() { + spark = new SparkMax(11, SparkLowLevel.MotorType.kBrushless); + encoder = spark.getEncoder(); + + var config = new SparkMaxConfig(); + config + .inverted(inverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(40, 50) + .voltageCompensation(12.0); + + config + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + config + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + spark.configure( + config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadsPerSec = encoder.getVelocity(); + inputs.appliedVoltage = spark.getAppliedOutput() * spark.getBusVoltage(); + inputs.currentAmps = spark.getOutputCurrent(); + inputs.tempCelsius = spark.getMotorTemperature(); + + inputs.connected = connectedDebouncer.calculate(!spark.hasActiveFault()); + } + + @Override + public void runVolts(double output) { + spark.setVoltage(output); + } + + @Override + public void stop() { + spark.stopMotor(); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); + + spark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java index 0f51f72..4e1e513 100644 --- a/src/main/java/frc/robot/util/AlertsUtil.java +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -1,13 +1,23 @@ package frc.robot.util; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +// LED Alerts +// endgameAlert +// autoScoring (Reef Side denotes side, color denotes level) +// superstructureEstopped +// lowBatteryAlert +// visionDisconnected +// following trajectory +// go to pose +// feederstation alert (alert feeder station player about being OTW) + +// TODO indicate robot initializing in LEDs + public class AlertsUtil { + private static final int numLeds = 120; private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; private static AlertsUtil instance; @@ -24,18 +34,32 @@ public static AlertsUtil getInstance() { private final Alert canErrorAlert = new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); private final Debouncer canErrorDebouncer = new Debouncer(0.5); - private final Alert lowBatteryVoltageAlert = new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); // Program Alerts private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); + private final Alert ledsDisabledAlert = + new Alert("LEDs disabled by program override", AlertType.kInfo); + + private AddressableLED leds; + private AddressableLEDBuffer ledBuffer; private AlertsUtil() { if (Constants.TUNING_MODE) { tuningModeAlert.set(true); } + + if (Constants.ENABLE_LEDs) { + leds = new AddressableLED(1); + ledBuffer = new AddressableLEDBuffer(numLeds); + + leds.setLength(ledBuffer.getLength()); + leds.start(); + } else { + ledsDisabledAlert.set(true); + } } public void periodic() { @@ -50,5 +74,108 @@ public void periodic() { lowBatteryVoltageAlert.set( batteryVoltageDebouncer.calculate( RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + + // Update Program Alerts + // TODO + + // Update Hardware Alerts + if (leds == null) return; + + // // Disable LEDs if battery voltage is too low + // if(ledVoltageDebouncer.calculate( + // RobotController.getBatteryVoltage() <= LED_DISABLE_VOLTAGE_THRESHOLD)) { + // ledsDisabledAlert.set(true); + // ledLowVoltageDisabledAlert.set(true); + // + // leds.stop(); + // leds.close(); + // + // leds = null; + // ledBuffer = null; + // + // return; + } + + // private Color solid(Section section, Color color) { + // if (color != null) { + // for (int i = section.start(); i < section.end(); i++) { + // ledBuffer.setLED(i, color); + // } + // } + // return color; + // } + // + // private Color strobe(Section section, Color c1, Color c2, double duration) { + // boolean c1On = ((Timer.getTimestamp() % duration) / duration) > 0.5; + // return solid(section, c1On ? c1 : c2); + // } + // + // private Color breath(Section section, Color c1, Color c2, double duration, double timestamp) { + // double x = ((timestamp % duration) / duration) * 2.0 * Math.PI; + // double ratio = (Math.sin(x) + 1.0) / 2.0; + // double red = (c1.red * (1 - ratio)) + (c2.red * ratio); + // double green = (c1.green * (1 - ratio)) + (c2.green * ratio); + // double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio); + // var color = new Color(red, green, blue); + // solid(section, color); + // return color; + // } + // + // private Color breath(Section section, Color c1, Color c2, double duration) { + // return breath(section, c1, c2, duration, Timer.getTimestamp()); + // } + // + // private void rainbow(Section section, double cycleLength, double duration) { + // double x = (1 - ((Timer.getTimestamp() / duration) % 1.0)) * 180.0; + // double xDiffPerLed = 180.0 / cycleLength; + // for (int i = section.end() - 1; i >= section.start(); i--) { + // x += xDiffPerLed; + // x %= 180.0; + // ledBuffer.setHSV(i, (int) x, 255, 255); + // } + // } + // + // private void wave(Section section, Color c1, Color c2, double cycleLength, double duration) { + // double x = (1 - ((Timer.getTimestamp() % duration) / duration)) * 2.0 * Math.PI; + // double xDiffPerLed = (2.0 * Math.PI) / cycleLength; + // for (int i = section.end() - 1; i >= section.start(); i--) { + // x += xDiffPerLed; + // double ratio = (Math.pow(Math.sin(x), waveExponent) + 1.0) / 2.0; + // if (Double.isNaN(ratio)) { + // ratio = (-Math.pow(Math.sin(x + Math.PI), waveExponent) + 1.0) / 2.0; + // } + // if (Double.isNaN(ratio)) { + // ratio = 0.5; + // } + // double red = (c1.red * (1 - ratio)) + (c2.red * ratio); + // double green = (c1.green * (1 - ratio)) + (c2.green * ratio); + // double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio); + // ledBuffer.setLED(i, new Color(red, green, blue)); + // } + // } + // + // private void stripes(Section section, List colors, int stripeLength, double duration) { + // int offset = (int) (Timer.getTimestamp() % duration / duration * stripeLength * + // colors.size()); + // for (int i = section.end() - 1; i >= section.start(); i--) { + // int colorIndex = + // (int) (Math.floor((double) (i - offset) / stripeLength) + colors.size()) % + // colors.size(); + // colorIndex = colors.size() - 1 - colorIndex; + // ledBuffer.setLED(i, colors.get(colorIndex)); + // } + // } + + // LEDs have a base mode / pattern / effect + // alerts will update and override specific sections if something is active + // apply the buffer + + public static class HardwareIndicatedAlert extends Alert { + public HardwareIndicatedAlert( + String text, AlertType type, int priority /*TODO handle LED stuff*/) { + super(text, type); + } } + + private static record Section(int start, int end) {} } diff --git a/src/main/java/frc/robot/util/Debouncer.java b/src/main/java/frc/robot/util/Debouncer.java new file mode 100644 index 0000000..657a83a --- /dev/null +++ b/src/main/java/frc/robot/util/Debouncer.java @@ -0,0 +1,90 @@ +package frc.robot.util; + +import edu.wpi.first.math.MathSharedStore; + +/** + * A simple debounce filter for boolean streams. Requires that the boolean change value from + * baseline for a specified period of time before the filtered value changes. + */ +public class Debouncer { + /** Type of debouncing to perform. */ + public enum DebounceType { + /** Rising edge. */ + kRising, + /** Falling edge. */ + kFalling, + /** Both rising and falling edges. */ + kBoth + } + + private double m_debounceTimeSeconds; + private final DebounceType m_debounceType; + private boolean m_baseline; + + private double m_prevTimeSeconds; + + /** + * Creates a new Debouncer. + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + * @param type Which type of state change the debouncing will be performed on. + */ + public Debouncer(double debounceTime, DebounceType type) { + m_debounceTimeSeconds = debounceTime; + m_debounceType = type; + + resetTimer(); + + m_baseline = + switch (m_debounceType) { + case kBoth, kRising -> false; + case kFalling -> true; + }; + } + + /** + * Creates a new Debouncer. Baseline value defaulted to "false." + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + */ + public Debouncer(double debounceTime) { + this(debounceTime, DebounceType.kRising); + } + + private void resetTimer() { + m_prevTimeSeconds = MathSharedStore.getTimestamp(); + } + + private boolean hasElapsed() { + return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds; + } + + /** + * Applies the debouncer to the input stream. + * + * @param input The current value of the input stream. + * @return The debounced value of the input stream. + */ + public boolean calculate(boolean input) { + if (input == m_baseline) { + resetTimer(); + } + + if (hasElapsed()) { + if (m_debounceType == DebounceType.kBoth) { + m_baseline = input; + resetTimer(); + } + return input; + } else { + return m_baseline; + } + } + + public void setDebounceTime(double debounceTime) { + m_debounceTimeSeconds = debounceTime; + resetTimer(); + } +} diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java index a267032..9dacb20 100644 --- a/src/main/java/frc/robot/util/EqualsUtil.java +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -4,7 +4,7 @@ public class EqualsUtil { public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); + return Math.abs(a - b) <= epsilon; } public static boolean epsilonEquals(double a, double b) { diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 71e4c88..30e0232 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -4,14 +4,15 @@ import java.util.Arrays; import java.util.HashMap; import java.util.Map; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. */ -public class LoggedTunableNumber { - private static final String tableKey = "TunableNumbers"; +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/TunableNumbers"; private final String key; private Double defaultValue = null; @@ -131,4 +132,9 @@ public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumb action.run(); } } + + @Override + public double getAsDouble() { + return get(); + } } diff --git a/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java b/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java new file mode 100644 index 0000000..316b2da --- /dev/null +++ b/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java @@ -0,0 +1,10 @@ +package frc.robot.util.trajectory; + +public class DriveTrajectories { + // Let Feederpose be some middle point between all feeder station nodes + // Let face_#_pose be some point between both left and right reef stations BUT not flush with the + // reef + + // known non-OTF trajectories + // feederpose to face_n_pose +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa81b2f..79bdf3e 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.1.0", + "version": "4.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.1.0" + "version": "4.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.1.0", + "version": "4.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ From 60ebba74d081177ca9ca9cc8cb231f7ae826ae81 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 1 Mar 2025 19:00:08 -0500 Subject: [PATCH 56/73] remove intake timeout --- src/main/java/frc/robot/commands/IntakeCommands.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 15c6a4f..68d5061 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -11,8 +11,6 @@ public class IntakeCommands { public static final LoggedTunableNumber intakeVolts = new LoggedTunableNumber("Intake/HopperIntakeVolts", 5.5); - public static final LoggedTunableNumber intakeTimeout = - new LoggedTunableNumber("Intake/IntakeTimeoutSecs", 5.0); public static Command intake(ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) @@ -20,8 +18,7 @@ public static Command intake(ElevatorBase elevator, IntakeBase intake, Dispenser Commands.waitUntil(elevator::isAtGoal) .andThen( Commands.deadline( - dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())) - .withTimeout(intakeTimeout.get()))) + dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())))) .finallyDo(() -> elevator.setGoal(ElevatorState.STOW)); } From 26c56be6041314f5ab5c4aab42ef00ee26996bc9 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 1 Mar 2025 19:00:19 -0500 Subject: [PATCH 57/73] add taxi auto --- src/main/java/frc/robot/RobotContainer.java | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2fcc6c1..f2d2279 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; @@ -120,6 +121,24 @@ public RobotContainer() { "Drive Quasi Reverse", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } + autoChooser.addOption("Do Nothing", Commands.none()); + autoChooser.addOption( + "Taxi", + Commands.runEnd( + () -> driveBase.runVelocity(new ChassisSpeeds(1.0, 0.0, 0.0)), + driveBase::stop, + driveBase) + .withTimeout(2.0) + .beforeStarting( + Commands.runOnce( + () -> + PoseEstimator.getInstance() + .resetPose( + new Pose2d( + PoseEstimator.getInstance().getEstimatedPose().getTranslation(), + AllianceFlipUtil.apply(Rotation2d.kPi))), + driveBase))); + configureButtonBindings(); } From c8e1dbb4acc6841e9bf38da6c89cba5acd75a319 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 1 Mar 2025 19:00:45 -0500 Subject: [PATCH 58/73] make eject configurable based on level --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/dispenser/DispenserBase.java | 20 +++++++++++++++---- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f2d2279..22ad335 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -177,7 +177,7 @@ private void configureButtonBindings() { .and(controller.leftTrigger().negate()) .onTrue( dispenserBase - .eject() + .eject(elevatorBase::getGoal) .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW)))); // Reserialize diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java index eb761d6..926dfb3 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -2,9 +2,12 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.elevator.ElevatorState; import frc.robot.util.Debouncer; import frc.robot.util.LoggedTunableNumber; +import java.util.function.Supplier; import lombok.Getter; import org.littletonrobotics.junction.Logger; @@ -12,13 +15,15 @@ public class DispenserBase extends SubsystemBase { private static final LoggedTunableNumber intakeVolts = new LoggedTunableNumber("Dispenser/IntakeVolts", 6.0); private static final LoggedTunableNumber ejectVolts = - new LoggedTunableNumber("Dispenser/EjectVolts", 4.0); + new LoggedTunableNumber("Dispenser/EjectVolts", 1.3); + private static final LoggedTunableNumber ejectVoltsSlow = + new LoggedTunableNumber("Dispenser/EjectVoltsSlow", 1.3); private static final LoggedTunableNumber holdingCoralPeriod = new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); private static final LoggedTunableNumber ejectPeriod = - new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.35); + new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.75); private final DispenserIO io; private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); @@ -57,7 +62,14 @@ public Command intakeTillHolding() { return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> holdingCoral); } - public Command eject() { - return startEnd(() -> io.runVolts(ejectVolts.get()), io::stop).withTimeout(ejectPeriod.get()); + public Command eject(Supplier state) { + return startEnd( + () -> + io.runVolts( + state.get() == ElevatorState.L1_CORAL + ? ejectVoltsSlow.get() + : ejectVolts.get()), + io::stop) + .withTimeout(ejectPeriod.get()); } } From db57af5ec2e028afb5a7e54af712e5fab6fa0c35 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 1 Mar 2025 19:01:14 -0500 Subject: [PATCH 59/73] add wait period to checking intake when enabling immediately after elevator (fix this later to be better) --- .../java/frc/robot/subsystems/dispenser/DispenserBase.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java index 926dfb3..e81694f 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -59,7 +59,10 @@ public Command runRollers(double inputVolts) { } public Command intakeTillHolding() { - return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop).until(() -> holdingCoral); + return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop) + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), Commands.waitUntil(this::isHoldingCoral))); } public Command eject(Supplier state) { From 2391b502be154be767571a01584c71948d8ec3ee Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 3 Mar 2025 09:49:19 -0500 Subject: [PATCH 60/73] Add robot relative override --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/commands/DriveCommands.java | 13 ++++++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22ad335..9bf91ef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -153,7 +153,8 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX(), - () -> slowModeEnabled)); + () -> slowModeEnabled, + () -> controller.leftBumper().and(controller.rightBumper()).getAsBoolean())); // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index e6b2f97..8d04b47 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -39,7 +39,8 @@ public static Command joystickDrive( DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier, - BooleanSupplier slowSupplier) { + BooleanSupplier slowSupplier, + BooleanSupplier robotRelativeSupplier) { return Commands.run( () -> { // Apply deadband @@ -66,11 +67,13 @@ public static Command joystickDrive( omega * DriveBase.getMaxAngularVelocityRadPerSec() * angularVelocityScalar); // Convert to field relative - Rotation2d rotation = PoseEstimator.getInstance().getRotation(); - if (AllianceFlipUtil.shouldFlip()) { - rotation = rotation.rotateBy(Rotation2d.kPi); + if (!robotRelativeSupplier.getAsBoolean()) { + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); } - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds driveBase.runVelocity(speeds); From 5e8c5a008d240a4a0fd332fbe51871553b515d07 Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Tue, 4 Mar 2025 09:00:41 -0500 Subject: [PATCH 61/73] added unit conversion and promptly commented it back out --- .../frc/robot/subsystems/drive/ModuleIOSpark.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 24414cf..b80427e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -169,9 +169,14 @@ public void runTurnOpenLoop(double output) { @Override public void runDriveVelocity(double velocityRadPerSec) { - double ffVolts = driveFeedforward.calculate(velocityRadPerSec); + double ffVolts = + driveFeedforward.calculate( + velocityRadPerSec + // * 1 / (Math.PI * 2) + ); driveController.setReference( velocityRadPerSec, + // * 1 / (Math.PI * 2), ControlType.kVelocity, ClosedLoopSlot.kSlot0, ffVolts, @@ -182,7 +187,11 @@ public void runDriveVelocity(double velocityRadPerSec) { public void runTurnPosition(Rotation2d rotation) { // Because internal encoder is relative, we want to wrap to the range -π to π radians. var updatedSetpoint = MathUtil.angleModulus(rotation.getRadians()); - turnController.setReference(updatedSetpoint, ControlType.kPosition); + turnController.setReference( + updatedSetpoint + // * 1 / (Math.PI * 2) + , + ControlType.kPosition); } @Override From 813d3df00fc8e4204b21972c10b2483b91cba476 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 4 Mar 2025 14:04:32 -0500 Subject: [PATCH 62/73] add algae mode --- src/main/java/frc/robot/RobotContainer.java | 52 ++++++++++--------- .../subsystems/elevator/ElevatorState.java | 22 +++++--- 2 files changed, 43 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9bf91ef..9defebc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,6 +98,14 @@ public RobotContainer() { "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); autoChooser.addOption( "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); + autoChooser.addOption( + "Drive Dynamic Forward", driveBase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Dynamic Reverse", driveBase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive Quasi Forward", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Quasi Reverse", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); autoChooser.addOption( "Elevator Dynamic Forward", elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kForward, 0.5)); @@ -110,18 +118,9 @@ public RobotContainer() { autoChooser.addOption( "Elevator Quasi Reverse", elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse, 3)); - - autoChooser.addOption( - "Drive Dynamic Forward", driveBase.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive Dynamic Reverse", driveBase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Drive Quasi Forward", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive Quasi Reverse", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); } - autoChooser.addOption("Do Nothing", Commands.none()); + autoChooser.addDefaultOption("Noting", Commands.none()); autoChooser.addOption( "Taxi", Commands.runEnd( @@ -163,30 +162,35 @@ private void configureButtonBindings() { .povLeft() .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); // L2 - controller.povUp().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL))); + controller + .povUp() + .onTrue( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)), + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25))); + // L3 controller .povRight() - .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL))); + .onTrue( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)), + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25))); // Intake controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); - // Dispense controller .rightTrigger() - .and(controller.leftTrigger().negate()) .onTrue( - dispenserBase - .eject(elevatorBase::getGoal) - .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW)))); - - // Reserialize - controller - .leftTrigger() - .and(controller.rightTrigger()) - .debounce(0.25) - .onTrue(IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase)); + Commands.either( + dispenserBase + .eject(elevatorBase::getGoal) + .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))), + IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), + controller.leftTrigger().negate().debounce(0.25))); // Home Elevator controller diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index f10f24c..17655d3 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -15,9 +15,12 @@ public enum ElevatorState { START(() -> 0.0), STOW("Stow", 0.0), INTAKE("Intake", 0.0), - L1_CORAL(ReefLevel.L1, Units.inchesToMeters(7)), - L2_CORAL(ReefLevel.L2, Units.inchesToMeters(1)), - L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0)); + L1_CORAL(ReefLevel.L1, Units.inchesToMeters(7), false), + L2_CORAL(ReefLevel.L2, Units.inchesToMeters(1), false), + L2_ALGAE_REMOVAL(ReefLevel.L2, Units.inchesToMeters(5), true), + L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0), false), + L3_ALGAE_REMOVAL(ReefLevel.L3, Units.inchesToMeters(0), true), + L4_CORAL(ReefLevel.L4, Units.inchesToMeters(0.0), false); private final DoubleSupplier elevatorHeightMeters; @@ -25,10 +28,15 @@ public enum ElevatorState { elevatorHeightMeters = new LoggedTunableNumber("Elevator/Presets/" + name, defaultValue); } - ElevatorState(ReefLevel reefLevel, double defaultOffset) { - var offsetTunable = - new LoggedTunableNumber( - String.format("Elevator/Presets/%s Offset", reefLevel), defaultOffset); + ElevatorState(ReefLevel reefLevel, double defaultOffset, boolean isAlgae) { + String stateName; + if (isAlgae) { + stateName = String.format("Elevator/Presets/%s_Algae Offset", reefLevel); + } else { + stateName = String.format("Elevator/Presets/%s Offset", reefLevel); + } + + var offsetTunable = new LoggedTunableNumber(stateName, defaultOffset); elevatorHeightMeters = () -> (reefLevel.height + offsetTunable.get() - originToBaseHeightMeters) From f47b2de7462a71805594f16d023da1ee4bd56801 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 4 Mar 2025 23:27:50 -0500 Subject: [PATCH 63/73] stuff for ayush to fix --- .../java/frc/robot/commands/DriveToPose.java | 50 ------------------- .../frc/robot/commands/DriveTrajectory.java | 8 --- .../subsystems/elevator/ElevatorBase.java | 6 --- .../elevator/ElevatorVisualizer.java | 11 ---- 4 files changed, 75 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DriveToPose.java delete mode 100644 src/main/java/frc/robot/commands/DriveTrajectory.java delete mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java deleted file mode 100644 index 8914e2f..0000000 --- a/src/main/java/frc/robot/commands/DriveToPose.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.drive.DriveBase; -import frc.robot.util.LoggedTunableNumber; -import java.util.function.Supplier; - -public class DriveToPose extends Command { - private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("DriveToPose/DrivekP"); - private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("DriveToPose/DrivekD"); - private static final LoggedTunableNumber thetakP = new LoggedTunableNumber("DriveToPose/ThetakP"); - private static final LoggedTunableNumber thetakD = new LoggedTunableNumber("DriveToPose/ThetakD"); - private static final LoggedTunableNumber driveMaxVelocity = - new LoggedTunableNumber("DriveToPose/DriveMaxVelocity"); - private static final LoggedTunableNumber driveMaxVelocitySlow = - new LoggedTunableNumber("DriveToPose/DriveMaxVelocitySlow"); - private static final LoggedTunableNumber driveMaxAcceleration = - new LoggedTunableNumber("DriveToPose/DriveMaxAcceleration"); - private static final LoggedTunableNumber thetaMaxVelocity = - new LoggedTunableNumber("DriveToPose/ThetaMaxVelocity"); - private static final LoggedTunableNumber thetaMaxAcceleration = - new LoggedTunableNumber("DriveToPose/ThetaMaxAcceleration"); - private static final LoggedTunableNumber driveTolerance = - new LoggedTunableNumber("DriveToPose/DriveTolerance"); - private static final LoggedTunableNumber thetaTolerance = - new LoggedTunableNumber("DriveToPose/ThetaTolerance"); - private static final LoggedTunableNumber ffMinRadius = - new LoggedTunableNumber("DriveToPose/FFMinRadius"); - private static final LoggedTunableNumber ffMaxRadius = - new LoggedTunableNumber("DriveToPose/FFMaxRadius"); - - static { - drivekP.initDefault(0.75); - drivekD.initDefault(0.0); - thetakP.initDefault(4.0); - thetakD.initDefault(0.0); - driveMaxVelocity.initDefault(3.8); - driveMaxAcceleration.initDefault(3.0); - thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0)); - thetaMaxAcceleration.initDefault(8.0); - driveTolerance.initDefault(0.01); - thetaTolerance.initDefault(Units.degreesToRadians(1.0)); - ffMinRadius.initDefault(0.1); - ffMaxRadius.initDefault(0.15); - } - - public DriveToPose(DriveBase drive, Supplier target) {} -} diff --git a/src/main/java/frc/robot/commands/DriveTrajectory.java b/src/main/java/frc/robot/commands/DriveTrajectory.java deleted file mode 100644 index 11a161c..0000000 --- a/src/main/java/frc/robot/commands/DriveTrajectory.java +++ /dev/null @@ -1,8 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.drive.DriveBase; - -public class DriveTrajectory extends Command { - public DriveTrajectory(DriveBase drive) {} -} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 0719f83..5f0eddc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -99,9 +99,6 @@ public class ElevatorBase extends SubsystemBase { @Getter private boolean atGoal = false; - private final ElevatorVisualizer measuredVisualizer = new ElevatorVisualizer("Measured"); - private final ElevatorVisualizer setpointVisualizer = new ElevatorVisualizer("Setpoint"); - private final SysIdRoutine sysId; public ElevatorBase(ElevatorIO io) { @@ -225,9 +222,6 @@ public void periodic() { if (!homed && !profileDisabled) { homingSequence().schedule(); } - - measuredVisualizer.update(getPositionMeters()); - setpointVisualizer.update(setpoint.position); } /** Returns a command to run a quasistatic test in the specified direction. */ diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java deleted file mode 100644 index 0196eb9..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.subsystems.elevator; - -public class ElevatorVisualizer { - private final String name; - - public ElevatorVisualizer(String name) { - this.name = name; - } - - public void update(double elevatorPositionMeters) {} -} From 807290376c46d7f105b2fbe289dcb65f693ef7b3 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 4 Mar 2025 23:39:29 -0500 Subject: [PATCH 64/73] update vendor deps --- ...enix6-25.2.2.json => Phoenix6-25.3.1.json} | 88 +++++++++++++------ vendordeps/{REVLib-2025.json => REVLib.json} | 12 +-- 2 files changed, 65 insertions(+), 35 deletions(-) rename vendordeps/{Phoenix6-25.2.2.json => Phoenix6-25.3.1.json} (86%) rename vendordeps/{REVLib-2025.json => REVLib.json} (89%) diff --git a/vendordeps/Phoenix6-25.2.2.json b/vendordeps/Phoenix6-25.3.1.json similarity index 86% rename from vendordeps/Phoenix6-25.2.2.json rename to vendordeps/Phoenix6-25.3.1.json index 39ae6c5..3ff25f8 100644 --- a/vendordeps/Phoenix6-25.2.2.json +++ b/vendordeps/Phoenix6-25.3.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.2.json", + "fileName": "Phoenix6-25.3.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.3.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.3.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } diff --git a/vendordeps/REVLib-2025.json b/vendordeps/REVLib.json similarity index 89% rename from vendordeps/REVLib-2025.json rename to vendordeps/REVLib.json index 717aa34..459a62f 100644 --- a/vendordeps/REVLib-2025.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.2", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.2" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 72df17df6088b8289b7983d49738152fdcb178bc Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 5 Mar 2025 00:49:10 -0500 Subject: [PATCH 65/73] Update tunable number API to avoid redundant resets and call correct static overrides --- .../frc/robot/subsystems/drive/Module.java | 19 ++++---- .../subsystems/elevator/ElevatorBase.java | 43 ++++++++++++------- .../frc/robot/util/LoggedTunableNumber.java | 41 ++++++++++++++---- 3 files changed, 70 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index eeee2a2..a05696f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -69,15 +69,16 @@ public void updateInputs() { 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()); - } + LoggedTunableNumber.ifChanged( + hashCode(), () -> m_io.setDriveFF(drivekS.get(), drivekV.get()), true, drivekS, drivekV); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> m_io.setDrivePID(drivekP.get(), 0, drivekD.get()), + true, + drivekP, + drivekD); + LoggedTunableNumber.ifChanged( + hashCode(), () -> m_io.setTurnPID(turnkP.get(), 0, turnkD.get()), true, turnkP, turnkD); // Update Odometry Positions int sampleCount = m_inputs.odometryDrivePositionsRad.length; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 5f0eddc..ed8e584 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -99,6 +99,9 @@ public class ElevatorBase extends SubsystemBase { @Getter private boolean atGoal = false; + private final ElevatorVisualizer measuredVisualizer = new ElevatorVisualizer("Measured"); + private final ElevatorVisualizer setpointVisualizer = new ElevatorVisualizer("Setpoint"); + private final SysIdRoutine sysId; public ElevatorBase(ElevatorIO io) { @@ -133,22 +136,27 @@ public void periodic() { followerDisconnectedAlert.set(!inputs.followerConnected); // Update tunable numbers - if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { - io.setPID(kP.get(), 0.0, kD.get()); - } - if (kS.hasChanged(hashCode()) || kG.hasChanged(hashCode()) || kA.hasChanged(hashCode())) { - feedforward.setKs(kS.get()); - feedforward.setKg(kG.get()); - feedforward.setKa(kA.get()); - } - - if (maxVelocityMetersPerSec.hasChanged(hashCode()) - || maxAccelerationMetersPerSec2.hasChanged(hashCode())) { - profile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); - } + LoggedTunableNumber.ifChanged(() -> io.setPID(kP.get(), 0.0, kD.get()), true, kP, kD); + LoggedTunableNumber.ifChanged( + () -> { + feedforward.setKs(kS.get()); + feedforward.setKg(kG.get()); + feedforward.setKa(kA.get()); + }, + true, + kS, + kG, + kA); + + LoggedTunableNumber.ifChanged( + () -> + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())), + true, + maxVelocityMetersPerSec, + maxAccelerationMetersPerSec2); // Run profile final boolean shouldRunProfile = @@ -222,6 +230,9 @@ public void periodic() { if (!homed && !profileDisabled) { homingSequence().schedule(); } + + measuredVisualizer.update(getPositionMeters()); + setpointVisualizer.update(setpoint.position); } /** Returns a command to run a quasistatic test in the specified direction. */ diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 30e0232..a265098 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -1,7 +1,6 @@ package frc.robot.util; import frc.robot.Constants; -import java.util.Arrays; import java.util.HashMap; import java.util.Map; import java.util.function.DoubleSupplier; @@ -109,15 +108,40 @@ public boolean hasChanged() { return hasChanged(0); } + public void resetLastValue(int id) { + lastValues.put(id, get()); + } + + public void resetLastValue() { + resetLastValue(0); + } + /** * Run callback if any tunable number has changed. See {@link #hasChanged(int)} for usage. * * @param action action to run + * @param resetAll if true and any TunableNumber in the set was changed, will reset the status of + * all TunableNumbers in the set. Useful for avoiding redundant resetting. * @param tunableNumbers tunable numbers to check */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(v -> v.hasChanged(id))) { - action.run(); + public static void ifChanged( + int id, Runnable action, boolean resetAll, LoggedTunableNumber... tunableNumbers) { + + // Only force resetLastValue on numbers that haven't already been checked for changes to avoid + // redundancy. If not, break the loop on the first found instance. + boolean hasRunAction = false; + for (var tunableNumber : tunableNumbers) { + if (hasRunAction) { + tunableNumber.resetLastValue(id); + } else if (tunableNumber.hasChanged(id)) { + action.run(); + hasRunAction = true; + + // Exit early if no further resets are needed + if (!resetAll) { + break; + } + } } } @@ -125,12 +149,13 @@ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tun * Run callback if any tunable number has changed. See {@link #hasChanged()} for usage. * * @param action action to run + * @param resetAll if true and any TunableNumber in the set was changed, will reset the status of + * all TunableNumbers in the set. Useful for avoiding redundant resetting. * @param tunableNumbers tunable numbers to check */ - public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(LoggedTunableNumber::hasChanged)) { - action.run(); - } + public static void ifChanged( + Runnable action, boolean resetAll, LoggedTunableNumber... tunableNumbers) { + ifChanged(0, action, resetAll, tunableNumbers); } @Override From 553d51a6b0895bc75a3d32d4ffa376010b4638d2 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 5 Mar 2025 01:01:36 -0500 Subject: [PATCH 66/73] Reimplement I term into SparkMax PID Controllers --- .../java/frc/robot/subsystems/drive/Module.java | 17 +++++++++++++++-- .../robot/subsystems/elevator/ElevatorBase.java | 5 ++++- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a05696f..3a9f3d5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -17,9 +17,12 @@ class Module { new LoggedTunableNumber("Drive/Module/DrivekV"); private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("Drive/Module/DrivekP"); + private static final LoggedTunableNumber drivekI = + new LoggedTunableNumber("Drive/Module/DrivekI"); private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("Drive/Module/DrivekD"); private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); + private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI"); private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); static { @@ -28,16 +31,20 @@ class Module { drivekS.initDefault(0.69641); drivekV.initDefault(0.12647); drivekP.initDefault(0.0); + drivekI.initDefault(0.0); drivekD.initDefault(0.0); turnkP.initDefault(1.5); + turnkI.initDefault(0.0); turnkD.initDefault(0.0); } default -> { drivekS.initDefault(0.113190); drivekV.initDefault(0.841640); drivekP.initDefault(0.1); + drivekI.initDefault(0.0); drivekD.initDefault(0.0); turnkP.initDefault(10.0); + turnkI.initDefault(0.0); turnkD.initDefault(0.0); } } @@ -73,12 +80,18 @@ public void periodic() { hashCode(), () -> m_io.setDriveFF(drivekS.get(), drivekV.get()), true, drivekS, drivekV); LoggedTunableNumber.ifChanged( hashCode(), - () -> m_io.setDrivePID(drivekP.get(), 0, drivekD.get()), + () -> m_io.setDrivePID(drivekP.get(), drivekI.get(), drivekD.get()), true, drivekP, + drivekI, drivekD); LoggedTunableNumber.ifChanged( - hashCode(), () -> m_io.setTurnPID(turnkP.get(), 0, turnkD.get()), true, turnkP, turnkD); + hashCode(), + () -> m_io.setTurnPID(turnkP.get(), turnkI.get(), turnkD.get()), + true, + turnkP, + turnkI, + turnkD); // Update Odometry Positions int sampleCount = m_inputs.odometryDrivePositionsRad.length; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index ed8e584..5681691 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -25,6 +25,7 @@ public class ElevatorBase extends SubsystemBase { // Tunable numbers private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/kI"); private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD"); private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); @@ -49,6 +50,7 @@ public class ElevatorBase extends SubsystemBase { switch (Constants.getRobot()) { case COMPBOT -> { kP.initDefault(0.3); + kI.initDefault(0.0); kD.initDefault(0.25); kS.initDefault(0); kG.initDefault(1.05); @@ -56,6 +58,7 @@ public class ElevatorBase extends SubsystemBase { } case SIMBOT -> { kP.initDefault(0); // TODO + kI.initDefault(0.0); // TODO kD.initDefault(0); // TODO kS.initDefault(0); // TODO kG.initDefault(0); // TODO @@ -136,7 +139,7 @@ public void periodic() { followerDisconnectedAlert.set(!inputs.followerConnected); // Update tunable numbers - LoggedTunableNumber.ifChanged(() -> io.setPID(kP.get(), 0.0, kD.get()), true, kP, kD); + LoggedTunableNumber.ifChanged(() -> io.setPID(kP.get(), kI.get(), kD.get()), true, kP, kI, kD); LoggedTunableNumber.ifChanged( () -> { feedforward.setKs(kS.get()); From b83d704f9093ff86cdc441e5d09cbc566b655db2 Mon Sep 17 00:00:00 2001 From: Ayush Pal Date: Wed, 5 Mar 2025 20:41:11 -0500 Subject: [PATCH 67/73] Merge branch 'sriman-dev' --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 54 ++-- src/main/java/frc/robot/FieldConstants.java | 195 +++++------- .../{RobotState.java => PoseEstimator.java} | 18 +- src/main/java/frc/robot/Robot.java | 14 +- src/main/java/frc/robot/RobotContainer.java | 185 +++++++++-- .../java/frc/robot/commands/AutoRoutine.java | 3 + .../frc/robot/commands/DriveCommands.java | 213 +++---------- .../frc/robot/commands/IntakeCommands.java | 33 ++ .../subsystems/dispenser/DispenserBase.java | 78 +++++ .../dispenser/DispenserConstants.java | 7 + .../subsystems/dispenser/DispenserIO.java | 26 ++ .../subsystems/dispenser/DispenserIOSim.java | 40 +++ .../dispenser/DispenserIOSpark.java | 90 ++++++ .../frc/robot/subsystems/drive/DriveBase.java | 203 +++++++++++- .../subsystems/drive/DriveConstants.java | 20 +- .../frc/robot/subsystems/drive/Module.java | 57 ++-- .../frc/robot/subsystems/drive/ModuleIO.java | 2 + .../robot/subsystems/drive/ModuleIOSim.java | 9 +- .../robot/subsystems/drive/ModuleIOSpark.java | 22 +- .../subsystems/drive/OdometryManager.java | 2 +- .../subsystems/elevator/ElevatorBase.java | 296 ++++++++++++++++++ .../elevator/ElevatorConstants.java | 23 ++ .../robot/subsystems/elevator/ElevatorIO.java | 31 ++ .../subsystems/elevator/ElevatorIOSim.java | 74 +++++ .../subsystems/elevator/ElevatorIOSpark.java | 150 +++++++++ .../subsystems/elevator/ElevatorState.java | 45 +++ .../robot/subsystems/intake/IntakeBase.java | 30 ++ .../subsystems/intake/IntakeConstants.java | 7 + .../frc/robot/subsystems/intake/IntakeIO.java | 24 ++ .../robot/subsystems/intake/IntakeIOSim.java | 40 +++ .../subsystems/intake/IntakeIOSpark.java | 83 +++++ src/main/java/frc/robot/util/AlertsUtil.java | 181 +++++++++++ .../java/frc/robot/util/AllianceFlipUtil.java | 51 ++- src/main/java/frc/robot/util/Debouncer.java | 90 ++++++ src/main/java/frc/robot/util/EqualsUtil.java | 8 +- .../frc/robot/util/LoggedTunableNumber.java | 82 ++--- src/main/java/frc/robot/util/LoggerUtil.java | 2 +- .../util/swerve/SwerveSetpointGenerator.java | 33 +- .../util/trajectory/DriveTrajectories.java | 10 + vendordeps/AdvantageKit.json | 6 +- ...c2025-latest.json => Phoenix6-25.3.1.json} | 88 ++++-- vendordeps/{REVLib-2025.json => REVLib.json} | 12 +- 43 files changed, 2128 insertions(+), 511 deletions(-) rename src/main/java/frc/robot/{RobotState.java => PoseEstimator.java} (88%) create mode 100644 src/main/java/frc/robot/commands/AutoRoutine.java create mode 100644 src/main/java/frc/robot/commands/IntakeCommands.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorState.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeBase.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java create mode 100644 src/main/java/frc/robot/util/AlertsUtil.java create mode 100644 src/main/java/frc/robot/util/Debouncer.java create mode 100644 src/main/java/frc/robot/util/trajectory/DriveTrajectories.java rename vendordeps/{Phoenix6-frc2025-latest.json => Phoenix6-25.3.1.json} (86%) rename vendordeps/{REVLib-2025.json => REVLib.json} (89%) diff --git a/build.gradle b/build.gradle index cf63f85..ea7a7aa 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "7.0.2" id "io.freefair.lombok" version "8.11" diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed989ab..2697903 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,49 +1,49 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotBase; public class Constants { - private static RobotType kRobotType = RobotType.ROBOT_2025_COMP; + private static RobotType robotType = RobotType.COMPBOT; // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running public static final boolean ENABLE_LOGGING = true; + // Disable LEDs, will reduce software and electrical overhead but disable hardware alerts + public static final boolean ENABLE_LEDs = false; public static final double kLoopPeriodSecs = 0.02; - public static final double LOW_VOLTAGE_WARNING_THRESHOLD = 10.0; - - public enum RobotMode { - REAL, - SIM, - REPLAY + public static RobotType getRobot() { + if (RobotBase.isReal() && robotType == RobotType.SIMBOT) { + new Alert( + "Invalid robot selected, using competition robot as default.", Alert.AlertType.kError) + .set(true); + robotType = RobotType.COMPBOT; + } + return robotType; } - public enum RobotType { - ROBOT_2025_COMP, - ROBOT_SIMBOT + public static Mode getMode() { + return switch (robotType) { + case COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case SIMBOT -> Mode.SIM; + }; } - public static RobotType getRobotType() { - if (RobotBase.isReal() && kRobotType == RobotType.ROBOT_SIMBOT) { - DriverStation.reportError( - "Robot is set to SIM but it isn't a SIM, setting it to Competition Robot as redundancy.", - false); - kRobotType = RobotType.ROBOT_2025_COMP; - } + public enum Mode { + /** Running on a real robot. */ + REAL, - if (RobotBase.isSimulation() && kRobotType != RobotType.ROBOT_SIMBOT) { - DriverStation.reportError( - "Robot is set to REAL but it is a SIM, setting it to SIMBOT as redundancy.", false); - kRobotType = RobotType.ROBOT_SIMBOT; - } + /** Running a physics simulator. */ + SIM, - return kRobotType; + /** Replaying from a log file. */ + REPLAY } - public static RobotMode getRobotMode() { - if (getRobotType() == RobotType.ROBOT_SIMBOT) return RobotMode.SIM; - else return RobotBase.isReal() ? RobotMode.REAL : RobotMode.REPLAY; + public enum RobotType { + SIMBOT, + COMPBOT } } diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index aad1e00..9a3e7b2 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,25 +1,32 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFieldLayout; 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; +import java.io.IOException; +import java.util.*; +import lombok.Getter; /** * 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 AprilTagFieldLayout fieldLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + + public static final double fieldLength = + AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldLength(); + public static final double fieldWidth = + AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldWidth(); 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)); + new Pose2d( + AprilTagLayoutType.OFFICIAL.getFieldLayout().getTagPose(16).get().getX(), + 0, + Rotation2d.fromDegrees(90)); } public static class Barge { @@ -36,73 +43,76 @@ public static class Barge { } 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 double stationLength = Units.inchesToMeters(79.750); public static final Pose2d rightCenterFace = new Pose2d( Units.inchesToMeters(33.526), Units.inchesToMeters(25.824), Rotation2d.fromDegrees(144.011 - 90)); + public static final Pose2d leftCenterFace = + new Pose2d( + rightCenterFace.getX(), + fieldWidth - rightCenterFace.getY(), + Rotation2d.fromRadians(-rightCenterFace.getRotation().getRadians())); + } + + public enum ReefLevel { + L1(Units.inchesToMeters(25.0), 0), + L2(Units.inchesToMeters(31.875 - Math.cos(Math.toRadians(35.0)) * 0.625), -35), + L3(Units.inchesToMeters(47.625 - Math.cos(Math.toRadians(35.0)) * 0.625), -35), + L4(Units.inchesToMeters(72), -90); + + public final double height; + public final double pitch; + + ReefLevel(double height, double pitch) { + this.height = height; + this.pitch = pitch; // Degrees + } + + public static ReefLevel fromLevel(int level) { + return Arrays.stream(values()) + .filter(height -> height.ordinal() == level) + .findFirst() + .orElse(L4); + } } public static class Reef { + public static final double faceLength = Units.inchesToMeters(36.792600); public static final Translation2d center = - new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + new Translation2d(Units.inchesToMeters(176.746), fieldWidth / 2.0); 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 = + public static final List> branchPositions = new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise + public static final List> branchPositions2d = new ArrayList<>(); 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)); + var aprilTagLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout(); + centerFaces[0] = aprilTagLayout.getTagPose(18).get().toPose2d(); + centerFaces[1] = aprilTagLayout.getTagPose(19).get().toPose2d(); + centerFaces[2] = aprilTagLayout.getTagPose(20).get().toPose2d(); + centerFaces[3] = aprilTagLayout.getTagPose(21).get().toPose2d(); + centerFaces[4] = aprilTagLayout.getTagPose(22).get().toPose2d(); + centerFaces[5] = aprilTagLayout.getTagPose(17).get().toPose2d(); // Initialize branch positions for (int face = 0; face < 6; face++) { - Map fillRight = new HashMap<>(); - Map fillLeft = new HashMap<>(); - for (var level : ReefHeight.values()) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + Map fillRight2d = new HashMap<>(); + Map fillLeft2d = new HashMap<>(); + for (var level : ReefLevel.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, + var rightBranchPose = new Pose3d( new Translation3d( poseDirection @@ -115,9 +125,8 @@ public static class Reef { new Rotation3d( 0, Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); - fillLeft.put( - level, + poseDirection.getRotation().getRadians())); + var leftBranchPose = new Pose3d( new Translation3d( poseDirection @@ -130,74 +139,44 @@ public static class Reef { new Rotation3d( 0, Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); + poseDirection.getRotation().getRadians())); + + fillRight.put(level, rightBranchPose); + fillLeft.put(level, leftBranchPose); + fillRight2d.put(level, rightBranchPose.toPose2d()); + fillLeft2d.put(level, leftBranchPose.toPose2d()); } - branchPositions.add((face * 2) + 1, fillRight); - branchPositions.add((face * 2) + 2, fillLeft); + branchPositions.add(fillRight); + branchPositions.add(fillLeft); + branchPositions2d.add(fillRight2d); + branchPositions2d.add(fillLeft2d); } } } 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 double separation = Units.inchesToMeters(72.0); public static final Pose2d middleIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); + new Pose2d(Units.inchesToMeters(48), fieldWidth / 2.0, new Rotation2d()); + public static final Pose2d leftIceCream = + new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() + separation, new Rotation2d()); public static final Pose2d rightIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); + new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() - separation, 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); + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2025-reefscape-welded.json"); - ReefHeight(double height, double pitch) { - this.height = height; - this.pitch = pitch; // in degrees - } + private final AprilTagFieldLayout fieldLayout; - public final double height; - public final double pitch; + private AprilTagLayoutType(String file) { + try { + fieldLayout = AprilTagFieldLayout.loadFromResource(file); + } catch (IOException exception) { + throw new RuntimeException("Failed to load AprilTagLayoutType: " + file, exception); + } + } } - - // 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/RobotState.java b/src/main/java/frc/robot/PoseEstimator.java similarity index 88% rename from src/main/java/frc/robot/RobotState.java rename to src/main/java/frc/robot/PoseEstimator.java index 149fb4e..07a05af 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/PoseEstimator.java @@ -11,32 +11,32 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.DriveBase; import lombok.Getter; import org.littletonrobotics.junction.AutoLogOutput; -public class RobotState { +public class PoseEstimator { // Standard deviations of the pose estimate (x position in meters, y position in meters, and // heading in radians). // Increase these numbers to trust your state estimate less. private static final Matrix odometryStateStdDevs = VecBuilder.fill(0.003, 0.003, 0.002); private static final double poseBufferSizeSec = 2.0; - private static RobotState instance; + private static PoseEstimator instance; - public static RobotState getInstance() { + public static PoseEstimator getInstance() { if (instance == null) { - instance = new RobotState(); + instance = new PoseEstimator(); } return instance; } @Getter - @AutoLogOutput(key = "RobotState/OdometryPose") + @AutoLogOutput(key = "PoseEstimator/OdometryPose") private Pose2d odometryPose = new Pose2d(); @Getter - @AutoLogOutput(key = "RobotState/EstimatedPose") + @AutoLogOutput(key = "PoseEstimator/EstimatedPose") private Pose2d estimatedPose = new Pose2d(); private final TimeInterpolatableBuffer poseBuffer = @@ -55,12 +55,12 @@ public static RobotState getInstance() { // Assume gyro starts at zero private Rotation2d gyroOffset = new Rotation2d(); - private RobotState() { + private PoseEstimator() { for (int i = 0; i < 3; ++i) { qStdDevs.set(i, 0, Math.pow(odometryStateStdDevs.get(i, 0), 2)); } - kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); + kinematics = new SwerveDriveKinematics(DriveBase.getModuleTranslations()); } public void resetPose(Pose2d pose) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 64b446b..73c12cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,13 +13,12 @@ package frc.robot; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.AlertsUtil; import frc.robot.util.LoggerUtil; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -39,16 +38,12 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private final RobotContainer robotContainer; - private final Alert lowBatteryVoltageAlert = - new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning); - private final Debouncer batteryVoltageDebouncer = new Debouncer(0.5); - public Robot() { super(Constants.kLoopPeriodSecs); LoggerUtil.initializeLoggerMetadata(); - switch (Constants.getRobotMode()) { + switch (Constants.getMode()) { case REAL -> { // Running on a real robot, log to a USB stick var loggerPath = LoggerUtil.getLogPath(); @@ -92,10 +87,7 @@ public void robotPeriodic() { // Run command scheduler CommandScheduler.getInstance().run(); - // Update Battery Voltage Alert - lowBatteryVoltageAlert.set( - batteryVoltageDebouncer.calculate( - RobotController.getBatteryVoltage() <= Constants.LOW_VOLTAGE_WARNING_THRESHOLD)); + AlertsUtil.getInstance().periodic(); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fc75b6..9defebc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,20 +2,39 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.commands.IntakeCommands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.dispenser.DispenserIO; +import frc.robot.subsystems.dispenser.DispenserIOSim; +import frc.robot.subsystems.dispenser.DispenserIOSpark; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.elevator.*; +import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.intake.IntakeIOSpark; import frc.robot.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class RobotContainer { - // Load RobotState class - private final RobotState robotState = RobotState.getInstance(); + // Load PoseEstimator class + private final PoseEstimator poseEstimator = PoseEstimator.getInstance(); // Subsystems private final DriveBase driveBase; + private final IntakeBase intakeBase; + private final ElevatorBase elevatorBase; + private final DispenserBase dispenserBase; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -23,8 +42,15 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + private final LoggedNetworkNumber endgameAlert1 = + new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #1", 30.0); + private final LoggedNetworkNumber endgameAlert2 = + new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0); + + private boolean slowModeEnabled; + public RobotContainer() { - switch (Constants.getRobotMode()) { + switch (Constants.getMode()) { case REAL -> { driveBase = new DriveBase( @@ -33,6 +59,9 @@ public RobotContainer() { new ModuleIOSpark(1), new ModuleIOSpark(2), new ModuleIOSpark(3)); + intakeBase = new IntakeBase(new IntakeIOSpark()); + elevatorBase = new ElevatorBase(new ElevatorIOSpark()); + dispenserBase = new DispenserBase(new DispenserIOSpark()); } case SIM -> { driveBase = @@ -42,6 +71,9 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + intakeBase = new IntakeBase(new IntakeIOSim()); + elevatorBase = new ElevatorBase(new ElevatorIOSim()); + dispenserBase = new DispenserBase(new DispenserIOSim()); } default -> { driveBase = @@ -51,6 +83,9 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + intakeBase = new IntakeBase(new IntakeIO() {}); + elevatorBase = new ElevatorBase(new ElevatorIO() {}); + dispenserBase = new DispenserBase(new DispenserIO() {}); } } @@ -60,50 +95,154 @@ public RobotContainer() { if (Constants.TUNING_MODE) { // Set up Characterization routines autoChooser.addOption( - "Drive Wheel Radius Characterization", - DriveCommands.wheelRadiusCharacterization(driveBase)); + "Drive Wheel Radius Characterization", driveBase.wheelRadiusCharacterization()); + autoChooser.addOption( + "Drive Simple FF Characterization", driveBase.feedforwardCharacterization()); + autoChooser.addOption( + "Drive Dynamic Forward", driveBase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Dynamic Reverse", driveBase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive Quasi Forward", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Quasi Reverse", driveBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Elevator Dynamic Forward", + elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kForward, 0.5)); + autoChooser.addOption( + "Elevator Dynamic Reverse", + elevatorBase.sysIdDynamic(SysIdRoutine.Direction.kReverse, 0.25)); autoChooser.addOption( - "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(driveBase)); + "Elevator Quasi Forward", + elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kForward, 25)); + autoChooser.addOption( + "Elevator Quasi Reverse", + elevatorBase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse, 3)); } + autoChooser.addDefaultOption("Noting", Commands.none()); + autoChooser.addOption( + "Taxi", + Commands.runEnd( + () -> driveBase.runVelocity(new ChassisSpeeds(1.0, 0.0, 0.0)), + driveBase::stop, + driveBase) + .withTimeout(2.0) + .beforeStarting( + Commands.runOnce( + () -> + PoseEstimator.getInstance() + .resetPose( + new Pose2d( + PoseEstimator.getInstance().getEstimatedPose().getTranslation(), + AllianceFlipUtil.apply(Rotation2d.kPi))), + driveBase))); + configureButtonBindings(); } private void configureButtonBindings() { + // Make slow mode toggleable + controller.y().toggleOnTrue(Commands.runOnce(() -> slowModeEnabled = !slowModeEnabled)); + // Default command, normal field-relative drive driveBase.setDefaultCommand( DriveCommands.joystickDrive( driveBase, () -> -controller.getLeftY(), () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -controller.getRightX(), + () -> slowModeEnabled, + () -> controller.leftBumper().and(controller.rightBumper()).getAsBoolean())); - // Lock to 0° when A button is held + // Stow + controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); + // L1 controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - driveBase, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> Rotation2d.kZero)); - - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(driveBase::stopWithX, driveBase)); - - // Reset gyro to 0° when B button is pressed + .povLeft() + .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); + // L2 controller - .b() + .povUp() + .onTrue( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)), + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25))); + + // L3 + controller + .povRight() + .onTrue( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)), + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25))); + + // Intake + controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); + + controller + .rightTrigger() + .onTrue( + Commands.either( + dispenserBase + .eject(elevatorBase::getGoal) + .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))), + IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), + controller.leftTrigger().negate().debounce(0.25))); + + // Home Elevator + controller + .back() + .and(controller.start().negate()) + .debounce(0.5) + .onTrue(elevatorBase.homingSequence()); + + // Auto Align (Left or Right) + // TODO + + // Human Player Alert (Strobe LEDs) + // TODO + + // Reset Gyro + controller + .start() + .and(controller.back()) + .debounce(0.5) .onTrue( Commands.runOnce( () -> - RobotState.getInstance() + PoseEstimator.getInstance() .resetPose( new Pose2d( - RobotState.getInstance().getEstimatedPose().getTranslation(), + PoseEstimator.getInstance().getEstimatedPose().getTranslation(), AllianceFlipUtil.apply(new Rotation2d()))), driveBase) .ignoringDisable(true)); + + // Endgame + new Trigger( + () -> + DriverStation.isTeleopEnabled() + && DriverStation.getMatchTime() > 0 + && DriverStation.getMatchTime() <= Math.round(endgameAlert1.get())) + .onTrue( + Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0)) + .withTimeout(0.5)); + + new Trigger( + () -> + DriverStation.isTeleopEnabled() + && DriverStation.getMatchTime() > 0 + && DriverStation.getMatchTime() <= Math.round(endgameAlert2.get())) + .onTrue( + Commands.startEnd( + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0), + () -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0)) + .withTimeout(0.5)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AutoRoutine.java b/src/main/java/frc/robot/commands/AutoRoutine.java new file mode 100644 index 0000000..f515ed2 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoRoutine.java @@ -0,0 +1,3 @@ +package frc.robot.commands; + +public class AutoRoutine {} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 26acb14..8d04b47 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -2,24 +2,16 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.RobotState; +import frc.robot.PoseEstimator; import frc.robot.subsystems.drive.DriveBase; -import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.LoggedTunableNumber; -import java.text.DecimalFormat; -import java.text.NumberFormat; -import java.util.LinkedList; -import java.util.List; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -27,22 +19,18 @@ public class DriveCommands { // Drive private static final double DEADBAND = 0.1; - private static final LoggedTunableNumber LINEAR_VELOCITY_SCALAR = - new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0, true); - private static final LoggedTunableNumber ANGULAR_VELOCITY_SCALAR = - new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0, true); + private static final LoggedTunableNumber teleopLinearScalar = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalar", 1.0); + private static final LoggedTunableNumber teleopLinearScalarSlowMode = + new LoggedTunableNumber("TeleopDrive/LinearVelocityScalarSprint", 0.5); + private static final LoggedTunableNumber teleopAngularScalar = + new LoggedTunableNumber("TeleopDrive/AngularVelocityScalar", 1.0); private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.4; private static final double ANGLE_MAX_VELOCITY = 8.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; - // Characterization - private static final double FF_START_DELAY = 2.0; // Secs - private static final double FF_RAMP_RATE = 0.85; // Volts/Sec - private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec - private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 - /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ @@ -50,7 +38,9 @@ public static Command joystickDrive( DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { + DoubleSupplier omegaSupplier, + BooleanSupplier slowSupplier, + BooleanSupplier robotRelativeSupplier) { return Commands.run( () -> { // Apply deadband @@ -64,18 +54,26 @@ public static Command joystickDrive( omega = Math.copySign(Math.pow(omega, 2), omega); // Generate robot relative speeds - double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); - double angularVelocityScalar = ANGULAR_VELOCITY_SCALAR.get(); + double linearVelocityScalar = + slowSupplier.getAsBoolean() + ? teleopLinearScalarSlowMode.get() + : teleopLinearScalar.get(); + double angularVelocityScalar = teleopAngularScalar.get(); + var speeds = new ChassisSpeeds( - x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - omega * DriveConstants.maxAngularVelocityRadPerSec * angularVelocityScalar); + x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + y * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + omega * DriveBase.getMaxAngularVelocityRadPerSec() * angularVelocityScalar); // Convert to field relative - Rotation2d rotation = RobotState.getInstance().getRotation(); - rotation = AllianceFlipUtil.apply(rotation); - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); + if (!robotRelativeSupplier.getAsBoolean()) { + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); + } // Apply speeds driveBase.runVelocity(speeds); @@ -92,7 +90,8 @@ public static Command joystickDriveAtAngle( DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - Supplier rotationSupplier) { + Supplier rotationSupplier, + BooleanSupplier slowSupplier) { ProfiledPIDController angleController = new ProfiledPIDController( ANGLE_KP, @@ -112,21 +111,26 @@ public static Command joystickDriveAtAngle( y = Math.copySign(Math.pow(y, 2), y); // Calculate angular speed - Rotation2d rotation = RobotState.getInstance().getRotation(); + Rotation2d rotation = PoseEstimator.getInstance().getRotation(); double omega = angleController.calculate( rotation.getRadians(), rotationSupplier.get().getRadians()); // Generate robot relative speeds - double linearVelocityScalar = LINEAR_VELOCITY_SCALAR.get(); + double linearVelocityScalar = + slowSupplier.getAsBoolean() + ? teleopLinearScalarSlowMode.get() + : teleopLinearScalar.get(); var speeds = new ChassisSpeeds( - x * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, - y * DriveConstants.maxLinearVelocityMetersPerSec * linearVelocityScalar, + x * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, + y * DriveBase.getMaxLinearVelocityMetersPerSecond() * linearVelocityScalar, omega); // Convert to field relative - rotation = AllianceFlipUtil.apply(rotation); + if (AllianceFlipUtil.shouldFlip()) { + rotation = rotation.rotateBy(Rotation2d.kPi); + } speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, rotation); // Apply speeds @@ -134,143 +138,6 @@ public static Command joystickDriveAtAngle( }, driveBase) .beforeStarting( - () -> angleController.reset(RobotState.getInstance().getRotation().getRadians())); - } - - /** - * Measures the velocity feedforward constants for the drive motors. - * - *

This command should only be used in voltage control mode. - */ - public static Command feedforwardCharacterization(DriveBase drive) { - List velocitySamples = new LinkedList<>(); - List voltageSamples = new LinkedList<>(); - Timer timer = new Timer(); - - return Commands.sequence( - // Reset data - Commands.runOnce( - () -> { - velocitySamples.clear(); - voltageSamples.clear(); - }), - - // Allow modules to orient - Commands.run( - () -> { - drive.runCharacterization(0.0); - }, - drive) - .withTimeout(FF_START_DELAY), - - // Start timer - Commands.runOnce(timer::restart), - - // Accelerate and gather data - Commands.run( - () -> { - double voltage = timer.get() * FF_RAMP_RATE; - drive.runCharacterization(voltage); - velocitySamples.add(drive.getFFCharacterizationVelocity()); - voltageSamples.add(voltage); - }, - drive) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - int n = velocitySamples.size(); - double sumX = 0.0; - double sumY = 0.0; - double sumXY = 0.0; - double sumX2 = 0.0; - for (int i = 0; i < n; i++) { - sumX += velocitySamples.get(i); - sumY += voltageSamples.get(i); - sumXY += velocitySamples.get(i) * voltageSamples.get(i); - sumX2 += velocitySamples.get(i) * velocitySamples.get(i); - } - double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); - double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); - - NumberFormat formatter = new DecimalFormat("#0.00000"); - SmartDashboard.putString("kS", formatter.format(kS)); - SmartDashboard.putString("kV", formatter.format(kV)); - })); - } - - /** Measures the robot's wheel radius by spinning in a circle. */ - public static Command wheelRadiusCharacterization(DriveBase drive) { - SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); - WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); - - return Commands.parallel( - // Drive control sequence - Commands.sequence( - // Reset acceleration limiter - Commands.runOnce( - () -> { - limiter.reset(0.0); - }), - - // Turn in place, accelerating up to full speed - Commands.run( - () -> { - double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); - drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); - }, - drive)), - - // Measurement sequence - Commands.sequence( - // Wait for modules to fully orient before starting measurement - Commands.waitSeconds(1.0), - - // Record starting measurement - Commands.runOnce( - () -> { - state.positions = drive.getWheelRadiusCharacterizationPositions(); - state.lastAngle = drive.getGyroRotation(); - state.gyroDelta = 0.0; - }), - - // Update gyro delta - Commands.run( - () -> { - var rotation = drive.getGyroRotation(); - state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); - state.lastAngle = rotation; - }) - - // When cancelled, calculate and print results - .finallyDo( - () -> { - double[] positions = drive.getWheelRadiusCharacterizationPositions(); - double wheelDelta = 0.0; - for (int i = 0; i < 4; i++) { - wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; - } - double wheelRadius = - (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; - - NumberFormat formatter = new DecimalFormat("#0.000"); - - SmartDashboard.putString( - "Wheel Delta", formatter.format(wheelDelta) + " radians"); - SmartDashboard.putString( - "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); - SmartDashboard.putString( - "Wheel Radius", - formatter.format(wheelRadius) - + " meters, " - + formatter.format(Units.metersToInches(wheelRadius)) - + " inches"); - }))); - } - - private static class WheelRadiusCharacterizationState { - double[] positions = new double[4]; - Rotation2d lastAngle = new Rotation2d(); - double gyroDelta = 0.0; + () -> angleController.reset(PoseEstimator.getInstance().getRotation().getRadians())); } } diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..68d5061 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,33 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.dispenser.DispenserBase; +import frc.robot.subsystems.elevator.ElevatorBase; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.util.LoggedTunableNumber; + +public class IntakeCommands { + public static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Intake/HopperIntakeVolts", 5.5); + + public static Command intake(ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { + return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) + .andThen( + Commands.waitUntil(elevator::isAtGoal) + .andThen( + Commands.deadline( + dispenser.intakeTillHolding(), intake.runRoller(intakeVolts.get())))) + .finallyDo(() -> elevator.setGoal(ElevatorState.STOW)); + } + + public static Command reserialize( + ElevatorBase elevator, IntakeBase intake, DispenserBase dispenser) { + return Commands.runOnce(() -> elevator.setGoal(ElevatorState.INTAKE)) + .andThen( + Commands.waitUntil(elevator::isAtGoal) + .andThen(dispenser.runRollers(-2.0).until(() -> !dispenser.isHoldingCoral())) + .andThen(IntakeCommands.intake(elevator, intake, dispenser))); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java new file mode 100644 index 0000000..e81694f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserBase.java @@ -0,0 +1,78 @@ +package frc.robot.subsystems.dispenser; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.util.Debouncer; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.Supplier; +import lombok.Getter; +import org.littletonrobotics.junction.Logger; + +public class DispenserBase extends SubsystemBase { + private static final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Dispenser/IntakeVolts", 6.0); + private static final LoggedTunableNumber ejectVolts = + new LoggedTunableNumber("Dispenser/EjectVolts", 1.3); + private static final LoggedTunableNumber ejectVoltsSlow = + new LoggedTunableNumber("Dispenser/EjectVoltsSlow", 1.3); + + private static final LoggedTunableNumber holdingCoralPeriod = + new LoggedTunableNumber("Dispenser/HoldingCoralPeriodSecs", 0.5); + + private static final LoggedTunableNumber ejectPeriod = + new LoggedTunableNumber("Dispenser/EjectPeriodSecs", 0.75); + + private final DispenserIO io; + private final DispenserIOInputsAutoLogged inputs = new DispenserIOInputsAutoLogged(); + + @Getter private boolean holdingCoral; + + private final Debouncer holdingCoralDebouncer = new Debouncer(0); + + private final Alert disconnected = + new Alert("Dispenser motor disconnected!", Alert.AlertType.kWarning); + + public DispenserBase(DispenserIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Dispenser", inputs); + + disconnected.set(!inputs.connected); + + if (holdingCoralPeriod.hasChanged()) { + holdingCoralDebouncer.setDebounceTime(holdingCoralPeriod.get()); + } + + // Update if holding coral. + holdingCoral = holdingCoralDebouncer.calculate(inputs.rearBeamBreakBroken); + } + + public Command runRollers(double inputVolts) { + return startEnd(() -> io.runVolts(inputVolts), io::stop); + } + + public Command intakeTillHolding() { + return startEnd(() -> io.runVolts(intakeVolts.get()), io::stop) + .raceWith( + Commands.sequence( + Commands.waitSeconds(0.25), Commands.waitUntil(this::isHoldingCoral))); + } + + public Command eject(Supplier state) { + return startEnd( + () -> + io.runVolts( + state.get() == ElevatorState.L1_CORAL + ? ejectVoltsSlow.get() + : ejectVolts.get()), + io::stop) + .withTimeout(ejectPeriod.get()); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java new file mode 100644 index 0000000..df11973 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.dispenser; + +class DispenserConstants { + public static final boolean inverted = true; + public static final double moi = 0.025; // TODO + public static final double gearing = 34.0 / 24.0; +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java new file mode 100644 index 0000000..ff8b74a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIO.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.dispenser; + +import org.littletonrobotics.junction.AutoLog; + +public interface DispenserIO { + @AutoLog + class DispenserIOInputs { + public boolean connected = false; + + public double positionRads; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + + public boolean rearBeamBreakBroken = false; + } + + default void updateInputs(DispenserIOInputs inputs) {} + + default void runVolts(double output) {} + + default void stop() {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java new file mode 100644 index 0000000..efcf3a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSim.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.dispenser; + +import static frc.robot.subsystems.dispenser.DispenserConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class DispenserIOSim implements DispenserIO { + private final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); + + private double appliedVoltage = 0.0; + + @Override + public void updateInputs(DispenserIOInputs inputs) { + sim.update(Constants.kLoopPeriodSecs); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java new file mode 100644 index 0000000..8a689b6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.dispenser; + +import static frc.robot.subsystems.dispenser.DispenserConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.util.Debouncer; + +public class DispenserIOSpark implements DispenserIO { + private final SparkBase spark; + private final RelativeEncoder encoder; + + private final Debouncer connectedDebouncer = new Debouncer(.5); + + // End Dispenser beam break + private final DigitalInput rearBeamBreak = new DigitalInput(0); + + public DispenserIOSpark() { + spark = new SparkMax(14, MotorType.kBrushless); + encoder = spark.getEncoder(); + + var config = new SparkMaxConfig(); + config + .inverted(inverted) + .idleMode(SparkBaseConfig.IdleMode.kBrake) + .smartCurrentLimit(40, 50) + .voltageCompensation(12.0); + + config + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + config + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + spark.configure( + config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(DispenserIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadsPerSec = encoder.getVelocity(); + inputs.appliedVoltage = spark.getAppliedOutput() * spark.getBusVoltage(); + inputs.currentAmps = spark.getOutputCurrent(); + inputs.tempCelsius = spark.getMotorTemperature(); + + inputs.rearBeamBreakBroken = !rearBeamBreak.get(); + + inputs.connected = connectedDebouncer.calculate(!spark.hasActiveFault()); + } + + @Override + public void runVolts(double output) { + spark.setVoltage(output); + } + + @Override + public void stop() { + spark.stopMotor(); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode( + enabled ? SparkBaseConfig.IdleMode.kBrake : SparkBaseConfig.IdleMode.kCoast); + + spark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index b038147..459c43d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,23 +1,42 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.RobotState; +import frc.robot.PoseEstimator; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.swerve.SwerveSetpointGenerator; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.Queue; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class DriveBase extends SubsystemBase { + // Characterization + private static final double FF_START_DELAY = 2; // Secs + private static final double FF_RAMP_RATE = 3.5; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + private final GyroIO gyroIO; private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); @@ -57,6 +76,16 @@ public class DriveBase extends SubsystemBase { @AutoLogOutput(key = "Drive/BrakeModeEnabled") private boolean BRAKE_MODE = true; + private final SysIdRoutine sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + public DriveBase( GyroIO gyroIO, ModuleIO flModuleIO, @@ -126,7 +155,7 @@ public void periodic() { for (int j = 0; j < 4; j++) { wheelPositions[j] = modules[j].getOdometryPositions()[i]; } - RobotState.getInstance() + PoseEstimator.getInstance() .addOdometryObservation( wheelPositions, m_gyroInputs.connected ? m_gyroInputs.odometryYawPositions[i] : null, @@ -154,8 +183,7 @@ public void periodic() { } // Update gyro alert - gyroDisconnectedAlert.set( - !m_gyroInputs.connected && Constants.getRobotMode() != Constants.RobotMode.SIM); + gyroDisconnectedAlert.set(!m_gyroInputs.connected && Constants.getMode() != Constants.Mode.SIM); } /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */ @@ -191,6 +219,8 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsUnoptimized", setpointStatesUnoptimized); Logger.recordOutput("SwerveStates/Setpoints", setpointStates); Logger.recordOutput("SwerveChassisSpeeds/Setpoints", currentSetpoint.chassisSpeeds()); + Logger.recordOutput( + "SwerveChassisSpeeds/SetpointsUnoptimized", currentSetpoint.chassisSpeeds()); // Send setpoints to modules for (int i = 0; i < 4; i++) { @@ -198,6 +228,8 @@ public void runVelocity(ChassisSpeeds speeds) { } } + // CAN CONNECTOR ON CANID 6 MUST BE REPLACED BEFORE COMP + /** Runs the drive in a straight(ish) line with the specified drive output. */ public void runCharacterization(double output) { CLOSED_LOOP_MODE = false; @@ -259,7 +291,7 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + /** Returns the average velocity of the modules in rad/sec. */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { @@ -272,4 +304,165 @@ public double getFFCharacterizationVelocity() { public Rotation2d getGyroRotation() { return m_gyroInputs.yawPosition; } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public Command feedforwardCharacterization() { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + this.runCharacterization(0.0); + }, + this) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + this.runCharacterization(voltage); + velocitySamples.add(this.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + this) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + SmartDashboard.putString("kS", formatter.format(kS)); + SmartDashboard.putString("kV", formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public Command wheelRadiusCharacterization() { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + this.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + this)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = this.getWheelRadiusCharacterizationPositions(); + state.lastAngle = this.getGyroRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = this.getGyroRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + + SmartDashboard.putString( + "Wheel Delta", formatter.format(wheelDelta) + " radians"); + SmartDashboard.putString( + "Gyro Delta", formatter.format(state.gyroDelta) + " radians"); + SmartDashboard.putString( + "Wheel Radius", + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } + + public static double getMaxLinearVelocityMetersPerSecond() { + return DriveConstants.maxLinearVelocityMetersPerSec; + } + + public static double getMaxAngularVelocityRadPerSec() { + return DriveConstants.maxAngularVelocityRadPerSec; + } + + public static Translation2d[] getModuleTranslations() { + return DriveConstants.moduleTranslations; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 170e10f..0f12398 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -7,9 +7,9 @@ import frc.robot.util.swerve.SwerveSetpointGenerator.ModuleLimits; import lombok.Builder; -public class DriveConstants { +class DriveConstants { public static final double odometryFrequencyHz = - Constants.getRobotMode() == Constants.RobotMode.SIM ? 50 : 250; + Constants.getMode() == Constants.Mode.SIM ? 50 : 250; public static final double trackWidthX = Units.inchesToMeters(20.75); public static final double trackWidthY = Units.inchesToMeters(20.75); @@ -38,8 +38,8 @@ public class DriveConstants { ModuleConfig.builder() .turnMotorId(2) .driveMotorId(3) - .encoderChannel(2) - .encoderOffset(Rotation2d.fromRadians(0.16028737150729522)) + .encoderChannel(0) + .encoderOffset(Rotation2d.fromRadians(1.7182357115138978).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -48,8 +48,8 @@ public class DriveConstants { ModuleConfig.builder() .turnMotorId(4) .driveMotorId(5) - .encoderChannel(3) - .encoderOffset(Rotation2d.fromRadians(-0.1422097592800296)) + .encoderChannel(1) + .encoderOffset(Rotation2d.fromRadians(-1.4361935561244243).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -58,8 +58,8 @@ public class DriveConstants { ModuleConfig.builder() .turnMotorId(6) .driveMotorId(7) - .encoderChannel(1) - .encoderOffset(Rotation2d.fromRadians(-3.009554996093968)) + .encoderChannel(2) + .encoderOffset(Rotation2d.fromRadians(0.9998617084472845).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) @@ -68,8 +68,8 @@ public class DriveConstants { ModuleConfig.builder() .turnMotorId(8) .driveMotorId(9) - .encoderChannel(0) - .encoderOffset(Rotation2d.fromRadians(2.559973505647124)) + .encoderChannel(3) + .encoderOffset(Rotation2d.fromRadians(-1.7285578862473199).rotateBy(Rotation2d.kPi)) .driveGearing(mk4iDriveGearing) .turnGearing(mk4iTurnGearing) .turnInverted(true) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 13d1284..3a9f3d5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -3,7 +3,6 @@ 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 edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import frc.robot.Constants; @@ -11,34 +10,41 @@ import lombok.Getter; import org.littletonrobotics.junction.Logger; -public class Module { +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 drivekI = + new LoggedTunableNumber("Drive/Module/DrivekI"); private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("Drive/Module/DrivekD"); private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); + private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI"); private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); static { - switch (Constants.getRobotType()) { - case ROBOT_2025_COMP -> { - drivekS.initDefault(0.19700); - drivekV.initDefault(0.12941); - drivekP.initDefault(0.005); + switch (Constants.getRobot()) { + case COMPBOT -> { + drivekS.initDefault(0.69641); + drivekV.initDefault(0.12647); + drivekP.initDefault(0.0); + drivekI.initDefault(0.0); drivekD.initDefault(0.0); - turnkP.initDefault(2.0); - turnkD.initDefault(0.05); + turnkP.initDefault(1.5); + turnkI.initDefault(0.0); + turnkD.initDefault(0.0); } default -> { - drivekS.initDefault(0.11400); - drivekV.initDefault(0.84144); + drivekS.initDefault(0.113190); + drivekV.initDefault(0.841640); drivekP.initDefault(0.1); + drivekI.initDefault(0.0); drivekD.initDefault(0.0); turnkP.initDefault(10.0); + turnkI.initDefault(0.0); turnkD.initDefault(0.0); } } @@ -70,15 +76,22 @@ public void updateInputs() { 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()); - } + LoggedTunableNumber.ifChanged( + hashCode(), () -> m_io.setDriveFF(drivekS.get(), drivekV.get()), true, drivekS, drivekV); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> m_io.setDrivePID(drivekP.get(), drivekI.get(), drivekD.get()), + true, + drivekP, + drivekI, + drivekD); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> m_io.setTurnPID(turnkP.get(), turnkI.get(), turnkD.get()), + true, + turnkP, + turnkI, + turnkD); // Update Odometry Positions int sampleCount = m_inputs.odometryDrivePositionsRad.length; @@ -141,9 +154,9 @@ public double getWheelRadiusCharacterizationPosition() { return m_inputs.drivePositionRad; } - /** Returns the module velocity in rotations/sec (Phoenix native units). */ + /** Returns the module velocity in rad/sec. */ public double getFFCharacterizationVelocity() { - return Units.radiansToRotations(m_inputs.driveVelocityRadPerSec); + return m_inputs.driveVelocityRadPerSec; } /* Sets brake mode to {@code enabled} */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 6739091..bf0eddb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -11,6 +11,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; + public double driveTempCelsius = 0.0; public boolean turnConnected = false; public Rotation2d turnAbsolutePosition = new Rotation2d(); @@ -18,6 +19,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; + public double turnTempCelsius = 0.0; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 5719ed6..d40eab4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -11,8 +11,8 @@ import java.util.Queue; public class ModuleIOSim implements ModuleIO { - private static final DCMotor driveMotorModel = DCMotor.getNEO(1); - private static final DCMotor turnMotorModel = DCMotor.getNEO(1); + private final DCMotor driveMotorModel = DCMotor.getNEO(1); + private final DCMotor turnMotorModel = DCMotor.getNEO(1); private final DCMotorSim driveSim = new DCMotorSim( @@ -29,7 +29,7 @@ public class ModuleIOSim implements ModuleIO { private final PIDController driveController = new PIDController(0, 0, 0); private final PIDController turnController = new PIDController(0, 0, 0); - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); private double driveAppliedVolts = 0.0; private double driveFFVolts = 0.0; @@ -125,7 +125,8 @@ public void setDrivePID(double kP, double kI, double kD) { @Override public void setDriveFF(double kS, double kV) { - driveFeedforward = new SimpleMotorFeedforward(kS, kV); + driveFeedforward.setKs(kS); + driveFeedforward.setKv(kV); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index a96e3ff..24414cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -16,10 +16,10 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.AnalogEncoder; import frc.robot.Constants; +import frc.robot.util.Debouncer; import java.util.Queue; public class ModuleIOSpark implements ModuleIO { @@ -35,7 +35,7 @@ public class ModuleIOSpark implements ModuleIO { // Closed loop controllers private final SparkClosedLoopController driveController; private final SparkClosedLoopController turnController; - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); + private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0, 0); // Queue inputs from odometry thread private final Queue drivePositionQueue; @@ -46,13 +46,13 @@ public class ModuleIOSpark implements ModuleIO { private final Debouncer turnConnectedDebounce = new Debouncer(0.5); public ModuleIOSpark(int index) { - switch (Constants.getRobotType()) { - case ROBOT_2025_COMP -> { + switch (Constants.getRobot()) { + case COMPBOT -> { config = moduleConfigs[index]; } default -> throw new IllegalStateException( - "Unexpected RobotType for Spark Module: " + Constants.getRobotType()); + "Unexpected RobotType for Spark Module: " + Constants.getRobot()); } // Initialize Hardware Devices @@ -84,10 +84,12 @@ public ModuleIOSpark(int index) { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); driveSpark.configure( driveConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + driveEncoder.setPosition(0.0); // Configure Turn var turnConfig = new SparkMaxConfig(); @@ -116,7 +118,8 @@ public ModuleIOSpark(int index) { .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); turnSpark.configure(turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -134,12 +137,14 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); inputs.driveAppliedVolts = driveSpark.getAppliedOutput() * driveSpark.getBusVoltage(); inputs.driveCurrentAmps = driveSpark.getOutputCurrent(); + inputs.driveTempCelsius = driveSpark.getMotorTemperature(); inputs.turnAbsolutePosition = getOffsetAbsoluteAngle(); inputs.turnPosition = Rotation2d.fromRadians(turnEncoder.getPosition()); inputs.turnVelocityRadPerSec = turnEncoder.getVelocity(); inputs.turnAppliedVolts = turnSpark.getAppliedOutput() * turnSpark.getBusVoltage(); inputs.turnCurrentAmps = turnSpark.getOutputCurrent(); + inputs.turnTempCelsius = turnSpark.getMotorTemperature(); inputs.driveConnected = driveConnectedDebounce.calculate(!driveSpark.hasActiveFault()); inputs.turnConnected = turnConnectedDebounce.calculate(!turnSpark.hasActiveFault()); @@ -191,7 +196,8 @@ public void setDrivePID(double kP, double kI, double kD) { @Override public void setDriveFF(double kS, double kV) { - driveFeedforward = new SimpleMotorFeedforward(kS, kV); + driveFeedforward.setKs(kS); + driveFeedforward.setKv(kV); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java index bd94521..90389dd 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryManager.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -12,7 +12,7 @@ import lombok.Getter; import org.littletonrobotics.junction.AutoLog; -public class OdometryManager implements AutoCloseable { +class OdometryManager implements AutoCloseable { public static Lock odometryLock = new ReentrantLock(); // Prevent conflicts when reading and writing data diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java new file mode 100644 index 0000000..5681691 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -0,0 +1,296 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.util.Debouncer; +import frc.robot.util.EqualsUtil; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class ElevatorBase extends SubsystemBase { + // Tunable numbers + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/kI"); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD"); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); + private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Elevator/kA"); + + private static final LoggedTunableNumber maxVelocityMetersPerSec = + new LoggedTunableNumber("Elevator/MaxVelocityMetersPerSec", 2.0); + private static final LoggedTunableNumber maxAccelerationMetersPerSec2 = + new LoggedTunableNumber("Elevator/MaxAccelerationMetersPerSec2", 10); + + private static final LoggedTunableNumber homingVolts = + new LoggedTunableNumber("Elevator/HomingVolts", -2.0); + private static final LoggedTunableNumber homingTimeSecs = + new LoggedTunableNumber("Elevator/HomingTimeSecs", 0.25); + private static final LoggedTunableNumber homingVelocityThresh = + new LoggedTunableNumber("Elevator/HomingVelocityThresh", 5.0); + + private static final LoggedTunableNumber toleranceMeters = + new LoggedTunableNumber("Elevator/ToleranceMeters", 0.2); + + static { + switch (Constants.getRobot()) { + case COMPBOT -> { + kP.initDefault(0.3); + kI.initDefault(0.0); + kD.initDefault(0.25); + kS.initDefault(0); + kG.initDefault(1.05); + kA.initDefault(0.0); + } + case SIMBOT -> { + kP.initDefault(0); // TODO + kI.initDefault(0.0); // TODO + kD.initDefault(0); // TODO + kS.initDefault(0); // TODO + kG.initDefault(0); // TODO + kA.initDefault(0); // TODO + } + } + } + + private final ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + private final Alert motorDisconnectedAlert = + new Alert("Elevator leader motor disconnected!", Alert.AlertType.kWarning); + private final Alert followerDisconnectedAlert = + new Alert("Elevator follower motor disconnected!", Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Goal") + @Getter + @Setter + private ElevatorState goal = ElevatorState.START; + + private TrapezoidProfile profile; + private State setpoint = new State(); + + private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.0, 0.0); + + private boolean profileDisabled = false; + @Setter private boolean eStopped = false; + + @AutoLogOutput(key = "Elevator/Homed") + @Getter + private boolean homed = false; + + private final Debouncer toleranceDebouncer = new Debouncer(0.25, Debouncer.DebounceType.kRising); + private final Alert outOfTolleranceAlert = + new Alert( + "Elevator emergency disabled due to high position error. Rehome the elevator to reset.", + Alert.AlertType.kWarning); + + @AutoLogOutput(key = "Elevator/Profile/AtGoal") + @Getter + private boolean atGoal = false; + + private final ElevatorVisualizer measuredVisualizer = new ElevatorVisualizer("Measured"); + private final ElevatorVisualizer setpointVisualizer = new ElevatorVisualizer("Setpoint"); + + private final SysIdRoutine sysId; + + public ElevatorBase(ElevatorIO io) { + this.io = io; + + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())); + + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(.1).per(Second), + Volts.of(4), + Seconds.of( + 45), // Effectively disable the timeout and allow the Command factories to set + // them + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> io.runOpenLoop(voltage.in(Volts)), + null, // No log consumer, since data is recorded by AdvantageKit + this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + + motorDisconnectedAlert.set(!inputs.leaderConnected); + followerDisconnectedAlert.set(!inputs.followerConnected); + + // Update tunable numbers + LoggedTunableNumber.ifChanged(() -> io.setPID(kP.get(), kI.get(), kD.get()), true, kP, kI, kD); + LoggedTunableNumber.ifChanged( + () -> { + feedforward.setKs(kS.get()); + feedforward.setKg(kG.get()); + feedforward.setKa(kA.get()); + }, + true, + kS, + kG, + kA); + + LoggedTunableNumber.ifChanged( + () -> + profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + maxVelocityMetersPerSec.get(), maxAccelerationMetersPerSec2.get())), + true, + maxVelocityMetersPerSec, + maxAccelerationMetersPerSec2); + + // Run profile + final boolean shouldRunProfile = + !profileDisabled && homed && !eStopped && DriverStation.isEnabled(); + Logger.recordOutput("Elevator/RunningProfile", shouldRunProfile); + + // // Check if out of tolerance + // boolean outOfTolerance = + // Math.abs(getPositionMeters() - setpoint.position) > toleranceMeters.get(); + // boolean shouldEStop = toleranceDebouncer.calculate(outOfTolerance && shouldRunProfile); + // + // outOfTolleranceAlert.set(shouldEStop); + // if (shouldEStop) { + // eStopped = true; + // } + + if (shouldRunProfile) { + // Clamp goal + var goalState = + new State( + MathUtil.clamp(goal.getElevatorHeightMeters().getAsDouble(), 0.0, maxTravel), 0); + + double previousVelocity = setpoint.velocity; + setpoint = profile.calculate(Constants.kLoopPeriodSecs, setpoint, goalState); + if (setpoint.position < 0.0 || setpoint.position > maxTravel) { + setpoint = new State(MathUtil.clamp(setpoint.position, 0.0, maxTravel), 0.0); + } + + io.runPosition( + setpoint.position / drumRadius / numStages, + feedforward.calculateWithVelocities(setpoint.velocity, previousVelocity)); + + // Check at goal + atGoal = + EqualsUtil.epsilonEquals(setpoint.position, goalState.position) + && EqualsUtil.epsilonEquals(setpoint.velocity, goalState.velocity); + + // Stop if at the bottom + if (atGoal && EqualsUtil.epsilonEquals(setpoint.position, 0.0)) { + io.stop(); + } + + // Log state + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", setpoint.position); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", setpoint.velocity); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", goalState.position); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", goalState.velocity); + } else { + if (DriverStation.isDisabled()) { + goal = ElevatorState.STOW; + } + + // Reset setpoint + setpoint = new State(getPositionMeters(), 0.0); + + // Clear logs + Logger.recordOutput("Elevator/Profile/SetpointPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/SetpointVelocityMetersPerSec", 0.0); + Logger.recordOutput("Elevator/Profile/GoalPositionMeters", 0.0); + Logger.recordOutput("Elevator/Profile/GoalVelocityMetersPerSec", 0.0); + } + + if (eStopped) { + io.stop(); + } + + Logger.recordOutput( + "Elevator/MeasuredVelocityMetersPerSec", inputs.velocityRadPerSec * drumRadius); + + // If not homed, schedule that command + if (!homed && !profileDisabled) { + homingSequence().schedule(); + } + + measuredVisualizer.update(getPositionMeters()); + setpointVisualizer.update(setpoint.position); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.quasistatic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction, double timeoutSecs) { + return runOnce( + () -> { + profileDisabled = true; + io.stop(); + }) + .andThen(Commands.waitSeconds(1)) + .andThen(sysId.dynamic(direction).withTimeout(timeoutSecs)) + .finallyDo(() -> profileDisabled = false); + } + + public Command homingSequence() { + var homingDebouncer = new Debouncer(homingTimeSecs.get()); + return Commands.startRun( + () -> { + profileDisabled = true; + homed = false; + homingDebouncer.calculate(false); + }, + () -> { + io.runOpenLoop(homingVolts.get()); + homed = + homingDebouncer.calculate( + Math.abs(inputs.velocityRadPerSec) <= homingVelocityThresh.get()); + }) + .until(() -> homed) + .andThen( + () -> { + io.resetOrigin(); + homed = true; + }) + .finallyDo(() -> profileDisabled = false); + } + + @AutoLogOutput(key = "Elevator/MeasuredHeightMeters") + public double getPositionMeters() { + return inputs.positionRads * drumRadius * numStages; + } + + public double getGoalMeters() { + return goal.getElevatorHeightMeters().getAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..85c509a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class ElevatorConstants { + // Pitch from Floor to Elevator + public static final Rotation2d elevatorPitch = Rotation2d.fromDegrees(84.5); + // Pitch from Elevator to Dispenser + public static final Rotation2d dispenserPitch = Rotation2d.fromDegrees(73.0); + + public static final double originToBaseHeightMeters = Units.inchesToMeters(5.149922); + + public static final double drumRadius = Units.inchesToMeters(1.751 / 2.0); + public static final double gearing = 3.0; + public static final int numStages = 2; + + // public static final double maxTravel = Units.inchesToMeters(42.244094); // TODO + public static final double maxTravel = Units.inchesToMeters(42); // TODO + + public static final double carriageMassKg = Units.lbsToKilograms(6.0); + public static final double stagesMassKg = Units.lbsToKilograms(12.0); +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..aceb856 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + public class ElevatorIOInputs { + public boolean leaderConnected = false; + public boolean followerConnected = false; + + public double positionRads = 0.0; + public double velocityRadPerSec = 0.0; + public double[] appliedVolts = new double[] {}; + public double[] currentAmps = new double[] {}; + public double[] tempCelsius = new double[] {}; + } + + default void updateInputs(ElevatorIOInputs inputs) {} + + default void runOpenLoop(double output) {} + + default void runPosition(double positionRads, double feedforwardVolts) {} + + default void stop() {} + + default void resetOrigin() {} + + default void setPID(double kP, double kI, double kD) {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java new file mode 100644 index 0000000..e84b6a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import frc.robot.Constants; + +public class ElevatorIOSim implements ElevatorIO { + private final DCMotor elevatorMotorModel = DCMotor.getNEO(2); + private final ElevatorSim sim = + new ElevatorSim( + elevatorMotorModel, + gearing, + carriageMassKg + stagesMassKg, + drumRadius, + 0, + maxTravel, + true, + 0, + 0.01, + 0.0); + + private final PIDController controller = new PIDController(0, 0, 0); + + private double appliedVolts = 0.0; + private boolean closedLoop = false; + private double feedforward = 0.0; + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + if (!closedLoop) { + controller.reset(); + } else { + appliedVolts = controller.calculate(sim.getPositionMeters()) + feedforward; + } + + sim.setInputVoltage(appliedVolts); + sim.update(Constants.kLoopPeriodSecs); + + inputs.leaderConnected = true; + inputs.followerConnected = true; + + inputs.positionRads = sim.getPositionMeters() / drumRadius; + inputs.velocityRadPerSec = sim.getVelocityMetersPerSecond() / drumRadius; + + inputs.appliedVolts = new double[] {appliedVolts}; + inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + } + + @Override + public void runOpenLoop(double output) { + closedLoop = false; + appliedVolts = output; + } + + @Override + public void runPosition(double positionRads, double feedforwardVolts) { + closedLoop = true; + controller.setSetpoint(positionRads); + feedforward = feedforwardVolts; + } + + @Override + public void stop() { + runOpenLoop(0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java new file mode 100644 index 0000000..f028ec0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -0,0 +1,150 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.util.Debouncer; + +public class ElevatorIOSpark implements ElevatorIO { + private final SparkBase leaderSpark; + private final SparkBase followerSpark; + + private final RelativeEncoder encoder; + + private final SparkClosedLoopController controller; + + private final Debouncer leaderConnectedDebouncer = new Debouncer(0.5); + private final Debouncer followerConnectedDebouncer = new Debouncer(0.5); + + public ElevatorIOSpark() { + leaderSpark = new SparkMax(12, MotorType.kBrushless); + followerSpark = new SparkMax(13, MotorType.kBrushless); + + encoder = leaderSpark.getEncoder(); + controller = leaderSpark.getClosedLoopController(); + + var leaderConfig = new SparkMaxConfig(); + leaderConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(60).voltageCompensation(12.0); + leaderConfig + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + leaderConfig + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) + .pidf(0.0, 0.0, 0.0, 0.0); + leaderConfig + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + leaderSpark.configure( + leaderConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + + var followerConfig = new SparkMaxConfig(); + followerConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(60) + .voltageCompensation(12.0) + .follow(leaderSpark, true); + + followerConfig + .signals + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + followerSpark.configure( + followerConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadPerSec = encoder.getVelocity(); + + inputs.appliedVolts = + new double[] { + leaderSpark.getAppliedOutput() * leaderSpark.getBusVoltage(), + followerSpark.getAppliedOutput() * followerSpark.getBusVoltage() + }; + inputs.currentAmps = + new double[] {leaderSpark.getOutputCurrent(), followerSpark.getOutputCurrent()}; + inputs.tempCelsius = + new double[] {leaderSpark.getMotorTemperature(), followerSpark.getMotorTemperature()}; + + inputs.leaderConnected = leaderConnectedDebouncer.calculate(!leaderSpark.hasActiveFault()); + inputs.followerConnected = + followerConnectedDebouncer.calculate(!followerSpark.hasActiveFault()); + } + + @Override + public void runOpenLoop(double output) { + leaderSpark.setVoltage(output); + } + + @Override + public void runPosition(double positionRads, double feedforwardVolts) { + controller.setReference( + positionRads, + SparkBase.ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedforwardVolts, + SparkClosedLoopController.ArbFFUnits.kVoltage); + } + + @Override + public void stop() { + leaderSpark.stopMotor(); + } + + @Override + public void resetOrigin() { + encoder.setPosition(0.0); + } + + @Override + public void setPID(double kP, double kI, double kD) { + var PIDConfig = new SparkMaxConfig(); + PIDConfig.closedLoop.pid(kP, kI, kD); + + leaderSpark.configure( + PIDConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); + + leaderSpark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + followerSpark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java new file mode 100644 index 0000000..17655d3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.util.Units; +import frc.robot.FieldConstants.ReefLevel; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +@Getter +@RequiredArgsConstructor +public enum ElevatorState { + START(() -> 0.0), + STOW("Stow", 0.0), + INTAKE("Intake", 0.0), + L1_CORAL(ReefLevel.L1, Units.inchesToMeters(7), false), + L2_CORAL(ReefLevel.L2, Units.inchesToMeters(1), false), + L2_ALGAE_REMOVAL(ReefLevel.L2, Units.inchesToMeters(5), true), + L3_CORAL(ReefLevel.L3, Units.inchesToMeters(0.0), false), + L3_ALGAE_REMOVAL(ReefLevel.L3, Units.inchesToMeters(0), true), + L4_CORAL(ReefLevel.L4, Units.inchesToMeters(0.0), false); + + private final DoubleSupplier elevatorHeightMeters; + + ElevatorState(String name, double defaultValue) { + elevatorHeightMeters = new LoggedTunableNumber("Elevator/Presets/" + name, defaultValue); + } + + ElevatorState(ReefLevel reefLevel, double defaultOffset, boolean isAlgae) { + String stateName; + if (isAlgae) { + stateName = String.format("Elevator/Presets/%s_Algae Offset", reefLevel); + } else { + stateName = String.format("Elevator/Presets/%s Offset", reefLevel); + } + + var offsetTunable = new LoggedTunableNumber(stateName, defaultOffset); + elevatorHeightMeters = + () -> + (reefLevel.height + offsetTunable.get() - originToBaseHeightMeters) + / elevatorPitch.getSin(); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeBase.java b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java new file mode 100644 index 0000000..dcca0a9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class IntakeBase extends SubsystemBase { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + private final Alert disconnected = + new Alert("Intake motor disconnected!", Alert.AlertType.kWarning); + + public IntakeBase(IntakeIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + disconnected.set(!inputs.connected); + } + + public Command runRoller(double inputVolts) { + return startEnd(() -> io.runVolts(inputVolts), io::stop); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..5fe42dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.intake; + +class IntakeConstants { + public static final boolean inverted = true; + public static final double moi = 0.025; // TODO + public static final double gearing = 2.0; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..a4fde97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + class IntakeIOInputs { + public boolean connected = false; + + public double positionRads; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double currentAmps = 0.0; + public double tempCelsius = 0.0; + } + + default void updateInputs(IntakeIOInputs inputs) {} + + default void runVolts(double output) {} + + default void stop() {} + + default void setBrakeMode(boolean enabled) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..a31f234 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class IntakeIOSim implements IntakeIO { + private final DCMotor intakeMotorModel = DCMotor.getNEO(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(intakeMotorModel, moi, gearing), intakeMotorModel); + + private double appliedVoltage = 0.0; + + @Override + public void updateInputs(IntakeIOInputs inputs) { + sim.update(Constants.kLoopPeriodSecs); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java new file mode 100644 index 0000000..7ed2a2e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -0,0 +1,83 @@ +package frc.robot.subsystems.intake; + +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.util.Debouncer; + +public class IntakeIOSpark implements IntakeIO { + private final SparkBase spark; + private final RelativeEncoder encoder; + + private final Debouncer connectedDebouncer = new Debouncer(.5); + + public IntakeIOSpark() { + spark = new SparkMax(11, SparkLowLevel.MotorType.kBrushless); + encoder = spark.getEncoder(); + + var config = new SparkMaxConfig(); + config + .inverted(inverted) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(40, 50) + .voltageCompensation(12.0); + + config + .encoder + .positionConversionFactor(2 * Math.PI / gearing) + .velocityConversionFactor(2 * Math.PI / 60.0 / gearing) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + config + .signals + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20) + .primaryEncoderVelocityAlwaysOn(true) + .primaryEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20) + .motorTemperaturePeriodMs(20); + + spark.configure( + config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.positionRads = encoder.getPosition(); + inputs.velocityRadsPerSec = encoder.getVelocity(); + inputs.appliedVoltage = spark.getAppliedOutput() * spark.getBusVoltage(); + inputs.currentAmps = spark.getOutputCurrent(); + inputs.tempCelsius = spark.getMotorTemperature(); + + inputs.connected = connectedDebouncer.calculate(!spark.hasActiveFault()); + } + + @Override + public void runVolts(double output) { + spark.setVoltage(output); + } + + @Override + public void stop() { + spark.stopMotor(); + } + + @Override + public void setBrakeMode(boolean enabled) { + var brakeModeConfig = new SparkMaxConfig(); + brakeModeConfig.idleMode(enabled ? IdleMode.kBrake : IdleMode.kCoast); + + spark.configure( + brakeModeConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java new file mode 100644 index 0000000..4e1e513 --- /dev/null +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -0,0 +1,181 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.robot.Constants; + +// LED Alerts +// endgameAlert +// autoScoring (Reef Side denotes side, color denotes level) +// superstructureEstopped +// lowBatteryAlert +// visionDisconnected +// following trajectory +// go to pose +// feederstation alert (alert feeder station player about being OTW) + +// TODO indicate robot initializing in LEDs + +public class AlertsUtil { + private static final int numLeds = 120; + private static final double LOW_VOLTAGE_WARNING_THRESHOLD = 11.75; + + private static AlertsUtil instance; + + public static AlertsUtil getInstance() { + if (instance == null) { + instance = new AlertsUtil(); + } + + return instance; + } + + // System Alerts + private final Alert canErrorAlert = + new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + private final Debouncer canErrorDebouncer = new Debouncer(0.5); + private final Alert lowBatteryVoltageAlert = + new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); + private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); + + // Program Alerts + private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); + private final Alert ledsDisabledAlert = + new Alert("LEDs disabled by program override", AlertType.kInfo); + + private AddressableLED leds; + private AddressableLEDBuffer ledBuffer; + + private AlertsUtil() { + if (Constants.TUNING_MODE) { + tuningModeAlert.set(true); + } + + if (Constants.ENABLE_LEDs) { + leds = new AddressableLED(1); + ledBuffer = new AddressableLEDBuffer(numLeds); + + leds.setLength(ledBuffer.getLength()); + leds.start(); + } else { + ledsDisabledAlert.set(true); + } + } + + public void periodic() { + // Update System Alerts + // Check CAN status + var canStatus = RobotController.getCANStatus(); + canErrorAlert.set( + canErrorDebouncer.calculate( + canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0)); + + // Update Battery Voltage Alert + lowBatteryVoltageAlert.set( + batteryVoltageDebouncer.calculate( + RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + + // Update Program Alerts + // TODO + + // Update Hardware Alerts + if (leds == null) return; + + // // Disable LEDs if battery voltage is too low + // if(ledVoltageDebouncer.calculate( + // RobotController.getBatteryVoltage() <= LED_DISABLE_VOLTAGE_THRESHOLD)) { + // ledsDisabledAlert.set(true); + // ledLowVoltageDisabledAlert.set(true); + // + // leds.stop(); + // leds.close(); + // + // leds = null; + // ledBuffer = null; + // + // return; + } + + // private Color solid(Section section, Color color) { + // if (color != null) { + // for (int i = section.start(); i < section.end(); i++) { + // ledBuffer.setLED(i, color); + // } + // } + // return color; + // } + // + // private Color strobe(Section section, Color c1, Color c2, double duration) { + // boolean c1On = ((Timer.getTimestamp() % duration) / duration) > 0.5; + // return solid(section, c1On ? c1 : c2); + // } + // + // private Color breath(Section section, Color c1, Color c2, double duration, double timestamp) { + // double x = ((timestamp % duration) / duration) * 2.0 * Math.PI; + // double ratio = (Math.sin(x) + 1.0) / 2.0; + // double red = (c1.red * (1 - ratio)) + (c2.red * ratio); + // double green = (c1.green * (1 - ratio)) + (c2.green * ratio); + // double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio); + // var color = new Color(red, green, blue); + // solid(section, color); + // return color; + // } + // + // private Color breath(Section section, Color c1, Color c2, double duration) { + // return breath(section, c1, c2, duration, Timer.getTimestamp()); + // } + // + // private void rainbow(Section section, double cycleLength, double duration) { + // double x = (1 - ((Timer.getTimestamp() / duration) % 1.0)) * 180.0; + // double xDiffPerLed = 180.0 / cycleLength; + // for (int i = section.end() - 1; i >= section.start(); i--) { + // x += xDiffPerLed; + // x %= 180.0; + // ledBuffer.setHSV(i, (int) x, 255, 255); + // } + // } + // + // private void wave(Section section, Color c1, Color c2, double cycleLength, double duration) { + // double x = (1 - ((Timer.getTimestamp() % duration) / duration)) * 2.0 * Math.PI; + // double xDiffPerLed = (2.0 * Math.PI) / cycleLength; + // for (int i = section.end() - 1; i >= section.start(); i--) { + // x += xDiffPerLed; + // double ratio = (Math.pow(Math.sin(x), waveExponent) + 1.0) / 2.0; + // if (Double.isNaN(ratio)) { + // ratio = (-Math.pow(Math.sin(x + Math.PI), waveExponent) + 1.0) / 2.0; + // } + // if (Double.isNaN(ratio)) { + // ratio = 0.5; + // } + // double red = (c1.red * (1 - ratio)) + (c2.red * ratio); + // double green = (c1.green * (1 - ratio)) + (c2.green * ratio); + // double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio); + // ledBuffer.setLED(i, new Color(red, green, blue)); + // } + // } + // + // private void stripes(Section section, List colors, int stripeLength, double duration) { + // int offset = (int) (Timer.getTimestamp() % duration / duration * stripeLength * + // colors.size()); + // for (int i = section.end() - 1; i >= section.start(); i--) { + // int colorIndex = + // (int) (Math.floor((double) (i - offset) / stripeLength) + colors.size()) % + // colors.size(); + // colorIndex = colors.size() - 1 - colorIndex; + // ledBuffer.setLED(i, colors.get(colorIndex)); + // } + // } + + // LEDs have a base mode / pattern / effect + // alerts will update and override specific sections if something is active + // apply the buffer + + public static class HardwareIndicatedAlert extends Alert { + public HardwareIndicatedAlert( + String text, AlertType type, int priority /*TODO handle LED stuff*/) { + super(text, type); + } + } + + private static record Section(int start, int end) {} +} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java index f632cec..7c1e824 100644 --- a/src/main/java/frc/robot/util/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -26,15 +26,24 @@ private static Translation3d applyTranslation(Translation3d translation3d) { applyX(translation3d.getX()), translation3d.getY(), translation3d.getZ()); } - private static Rotation2d applyRotation(Rotation2d rotation) { - return new Rotation2d(-rotation.getCos(), rotation.getSin()); + private static Rotation2d applyRotation(Rotation2d rotation2d) { + return rotation2d.rotateBy(Rotation2d.kPi); + } + + private static Rotation3d applyRotation(Rotation3d rotation3d) { + return rotation3d.rotateBy(new Rotation3d(0.0, 0.0, Math.PI)); } private static Pose2d applyPose(Pose2d pose) { return new Pose2d(applyTranslation(pose.getTranslation()), applyRotation(pose.getRotation())); } - private static boolean shouldFlip() { + private static Pose3d applyPose(Pose3d pose3d) { + return new Pose3d( + applyTranslation(pose3d.getTranslation()), applyRotation(pose3d.getRotation())); + } + + public static boolean shouldFlip() { var currentAllianceOpt = DriverStation.getAlliance(); return currentAllianceOpt.isPresent() && currentAllianceOpt.get() == DriverStation.Alliance.Red; } @@ -43,20 +52,28 @@ public static double apply(double x) { return shouldFlip() ? applyX(x) : x; } - public static Translation2d apply(Translation2d translation) { - return shouldFlip() ? applyTranslation(translation) : translation; + public static Translation2d apply(Translation2d translation2d) { + return shouldFlip() ? applyTranslation(translation2d) : translation2d; } - public static Rotation2d apply(Rotation2d rotation) { - return shouldFlip() ? applyRotation(rotation) : rotation; + public static Translation3d apply(Translation3d translation3d) { + return shouldFlip() ? applyTranslation(translation3d) : translation3d; } - public static Pose2d apply(Pose2d pose) { - return shouldFlip() ? applyPose(pose) : pose; + public static Rotation2d apply(Rotation2d rotation2d) { + return shouldFlip() ? applyRotation(rotation2d) : rotation2d; } - public static Translation3d apply(Translation3d translation3d) { - return shouldFlip() ? applyTranslation(translation3d) : translation3d; + public static Rotation3d apply(Rotation3d rotation3d) { + return shouldFlip() ? applyRotation(rotation3d) : rotation3d; + } + + public static Pose2d apply(Pose2d pose2d) { + return shouldFlip() ? applyPose(pose2d) : pose2d; + } + + public static Pose3d apply(Pose3d pose3d) { + return shouldFlip() ? applyPose(pose3d) : pose3d; } public static class AllianceRelative { @@ -84,16 +101,24 @@ public static AllianceRelative from(Rotation2d value) { return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); } + public static AllianceRelative from(Rotation3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyRotation); + } + public static AllianceRelative from(Translation2d value) { return new AllianceRelative<>(value, AllianceFlipUtil::applyTranslation); } + public static AllianceRelative from(Translation3d 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); + public static AllianceRelative from(Pose3d value) { + return new AllianceRelative<>(value, AllianceFlipUtil::applyPose); } } } diff --git a/src/main/java/frc/robot/util/Debouncer.java b/src/main/java/frc/robot/util/Debouncer.java new file mode 100644 index 0000000..657a83a --- /dev/null +++ b/src/main/java/frc/robot/util/Debouncer.java @@ -0,0 +1,90 @@ +package frc.robot.util; + +import edu.wpi.first.math.MathSharedStore; + +/** + * A simple debounce filter for boolean streams. Requires that the boolean change value from + * baseline for a specified period of time before the filtered value changes. + */ +public class Debouncer { + /** Type of debouncing to perform. */ + public enum DebounceType { + /** Rising edge. */ + kRising, + /** Falling edge. */ + kFalling, + /** Both rising and falling edges. */ + kBoth + } + + private double m_debounceTimeSeconds; + private final DebounceType m_debounceType; + private boolean m_baseline; + + private double m_prevTimeSeconds; + + /** + * Creates a new Debouncer. + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + * @param type Which type of state change the debouncing will be performed on. + */ + public Debouncer(double debounceTime, DebounceType type) { + m_debounceTimeSeconds = debounceTime; + m_debounceType = type; + + resetTimer(); + + m_baseline = + switch (m_debounceType) { + case kBoth, kRising -> false; + case kFalling -> true; + }; + } + + /** + * Creates a new Debouncer. Baseline value defaulted to "false." + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + */ + public Debouncer(double debounceTime) { + this(debounceTime, DebounceType.kRising); + } + + private void resetTimer() { + m_prevTimeSeconds = MathSharedStore.getTimestamp(); + } + + private boolean hasElapsed() { + return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds; + } + + /** + * Applies the debouncer to the input stream. + * + * @param input The current value of the input stream. + * @return The debounced value of the input stream. + */ + public boolean calculate(boolean input) { + if (input == m_baseline) { + resetTimer(); + } + + if (hasElapsed()) { + if (m_debounceType == DebounceType.kBoth) { + m_baseline = input; + resetTimer(); + } + return input; + } else { + return m_baseline; + } + } + + public void setDebounceTime(double debounceTime) { + m_debounceTimeSeconds = debounceTime; + resetTimer(); + } +} diff --git a/src/main/java/frc/robot/util/EqualsUtil.java b/src/main/java/frc/robot/util/EqualsUtil.java index e839a5c..9dacb20 100644 --- a/src/main/java/frc/robot/util/EqualsUtil.java +++ b/src/main/java/frc/robot/util/EqualsUtil.java @@ -4,7 +4,7 @@ public class EqualsUtil { public static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); + return Math.abs(a - b) <= epsilon; } public static boolean epsilonEquals(double a, double b) { @@ -18,5 +18,11 @@ public static boolean epsilonEquals(Twist2d twist, Twist2d other) { && EqualsUtil.epsilonEquals(twist.dy, other.dy) && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta); } + + public static boolean equalsZero(Twist2d twist) { + return EqualsUtil.epsilonEquals(twist.dx, 0.0) + && EqualsUtil.epsilonEquals(twist.dy, 0.0) + && EqualsUtil.epsilonEquals(twist.dtheta, 0.0); + } } } diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 5d7bfe7..a265098 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -1,17 +1,17 @@ package frc.robot.util; import frc.robot.Constants; -import java.util.Arrays; import java.util.HashMap; import java.util.Map; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. */ -public class LoggedTunableNumber { - private static final String tableKey = "TunableNumbers"; +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/TunableNumbers"; private final String key; private Double defaultValue = null; @@ -20,38 +20,13 @@ public class LoggedTunableNumber { private LoggedNetworkNumber dashboardNumber; - private final boolean ntPubEnabled; - - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode - */ - public LoggedTunableNumber(String dashboardKey, boolean alwaysEnabled) { - this.key = tableKey + "/" + dashboardKey; - this.ntPubEnabled = alwaysEnabled; - } - /** * Create a new LoggedTunableNumber * * @param dashboardKey Key on dashboard */ public LoggedTunableNumber(String dashboardKey) { - this(dashboardKey, Constants.TUNING_MODE); - } - - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - * @param alwaysEnabled Always publish modifiers to NT, even if not in Tuning Mode - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue, boolean alwaysEnabled) { - this(dashboardKey, alwaysEnabled); - initDefault(defaultValue); + this.key = tableKey + "/" + dashboardKey; } /** @@ -80,7 +55,7 @@ public void initDefault(double defaultValue) { } this.defaultValue = defaultValue; - if (ntPubEnabled) { + if (Constants.TUNING_MODE) { dashboardNumber = new LoggedNetworkNumber(key, defaultValue); } } @@ -99,7 +74,7 @@ public double get() { key)); } - return ntPubEnabled ? dashboardNumber.get() : defaultValue; + return Constants.TUNING_MODE ? dashboardNumber.get() : defaultValue; } /** @@ -133,15 +108,40 @@ public boolean hasChanged() { return hasChanged(0); } + public void resetLastValue(int id) { + lastValues.put(id, get()); + } + + public void resetLastValue() { + resetLastValue(0); + } + /** * Run callback if any tunable number has changed. See {@link #hasChanged(int)} for usage. * * @param action action to run + * @param resetAll if true and any TunableNumber in the set was changed, will reset the status of + * all TunableNumbers in the set. Useful for avoiding redundant resetting. * @param tunableNumbers tunable numbers to check */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(v -> v.hasChanged(id))) { - action.run(); + public static void ifChanged( + int id, Runnable action, boolean resetAll, LoggedTunableNumber... tunableNumbers) { + + // Only force resetLastValue on numbers that haven't already been checked for changes to avoid + // redundancy. If not, break the loop on the first found instance. + boolean hasRunAction = false; + for (var tunableNumber : tunableNumbers) { + if (hasRunAction) { + tunableNumber.resetLastValue(id); + } else if (tunableNumber.hasChanged(id)) { + action.run(); + hasRunAction = true; + + // Exit early if no further resets are needed + if (!resetAll) { + break; + } + } } } @@ -149,11 +149,17 @@ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tun * Run callback if any tunable number has changed. See {@link #hasChanged()} for usage. * * @param action action to run + * @param resetAll if true and any TunableNumber in the set was changed, will reset the status of + * all TunableNumbers in the set. Useful for avoiding redundant resetting. * @param tunableNumbers tunable numbers to check */ - public static void ifChanged(Runnable action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(LoggedTunableNumber::hasChanged)) { - action.run(); - } + public static void ifChanged( + Runnable action, boolean resetAll, LoggedTunableNumber... tunableNumbers) { + ifChanged(0, action, resetAll, tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); } } diff --git a/src/main/java/frc/robot/util/LoggerUtil.java b/src/main/java/frc/robot/util/LoggerUtil.java index b84fb7f..4a5bbfa 100644 --- a/src/main/java/frc/robot/util/LoggerUtil.java +++ b/src/main/java/frc/robot/util/LoggerUtil.java @@ -11,7 +11,7 @@ public class LoggerUtil { /** Initialize the Logger with the auto-generated data from the build. */ public static void initializeLoggerMetadata() { // Record metadata from generated state file. - Logger.recordMetadata("ROBOT_NAME", Constants.getRobotType().toString()); + Logger.recordMetadata("ROBOT_NAME", Constants.getRobot().toString()); Logger.recordMetadata("RUNTIME_ENVIRONMENT", RobotBase.getRuntimeType().toString()); Logger.recordMetadata("TUNING_MODE", Boolean.toString(Constants.TUNING_MODE)); Logger.recordMetadata("PROJECT_NAME", BuildConstants.MAVEN_NAME); diff --git a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java index bc9a57e..85f1ae2 100644 --- a/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -2,7 +2,6 @@ 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.SwerveModuleState; @@ -195,8 +194,6 @@ public SwerveSetpoint generateSetpoint( final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredState, double dt) { - final Translation2d[] modules = moduleLocations; - SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); // Make sure desiredState respects velocity limits. if (limits.maxDriveVelocity() > 0.0) { @@ -207,23 +204,23 @@ public SwerveSetpoint generateSetpoint( // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so // just use the previous angle. boolean need_to_steer = true; - if (desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + if (desiredState.toTwist2d().equalsZero()) { need_to_steer = false; - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; desiredModuleState[i].speedMetersPerSecond = 0.0; } } // For each module, compute local Vx and Vy vectors. - double[] prev_vx = new double[modules.length]; - double[] prev_vy = new double[modules.length]; - Rotation2d[] prev_heading = new Rotation2d[modules.length]; - double[] desired_vx = new double[modules.length]; - double[] desired_vy = new double[modules.length]; - Rotation2d[] desired_heading = new Rotation2d[modules.length]; + double[] prev_vx = new double[moduleLocations.length]; + double[] prev_vy = new double[moduleLocations.length]; + Rotation2d[] prev_heading = new Rotation2d[moduleLocations.length]; + double[] desired_vx = new double[moduleLocations.length]; + double[] desired_vy = new double[moduleLocations.length]; + Rotation2d[] desired_heading = new Rotation2d[moduleLocations.length]; boolean all_modules_should_flip = true; - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { prev_vx[i] = prevSetpoint.moduleStates()[i].angle.getCos() * prevSetpoint.moduleStates()[i].speedMetersPerSecond; @@ -251,8 +248,8 @@ public SwerveSetpoint generateSetpoint( } } if (all_modules_should_flip - && !prevSetpoint.chassisSpeeds().toTwist2d().epsilonEquals(new Twist2d()) - && !desiredState.toTwist2d().epsilonEquals(new Twist2d())) { + && !prevSetpoint.chassisSpeeds().toTwist2d().equalsZero() + && !desiredState.toTwist2d().equalsZero()) { // It will (likely) be faster to stop the robot, rotate the modules in place to the complement // of the desired // angle, and accelerate again. @@ -275,14 +272,14 @@ public SwerveSetpoint generateSetpoint( // In cases where an individual module is stopped, we want to remember the right steering angle // to command (since // inverse kinematics doesn't care about angle, we can be opportunistically lazy). - List> overrideSteering = new ArrayList<>(modules.length); + List> overrideSteering = new ArrayList<>(moduleLocations.length); // Enforce steering velocity limits. We do this by taking the derivative of steering angle at // the current angle, // and then backing out the maximum interpolant between start and goal states. We remember the // minimum across all modules, since // that is the active constraint. final double max_theta_step = dt * limits.maxSteeringVelocity(); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { if (!need_to_steer) { overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); continue; @@ -344,7 +341,7 @@ public SwerveSetpoint generateSetpoint( // Enforce drive wheel acceleration limits. final double max_vel_step = dt * limits.maxDriveAcceleration(); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { if (min_s == 0.0) { // No need to carry on. break; @@ -377,7 +374,7 @@ public SwerveSetpoint generateSetpoint( prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); var retStates = kinematics.toSwerveModuleStates(retSpeeds); - for (int i = 0; i < modules.length; ++i) { + for (int i = 0; i < moduleLocations.length; ++i) { final var maybeOverride = overrideSteering.get(i); if (maybeOverride.isPresent()) { var override = maybeOverride.get(); diff --git a/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java b/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java new file mode 100644 index 0000000..316b2da --- /dev/null +++ b/src/main/java/frc/robot/util/trajectory/DriveTrajectories.java @@ -0,0 +1,10 @@ +package frc.robot.util.trajectory; + +public class DriveTrajectories { + // Let Feederpose be some middle point between all feeder station nodes + // Let face_#_pose be some point between both left and right reef stations BUT not flush with the + // reef + + // known non-OTF trajectories + // feederpose to face_n_pose +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa81b2f..79bdf3e 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.1.0", + "version": "4.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.1.0" + "version": "4.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.1.0", + "version": "4.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-25.3.1.json similarity index 86% rename from vendordeps/Phoenix6-frc2025-latest.json rename to vendordeps/Phoenix6-25.3.1.json index 820c61a..3ff25f8 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-25.3.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-latest.json", + "fileName": "Phoenix6-25.3.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.3.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.3.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.3.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } diff --git a/vendordeps/REVLib-2025.json b/vendordeps/REVLib.json similarity index 89% rename from vendordeps/REVLib-2025.json rename to vendordeps/REVLib.json index 717aa34..459a62f 100644 --- a/vendordeps/REVLib-2025.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.2", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.2" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From adf1c0680bedeb433327b2343668858fb65f5dba Mon Sep 17 00:00:00 2001 From: Ayush Pal Date: Wed, 5 Mar 2025 20:55:44 -0500 Subject: [PATCH 68/73] Create ElevatorVisualizer.java --- .../robot/subsystems/elevator/ElevatorVisualizer.java | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..744d9fa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.elevator; + +public class ElevatorVisualizer { + private final String name; + + public ElevatorVisualizer(String name) { + this.name = name; + } + + public void update(double elevatorPositionMeters) {} +} \ No newline at end of file From cc7e3ef5a8960f0f65a6c48600dd64d9eae9e3b7 Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu, 6 Mar 2025 10:10:52 -0500 Subject: [PATCH 69/73] fixes --- .../frc/robot/subsystems/elevator/ElevatorVisualizer.java | 2 +- src/main/java/frc/robot/util/LoggedTunableNumber.java | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java index 744d9fa..0196eb9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorVisualizer.java @@ -8,4 +8,4 @@ public ElevatorVisualizer(String name) { } public void update(double elevatorPositionMeters) {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 3dfa43c..a265098 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -162,9 +162,4 @@ public static void ifChanged( public double getAsDouble() { return get(); } - - @Override - public double getAsDouble() { - return get(); - } } From 63323d3c23ca5a97443e2052b314e0cc379708f9 Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu, 6 Mar 2025 10:37:25 -0500 Subject: [PATCH 70/73] Auto-enable robot-relative and slow mode when extending elevator --- src/main/java/frc/robot/RobotContainer.java | 76 ++++++++++++++++----- 1 file changed, 58 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9defebc..0c872b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,10 +23,15 @@ import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeIOSpark; import frc.robot.util.AllianceFlipUtil; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class RobotContainer { + // Button Binding variables + @AutoLogOutput private boolean robotRelativeEnabled = false; + @AutoLogOutput private boolean slowModeEnabled = false; + // Load PoseEstimator class private final PoseEstimator poseEstimator = PoseEstimator.getInstance(); @@ -47,8 +52,6 @@ public class RobotContainer { private final LoggedNetworkNumber endgameAlert2 = new LoggedNetworkNumber("/SmartDashboard/Endgame Alert #2", 15.0); - private boolean slowModeEnabled; - public RobotContainer() { switch (Constants.getMode()) { case REAL -> { @@ -145,6 +148,16 @@ private void configureButtonBindings() { // Make slow mode toggleable controller.y().toggleOnTrue(Commands.runOnce(() -> slowModeEnabled = !slowModeEnabled)); + controller + .leftBumper() + .and(controller.rightBumper()) + .onTrue(Commands.runOnce(() -> robotRelativeEnabled = true)); + + controller + .leftBumper() + .and(controller.rightBumper()) + .onFalse(Commands.runOnce(() -> robotRelativeEnabled = false)); + // Default command, normal field-relative drive driveBase.setDefaultCommand( DriveCommands.joystickDrive( @@ -153,31 +166,51 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> -controller.getRightX(), () -> slowModeEnabled, - () -> controller.leftBumper().and(controller.rightBumper()).getAsBoolean())); + () -> robotRelativeEnabled)); // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); // L1 controller .povLeft() - .onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL))); + .onTrue( + Commands.runOnce( + () -> { + robotRelativeEnabled = true; + slowModeEnabled = true; + }) + .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L1_CORAL)))); // L2 controller .povUp() .onTrue( - Commands.either( - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)), - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), - controller.b().negate().debounce(0.25))); + Commands.runOnce( + () -> { + robotRelativeEnabled = true; + slowModeEnabled = true; + }) + .andThen( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L2_CORAL)), + Commands.runOnce( + () -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25)))); // L3 controller .povRight() .onTrue( - Commands.either( - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)), - Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), - controller.b().negate().debounce(0.25))); + Commands.runOnce( + () -> { + robotRelativeEnabled = true; + slowModeEnabled = true; + }) + .andThen( + Commands.either( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.L3_CORAL)), + Commands.runOnce( + () -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), + controller.b().negate().debounce(0.25)))); // Intake controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); @@ -185,12 +218,19 @@ private void configureButtonBindings() { controller .rightTrigger() .onTrue( - Commands.either( - dispenserBase - .eject(elevatorBase::getGoal) - .andThen(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))), - IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), - controller.leftTrigger().negate().debounce(0.25))); + Commands.runOnce( + () -> { + robotRelativeEnabled = false; + slowModeEnabled = false; + }) + .andThen( + Commands.either( + dispenserBase + .eject(elevatorBase::getGoal) + .andThen( + Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))), + IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), + controller.leftTrigger().negate().debounce(0.25)))); // Home Elevator controller From 2609d97e70d611f91d18d15f94dc36207c2bc393 Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu, 6 Mar 2025 11:21:32 -0500 Subject: [PATCH 71/73] added joystick disconnected alert and silenced driverstation warnings --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 5 ++++- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/util/AlertsUtil.java | 6 ++++++ 4 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2697903..a5abb8c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.RobotBase; public class Constants { - private static RobotType robotType = RobotType.COMPBOT; + private static RobotType robotType = RobotType.SIMBOT; // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 73c12cf..2690bec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -75,7 +75,10 @@ public Robot() { // Configure brownout voltage RobotController.setBrownoutVoltage(6.0); - // Create RobotConatiner + // Disable joysick warnings in Driver Station + DriverStation.silenceJoystickConnectionWarning(true); + + // Create RobotContainer robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c872b1..0081025 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; public class RobotContainer { + // Button Binding variables @AutoLogOutput private boolean robotRelativeEnabled = false; @AutoLogOutput private boolean slowModeEnabled = false; diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java index 4e1e513..615c3f4 100644 --- a/src/main/java/frc/robot/util/AlertsUtil.java +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -37,6 +37,9 @@ public static AlertsUtil getInstance() { private final Alert lowBatteryVoltageAlert = new Alert("Battery voltage is too low, change the battery", AlertType.kWarning); private final Debouncer batteryVoltageDebouncer = new Debouncer(1.5); + private final Alert joystickDisconnectedAlert = + new Alert("At least one joystick button is not detected", AlertType.kWarning); + private final Debouncer joystickDebouncer = new Debouncer(2); // Program Alerts private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); @@ -75,6 +78,9 @@ public void periodic() { batteryVoltageDebouncer.calculate( RobotController.getBatteryVoltage() <= LOW_VOLTAGE_WARNING_THRESHOLD)); + joystickDisconnectedAlert.set( + joystickDebouncer.calculate(DriverStation.isJoystickConnected(0))); + // Update Program Alerts // TODO From de23ffdfbf6c4d1756a0375683dd4f11c8f7399a Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Wed, 12 Mar 2025 15:16:48 -0400 Subject: [PATCH 72/73] Squashed commit of the following: commit f82a33da3fd9876feb3ff74ff88bf121fabdc327 Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Mon Mar 10 15:46:43 2025 -0400 Moved hardware IDs to respective subsystem constants files commit 9a6732fb44614ec853c23fbe1afbc7f80570962e Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Mar 9 13:26:05 2025 -0400 Fix drive PID Coefficients and Deep Run Changes commit 8773d7dd33229b83e0fe12f9bdfee179060b2ae1 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Mar 8 23:35:34 2025 -0500 Semi-final PID Coefficients commit d390ead2833286f802b17869aee3def0863d9293 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Mar 5 01:01:36 2025 -0500 Reimplement I term into SparkMax PID Controllers Revert "Reimplement I term into SparkMax PID Controllers" This reverts commit 553d51a6b0895bc75a3d32d4ffa376010b4638d2. proto drive I term and IZone commit 72df17df6088b8289b7983d49738152fdcb178bc Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Mar 5 00:49:10 2025 -0500 Update tunable number API to avoid redundant resets and call correct static overrides commit 807290376c46d7f105b2fbe289dcb65f693ef7b3 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Mar 4 23:39:29 2025 -0500 update vendor deps commit f47b2de7462a71805594f16d023da1ee4bd56801 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Mar 4 23:27:50 2025 -0500 stuff for ayush to fix commit 813d3df00fc8e4204b21972c10b2483b91cba476 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Mar 4 14:04:32 2025 -0500 add algae mode commit 2391b502be154be767571a01584c71948d8ec3ee Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Mar 3 09:49:19 2025 -0500 Add robot relative override commit db57af5ec2e028afb5a7e54af712e5fab6fa0c35 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Mar 1 19:01:14 2025 -0500 add wait period to checking intake when enabling immediately after elevator (fix this later to be better) commit c8e1dbb4acc6841e9bf38da6c89cba5acd75a319 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Mar 1 19:00:45 2025 -0500 make eject configurable based on level commit 26c56be6041314f5ab5c4aab42ef00ee26996bc9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Mar 1 19:00:19 2025 -0500 add taxi auto commit 60ebba74d081177ca9ca9cc8cb231f7ae826ae81 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Mar 1 19:00:08 2025 -0500 remove intake timeout commit 773704f71a039f1825a2f3807e5faf9dd4e607f9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:44:29 2025 -0500 add reserialize command commit fd13f3f313e0a6fa053e1f084e78f7a4697b2580 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:44:13 2025 -0500 update dispenser eject to be faster also goes back to stow automatically commit 5b7bf78cee0ade7d5a032ee35edad0ce24e4360e Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:42:38 2025 -0500 Rename holdingCoral to better match lombok commit 3d7c324107b5835ec39c64155be03a194fdcd2b6 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:41:47 2025 -0500 stop elevator from honing on gyro reset commit f17c27284704a73ea111ba047bf25cbb7c4eb561 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:41:19 2025 -0500 Make intake toggleable commit de0014ccd9ec2329c01e64120c62610973842a77 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 12:40:28 2025 -0500 Make slow mode toggleable and cleanup changes from Thursday commit e9c169485efbf1c1873457f1c69f7b7c3f43d1da Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:51:20 2025 -0500 idk if this matters commit 24e9f2b4ccb65aa2641f5b542847f92649a702d5 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:50:57 2025 -0500 Auto hone elevator on start commit eda4d4bdaa315a1e4c2bb4ad9718232aa2974e22 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:50:05 2025 -0500 Fix characterization DT bug commit a6ac1fe2c2a0712859b2c78dadd7086bae7401b2 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri Feb 28 10:49:23 2025 -0500 fix merge commit 32aca1e42958e7faccf81f6fc759329ae5b35b0b Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu Feb 27 01:05:11 2025 -0500 tuning stuff commit 1b9467e64e8533dff54591e8612d4cad12a4711d Merge: 07083ba 2e7b277 Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Wed Feb 26 18:55:24 2025 -0500 Merge branch 'sriman-dev' of https://github.com/Talon540Programming/Reefscape2025 into sriman-dev commit 2e7b277a430c9b1cfb39e9c44657eda154c0ca3f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 18:48:56 2025 -0500 Create ElevatorVisualizer.java commit b32b9fc75f2d4b8d5533fadc176c701eec4eff0d Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 18:48:48 2025 -0500 Update RobotContainer.java commit 6cfc8e339566071b0dfe1b55291828f5523012ca Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 14:57:45 2025 -0500 Update LoggedTunableNumber.java commit e7fadb78556a97f651eceaa2934ee1f3684f20f4 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 11:20:15 2025 -0500 schedule home command if not homed on teleop init commit 1d3b19195f0965de2da490d954342cacf353d2dc Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 10:24:53 2025 -0500 add proto auto intake commit d1f8beb647341078b591bf9d92889cd945de9e86 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 10:24:33 2025 -0500 add sprint commit d267a5f5fa61a4fc23719ef03079e075d88fb78c Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 26 01:54:57 2025 -0500 add elevator and end effector subsystems commit 07083ba1d80b32fe6304f25afe028c50cd5f4395 Author: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Wed Feb 26 01:36:12 2025 -0500 fixes commit ee57f87138c8c7ace29a1366c6b6c1770a7e8484 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Feb 25 22:03:17 2025 -0500 WIP commit 9f9de79cf2f5d6650b46a4b92adedfda7012d27f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue Feb 25 03:29:28 2025 -0500 Update IO commit ba09067684158f1859c2290eb650eb8f567363c2 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:57 2025 -0500 Implement ElevatorIO commit 19e39761c03d84aab7256b660042e199dd804225 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:49 2025 -0500 Implement DispenserIO commit bd348ee7efa5ff7adddc9e6eb5648bf8f1c8448f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:40 2025 -0500 Make sim models non static avoids building creating these objects when running as non-sim commit 1baa58d8dd96d97f443ec8aab682cd5d8d5466bf Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 11:09:27 2025 -0500 Add updatable debouncer commit ba6e2991d808c364586857695d9867a73bdd8b99 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 05:03:14 2025 -0500 Update EqualsUtil.java commit fb125076708266e1c948eca930a01f8695ea8e15 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 23 13:58:29 2025 -0500 Update FieldConstants.java commit 96f6eae34148a6acd41b60d069dcd7a052245bd9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 10:40:15 2025 -0500 dont make conversion factors variables commit 43435348a07e05f7cc8096adecdd54e234ab25e3 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 10:39:38 2025 -0500 explicitly set motor temp signals commit e8fbd8094026f56fd72af4489b30a31a5e2d366c Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 01:28:04 2025 -0500 Update Intake commit 6adf211c5e34a0c35c5a679c734caf39afb0f72e Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 01:05:32 2025 -0500 Fix DT bug commit ca2f600f234c53cbbfd7de39552818c127d4f15f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 22 00:53:08 2025 -0500 Update robot constants to match bot commit 68e75f96ca0b1cbb53b0039c66e3a05ac3f9bedd Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 19 23:34:57 2025 -0500 Add intake subsystem commit de577e53203b8b230ad9d0794b68207aba6775e7 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 19 21:47:57 2025 -0500 fix akit version mismatch commit 2a1068cc911a360e906143ae3fe51a5c9e20aa4c Merge: 420fa3d 3be2661 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 16 15:45:12 2025 -0500 Merge branch 'main' into sriman-dev commit 3be26614e98132d3837aeb0bd1bb7313c4d5e973 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 16 15:43:57 2025 -0500 Update stuff (#7) * Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI Co-Authored-By: Talon540-root <122660543+Talon540-root@users.noreply.github.com> * Increase voltage warning on battery and add CAN error alert * clean * Fix Modules Falsely Reporting as Disconnected * Cleanup typing, make errors more obvious * Cleanup dashboard setting stuff * Update sim characterization constants to be more accurate, stops a massive overshooting for some reason * Log drive motor temps * Formatting fixes * Inline tuning mode alert * Rename RobotState to PoseEstimator Because only vision and drive will interact with pose (and purely position) and no other robot state is tracked, it makes more sense for this to be renammed to reflect that. * Refactor abstract alerts into util class to later roll into LEDs * Update Phoenix6 to 2025.2.2 * Refactor code structure to not over-expose subsystem specific stuff * Shorten name for drive temp * Fix red alliance drive commands bug * Update WPILIB to 2025.3.1 --------- Co-authored-by: Talon540-root <122660543+Talon540-root@users.noreply.github.com> commit 420fa3d8329e495d1a9422680ba392a58594e3c8 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun Feb 16 15:38:07 2025 -0500 Update WPILIB to 2025.3.1 commit 5b6968a286336017765f9eb07022e52eedd2e6b0 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu Feb 13 01:44:37 2025 -0500 Fix red alliance drive commands bug commit 295299829ca2ac25090cc684e1a29941dba6a40c Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 15 01:43:00 2025 -0500 Shorten name for drive temp commit b2626e8188e48faf889353291b823806ebb62044 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu Feb 13 00:06:04 2025 -0500 Refactor code structure to not over-expose subsystem specific stuff commit 2af259b8051e899a7fd4f0881f469a1e9c042b21 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 23:47:30 2025 -0500 Update Phoenix6 to 2025.2.2 commit 2ae942fab2a6df947bbd95de3acb054f21613be7 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 23:44:07 2025 -0500 Refactor abstract alerts into util class to later roll into LEDs commit 876046fc313910b509223089d7cc4a642dcb343d Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 23:42:36 2025 -0500 Rename RobotState to PoseEstimator Because only vision and drive will interact with pose (and purely position) and no other robot state is tracked, it makes more sense for this to be renammed to reflect that. commit e5c48434c24987d890808075435f263c189dbc6a Merge: f7606cf 9325f68 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:08:31 2025 -0500 Merge branch 'main' into sriman-dev commit 9325f68f55fe599daf580740b542113f3ed9cf2b Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:07:52 2025 -0500 Misc drive fixes (#6) * Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI Co-Authored-By: Talon540-root <122660543+Talon540-root@users.noreply.github.com> * Increase voltage warning on battery and add CAN error alert * clean * Fix Modules Falsely Reporting as Disconnected * Cleanup typing, make errors more obvious * Cleanup dashboard setting stuff * Update sim characterization constants to be more accurate, stops a massive overshooting for some reason * Log drive motor temps * Formatting fixes * Inline tuning mode alert --------- Co-authored-by: Talon540-root <122660543+Talon540-root@users.noreply.github.com> commit f7606cf1df2482dd259d1795aac8e2c650479f3f Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:07:08 2025 -0500 Inline tuning mode alert commit addcdcebe1579fa6625bd3a10573b4d6e567c1f4 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:04:53 2025 -0500 Formatting fixes commit 1b089c2e48d60b91746ec9027c54081b24d39cfe Merge: 37cd26f 93e655d Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 22:03:18 2025 -0500 Merge branch 'main' into sriman-dev commit 37cd26f8f16c0ff68b443e1e34f1b537ac1502c9 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:55:27 2025 -0500 Log drive motor temps commit 15e8caf8a87b3117208c2d49c3bfa086356575df Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:43:07 2025 -0500 Update sim characterization constants to be more accurate, stops a massive overshooting for some reason commit 803498a62e7d6a01598a80db76b12c4ea44af728 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:42:46 2025 -0500 Cleanup dashboard setting stuff commit 1cc2430a80c8e3bc42f9ffda1be27114f37ac568 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 12 21:42:25 2025 -0500 Cleanup typing, make errors more obvious commit 98c071538f2695fa8bae86db29b2520814843909 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 5 13:21:38 2025 -0500 Fix Modules Falsely Reporting as Disconnected commit 93e655dd78d69d1fe1200e81de80e00050792309 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Jan 18 04:02:47 2025 -0500 Implement Full Logging and Drivetrain Subsystem (#1) * Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI commit f9babf3b43a48e78657bb152028209e1788a6230 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat Feb 1 09:57:53 2025 -0500 clean commit 16c4eba5a4e66e6019a69086de7db2b8c3653f43 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Jan 29 12:44:39 2025 -0500 Increase voltage warning on battery and add CAN error alert commit 9485d1dfe8b218526379c481ff86c33b4e43b384 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon Feb 24 14:36:49 2025 -0500 Implement Elevator Subsystem Implement ElevatorIO Update IO WIP fixes add elevator and end effector subsystems commit 0aa671a186a1c62c34da4335d71a97fe6717be85 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Feb 19 23:34:57 2025 -0500 Implement Intake Subsystem, fix SIM initialization Update Intake explicitly set motor temp signals dont make conversion factors variables Update FieldConstants.java Add updatable debouncer Make sim models non static avoids building creating these objects when running as non-sim commit b616b8ff4e6b7f403e48ba0486a4c7823c0de331 Author: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed Jan 29 12:44:39 2025 -0500 Alerts usage update, dependency bump, and misc drive fixes clean Fix Modules Falsely Reporting as Disconnected Cleanup typing, make errors more obvious Cleanup dashboard setting stuff Update sim characterization constants to be more accurate, stops a massive overshooting for some reason Log drive motor temps Inline tuning mode alert Rename RobotState to PoseEstimator Because only vision and drive will interact with pose (and purely position) and no other robot state is tracked, it makes more sense for this to be renammed to reflect that. Refactor abstract alerts into util class to later roll into LEDs Update Phoenix6 to 2025.2.2 Refactor code structure to not over-expose subsystem specific stuff Shorten name for drive temp Fix red alliance drive commands bug Update WPILIB to 2025.3.1 fix akit version mismatch Fix DT bug Update EqualsUtil.java Update robot constants to match bot --- src/main/java/frc/robot/Constants.java | 2 +- .../dispenser/DispenserConstants.java | 1 + .../dispenser/DispenserIOSpark.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 27 +++++++++---------- .../frc/robot/subsystems/drive/ModuleIO.java | 2 +- .../robot/subsystems/drive/ModuleIOSim.java | 3 ++- .../robot/subsystems/drive/ModuleIOSpark.java | 4 +-- .../subsystems/elevator/ElevatorBase.java | 5 +--- .../elevator/ElevatorConstants.java | 3 +++ .../subsystems/elevator/ElevatorIOSpark.java | 4 +-- .../subsystems/intake/IntakeConstants.java | 2 ++ .../subsystems/intake/IntakeIOSpark.java | 2 +- 12 files changed, 30 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a5abb8c..2697903 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.RobotBase; public class Constants { - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.COMPBOT; // Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector public static final boolean TUNING_MODE = true; // Disable the AdvantageKit logger from running diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java index df11973..532f80d 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserConstants.java @@ -4,4 +4,5 @@ class DispenserConstants { public static final boolean inverted = true; public static final double moi = 0.025; // TODO public static final double gearing = 34.0 / 24.0; + public static final int id = 14; } diff --git a/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java index 8a689b6..8694d60 100644 --- a/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java +++ b/src/main/java/frc/robot/subsystems/dispenser/DispenserIOSpark.java @@ -21,7 +21,7 @@ public class DispenserIOSpark implements DispenserIO { private final DigitalInput rearBeamBreak = new DigitalInput(0); public DispenserIOSpark() { - spark = new SparkMax(14, MotorType.kBrushless); + spark = new SparkMax(id, MotorType.kBrushless); encoder = spark.getEncoder(); var config = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 3a9f3d5..823fa88 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -21,6 +21,8 @@ class Module { new LoggedTunableNumber("Drive/Module/DrivekI"); private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("Drive/Module/DrivekD"); + private static final LoggedTunableNumber driveIZone = + new LoggedTunableNumber("Drive/Module/DrivekIZone"); private static final LoggedTunableNumber turnkP = new LoggedTunableNumber("Drive/Module/TurnkP"); private static final LoggedTunableNumber turnkI = new LoggedTunableNumber("Drive/Module/TurnkI"); private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD"); @@ -30,12 +32,12 @@ class Module { case COMPBOT -> { drivekS.initDefault(0.69641); drivekV.initDefault(0.12647); - drivekP.initDefault(0.0); - drivekI.initDefault(0.0); - drivekD.initDefault(0.0); - turnkP.initDefault(1.5); - turnkI.initDefault(0.0); - turnkD.initDefault(0.0); + drivekP.initDefault(0.0075); + drivekI.initDefault(0.0000005); + drivekD.initDefault(0.001); + driveIZone.initDefault(0.01); + turnkP.initDefault(0.65); + turnkD.initDefault(0.1); } default -> { drivekS.initDefault(0.113190); @@ -43,6 +45,7 @@ class Module { drivekP.initDefault(0.1); drivekI.initDefault(0.0); drivekD.initDefault(0.0); + driveIZone.initDefault(0.0); turnkP.initDefault(10.0); turnkI.initDefault(0.0); turnkD.initDefault(0.0); @@ -80,18 +83,14 @@ public void periodic() { hashCode(), () -> m_io.setDriveFF(drivekS.get(), drivekV.get()), true, drivekS, drivekV); LoggedTunableNumber.ifChanged( hashCode(), - () -> m_io.setDrivePID(drivekP.get(), drivekI.get(), drivekD.get()), + () -> m_io.setDrivePID(drivekP.get(), drivekI.get(), drivekD.get(), driveIZone.get()), true, drivekP, drivekI, - drivekD); + drivekD, + driveIZone); LoggedTunableNumber.ifChanged( - hashCode(), - () -> m_io.setTurnPID(turnkP.get(), turnkI.get(), turnkD.get()), - true, - turnkP, - turnkI, - turnkD); + hashCode(), () -> m_io.setTurnPID(turnkP.get(), 0, turnkD.get()), true, turnkP, turnkD); // Update Odometry Positions int sampleCount = m_inputs.odometryDrivePositionsRad.length; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index bf0eddb..d1aed3f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -41,7 +41,7 @@ public default void runDriveVelocity(double velocityRadPerSec) {} 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) {} + public default void setDrivePID(double kP, double kI, double kD, double IZone) {} /** Set kS, kV gains for closed loop control on drive motor. */ public default void setDriveFF(double kS, double kV) {} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index d40eab4..85b3178 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -119,8 +119,9 @@ public void runTurnPosition(Rotation2d rotation) { } @Override - public void setDrivePID(double kP, double kI, double kD) { + public void setDrivePID(double kP, double kI, double kD, double IZone) { driveController.setPID(kP, kI, kD); + driveController.setI(IZone); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index b80427e..3dfd80e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -195,9 +195,9 @@ public void runTurnPosition(Rotation2d rotation) { } @Override - public void setDrivePID(double kP, double kI, double kD) { + public void setDrivePID(double kP, double kI, double kD, double IZone) { var drivePIDConfig = new SparkMaxConfig(); - drivePIDConfig.closedLoop.pid(kP, kI, kD); + drivePIDConfig.closedLoop.pid(kP, kI, kD).iZone(IZone); driveSpark.configure( drivePIDConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java index 5681691..ed8e584 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorBase.java @@ -25,7 +25,6 @@ public class ElevatorBase extends SubsystemBase { // Tunable numbers private static final LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP"); - private static final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/kI"); private static final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD"); private static final LoggedTunableNumber kS = new LoggedTunableNumber("Elevator/kS"); private static final LoggedTunableNumber kG = new LoggedTunableNumber("Elevator/kG"); @@ -50,7 +49,6 @@ public class ElevatorBase extends SubsystemBase { switch (Constants.getRobot()) { case COMPBOT -> { kP.initDefault(0.3); - kI.initDefault(0.0); kD.initDefault(0.25); kS.initDefault(0); kG.initDefault(1.05); @@ -58,7 +56,6 @@ public class ElevatorBase extends SubsystemBase { } case SIMBOT -> { kP.initDefault(0); // TODO - kI.initDefault(0.0); // TODO kD.initDefault(0); // TODO kS.initDefault(0); // TODO kG.initDefault(0); // TODO @@ -139,7 +136,7 @@ public void periodic() { followerDisconnectedAlert.set(!inputs.followerConnected); // Update tunable numbers - LoggedTunableNumber.ifChanged(() -> io.setPID(kP.get(), kI.get(), kD.get()), true, kP, kI, kD); + LoggedTunableNumber.ifChanged(() -> io.setPID(kP.get(), 0.0, kD.get()), true, kP, kD); LoggedTunableNumber.ifChanged( () -> { feedforward.setKs(kS.get()); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 85c509a..3973822 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -20,4 +20,7 @@ public class ElevatorConstants { public static final double carriageMassKg = Units.lbsToKilograms(6.0); public static final double stagesMassKg = Units.lbsToKilograms(12.0); + + public static final int leaderId = 12; + public static final int followerId = 13; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java index f028ec0..1c483c5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -22,8 +22,8 @@ public class ElevatorIOSpark implements ElevatorIO { private final Debouncer followerConnectedDebouncer = new Debouncer(0.5); public ElevatorIOSpark() { - leaderSpark = new SparkMax(12, MotorType.kBrushless); - followerSpark = new SparkMax(13, MotorType.kBrushless); + leaderSpark = new SparkMax(leaderId, MotorType.kBrushless); + followerSpark = new SparkMax(followerId, MotorType.kBrushless); encoder = leaderSpark.getEncoder(); controller = leaderSpark.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 5fe42dd..a62d35e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -4,4 +4,6 @@ class IntakeConstants { public static final boolean inverted = true; public static final double moi = 0.025; // TODO public static final double gearing = 2.0; + + public static final int id = 11; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java index 7ed2a2e..be9a81a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSpark.java @@ -17,7 +17,7 @@ public class IntakeIOSpark implements IntakeIO { private final Debouncer connectedDebouncer = new Debouncer(.5); public IntakeIOSpark() { - spark = new SparkMax(11, SparkLowLevel.MotorType.kBrushless); + spark = new SparkMax(id, SparkLowLevel.MotorType.kBrushless); encoder = spark.getEncoder(); var config = new SparkMaxConfig(); From b53644f2d5a8d22d667c24e6ece97627e8b3218b Mon Sep 17 00:00:00 2001 From: Talon540-root <122660543+Talon540-root@users.noreply.github.com> Date: Thu, 13 Mar 2025 10:54:42 -0400 Subject: [PATCH 73/73] formatting fixes --- src/main/java/frc/robot/RobotContainer.java | 4 ---- src/main/java/frc/robot/util/AlertsUtil.java | 2 -- 2 files changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0470e96..1a3834d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -168,7 +168,6 @@ private void configureButtonBindings() { () -> slowModeEnabled, () -> robotRelativeEnabled)); - // Stow controller.povDown().onTrue(Commands.runOnce(() -> elevatorBase.setGoal(ElevatorState.STOW))); // L1 @@ -198,7 +197,6 @@ private void configureButtonBindings() { () -> elevatorBase.setGoal(ElevatorState.L2_ALGAE_REMOVAL)), controller.b().negate().debounce(0.25)))); - // L3 controller .povRight() @@ -215,7 +213,6 @@ private void configureButtonBindings() { () -> elevatorBase.setGoal(ElevatorState.L3_ALGAE_REMOVAL)), controller.b().negate().debounce(0.25)))); - // Intake controller.x().toggleOnTrue(IntakeCommands.intake(elevatorBase, intakeBase, dispenserBase)); @@ -236,7 +233,6 @@ private void configureButtonBindings() { IntakeCommands.reserialize(elevatorBase, intakeBase, dispenserBase), controller.leftTrigger().negate().debounce(0.25)))); - // Home Elevator controller .back() diff --git a/src/main/java/frc/robot/util/AlertsUtil.java b/src/main/java/frc/robot/util/AlertsUtil.java index a40946a..615c3f4 100644 --- a/src/main/java/frc/robot/util/AlertsUtil.java +++ b/src/main/java/frc/robot/util/AlertsUtil.java @@ -41,7 +41,6 @@ public static AlertsUtil getInstance() { new Alert("At least one joystick button is not detected", AlertType.kWarning); private final Debouncer joystickDebouncer = new Debouncer(2); - // Program Alerts private final Alert tuningModeAlert = new Alert("Robot in Tuning Mode", AlertType.kInfo); private final Alert ledsDisabledAlert = @@ -82,7 +81,6 @@ public void periodic() { joystickDisconnectedAlert.set( joystickDebouncer.calculate(DriverStation.isJoystickConnected(0))); - // Update Program Alerts // TODO