From 54b27d50078f47c50e083880a42eec7aca009655 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] WIP (squash later) --- .../frc/robot/commands/DriveCommands.java | 44 ++ .../frc/robot/subsystems/drive/DriveBase.java | 25 ++ .../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 | 58 +++ 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 | 400 ++++++++++++++++++ 13 files changed, 855 insertions(+) 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/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..b7c55f3 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,44 @@ +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..c6a3797 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Alert; +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..76b32d7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/OdometryManager.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import org.littletonrobotics.junction.AutoLog; + +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; + +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..eade50a --- /dev/null +++ b/src/main/java/frc/robot/util/swerve/SwerveSetpointGenerator.java @@ -0,0 +1,400 @@ +package frc.robot.util.swerve; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.util.EqualsUtil; +import frc.robot.util.GeomUtil; +import lombok.Builder; +import lombok.RequiredArgsConstructor; +import lombok.experimental.ExtensionMethod; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +/** + * 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) {} +}