diff --git a/src/main/java/frc/excalib/additional_utilities/Elastic.java b/src/main/java/frc/excalib/additional_utilities/Elastic.java deleted file mode 100644 index 24a796c..0000000 --- a/src/main/java/frc/excalib/additional_utilities/Elastic.java +++ /dev/null @@ -1,390 +0,0 @@ -// Copyright (c) 2023-2025 Gold87 and other Elastic contributors -// This software can be modified and/or shared under the terms -// defined by the Elastic license: -// https://github.com/Gold872/elastic-dashboard/blob/main/LICENSE - -package frc.excalib.additional_utilities; - -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StringTopic; - -public final class Elastic { - private static final StringTopic notificationTopic = - NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); - private static final StringPublisher notificationPublisher = - notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); - private static final StringTopic selectedTabTopic = - NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); - private static final StringPublisher selectedTabPublisher = - selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); - private static final ObjectMapper objectMapper = new ObjectMapper(); - - /** - * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string - * before being published. - * - * @param notification the {@link Notification} object containing notification details - */ - public static void sendNotification(Notification notification) { - try { - notificationPublisher.set(objectMapper.writeValueAsString(notification)); - } catch (JsonProcessingException e) { - e.printStackTrace(); - } - } - - /** - * Selects the tab of the dashboard with the given name. If no tab matches the name, this will - * have no effect on the widgets or tabs in view. - * - *

If the given name is a number, Elastic will select the tab whose index equals the number - * provided. - * - * @param tabName the name of the tab to select - */ - public static void selectTab(String tabName) { - selectedTabPublisher.set(tabName); - } - - /** - * Selects the tab of the dashboard at the given index. If this index is greater than or equal to - * the number of tabs, this will have no effect. - * - * @param tabIndex the index of the tab to select. - */ - public static void selectTab(int tabIndex) { - selectTab(Integer.toString(tabIndex)); - } - - /** - * Represents an notification object to be sent to the Elastic dashboard. This object holds - * properties such as level, title, description, display time, and dimensions to control how the - * notification is displayed on the dashboard. - */ - public static class Notification { - @JsonProperty("level") - private NotificationLevel level; - - @JsonProperty("title") - private String title; - - @JsonProperty("description") - private String description; - - @JsonProperty("displayTime") - private int displayTimeMillis; - - @JsonProperty("width") - private double width; - - @JsonProperty("height") - private double height; - - /** - * Creates a new Notification with all default parameters. This constructor is intended to be - * used with the chainable decorator methods - * - *

Title and description fields are empty. - */ - public Notification() { - this(NotificationLevel.INFO, "", ""); - } - - /** - * Creates a new Notification with all properties specified. - * - * @param level the level of the notification (e.g., INFO, WARNING, ERROR) - * @param title the title text of the notification - * @param description the descriptive text of the notification - * @param displayTimeMillis the time in milliseconds for which the notification is displayed - * @param width the width of the notification display area - * @param height the height of the notification display area, inferred if below zero - */ - public Notification( - NotificationLevel level, - String title, - String description, - int displayTimeMillis, - double width, - double height) { - this.level = level; - this.title = title; - this.displayTimeMillis = displayTimeMillis; - this.description = description; - this.height = height; - this.width = width; - } - - /** - * Creates a new Notification with default display time and dimensions. - * - * @param level the level of the notification - * @param title the title text of the notification - * @param description the descriptive text of the notification - */ - public Notification(NotificationLevel level, String title, String description) { - this(level, title, description, 3000, 350, -1); - } - - /** - * Creates a new Notification with a specified display time and default dimensions. - * - * @param level the level of the notification - * @param title the title text of the notification - * @param description the descriptive text of the notification - * @param displayTimeMillis the display time in milliseconds - */ - public Notification( - NotificationLevel level, String title, String description, int displayTimeMillis) { - this(level, title, description, displayTimeMillis, 350, -1); - } - - /** - * Creates a new Notification with specified dimensions and default display time. If the height - * is below zero, it is automatically inferred based on screen size. - * - * @param level the level of the notification - * @param title the title text of the notification - * @param description the descriptive text of the notification - * @param width the width of the notification display area - * @param height the height of the notification display area, inferred if below zero - */ - public Notification( - NotificationLevel level, String title, String description, double width, double height) { - this(level, title, description, 3000, width, height); - } - - /** - * Updates the level of this notification - * - * @param level the level to set the notification to - */ - public void setLevel(NotificationLevel level) { - this.level = level; - } - - /** - * @return the level of this notification - */ - public NotificationLevel getLevel() { - return level; - } - - /** - * Updates the title of this notification - * - * @param title the title to set the notification to - */ - public void setTitle(String title) { - this.title = title; - } - - /** - * Gets the title of this notification - * - * @return the title of this notification - */ - public String getTitle() { - return title; - } - - /** - * Updates the description of this notification - * - * @param description the description to set the notification to - */ - public void setDescription(String description) { - this.description = description; - } - - public String getDescription() { - return description; - } - - /** - * Updates the display time of the notification - * - * @param seconds the number of seconds to display the notification for - */ - public void setDisplayTimeSeconds(double seconds) { - setDisplayTimeMillis((int) Math.round(seconds * 1000)); - } - - /** - * Updates the display time of the notification in milliseconds - * - * @param displayTimeMillis the number of milliseconds to display the notification for - */ - public void setDisplayTimeMillis(int displayTimeMillis) { - this.displayTimeMillis = displayTimeMillis; - } - - /** - * Gets the display time of the notification in milliseconds - * - * @return the number of milliseconds the notification is displayed for - */ - public int getDisplayTimeMillis() { - return displayTimeMillis; - } - - /** - * Updates the width of the notification - * - * @param width the width to set the notification to - */ - public void setWidth(double width) { - this.width = width; - } - - /** - * Gets the width of the notification - * - * @return the width of the notification - */ - public double getWidth() { - return width; - } - - /** - * Updates the height of the notification - * - *

If the height is set to -1, the height will be determined automatically by the dashboard - * - * @param height the height to set the notification to - */ - public void setHeight(double height) { - this.height = height; - } - - /** - * Gets the height of the notification - * - * @return the height of the notification - */ - public double getHeight() { - return height; - } - - /** - * Modifies the notification's level and returns itself to allow for method chaining - * - * @param level the level to set the notification to - * @return the current notification - */ - public Notification withLevel(NotificationLevel level) { - this.level = level; - return this; - } - - /** - * Modifies the notification's title and returns itself to allow for method chaining - * - * @param title the title to set the notification to - * @return the current notification - */ - public Notification withTitle(String title) { - setTitle(title); - return this; - } - - /** - * Modifies the notification's description and returns itself to allow for method chaining - * - * @param description the description to set the notification to - * @return the current notification - */ - public Notification withDescription(String description) { - setDescription(description); - return this; - } - - /** - * Modifies the notification's display time and returns itself to allow for method chaining - * - * @param seconds the number of seconds to display the notification for - * @return the current notification - */ - public Notification withDisplaySeconds(double seconds) { - return withDisplayMilliseconds((int) Math.round(seconds * 1000)); - } - - /** - * Modifies the notification's display time and returns itself to allow for method chaining - * - * @param displayTimeMillis the number of milliseconds to display the notification for - * @return the current notification - */ - public Notification withDisplayMilliseconds(int displayTimeMillis) { - setDisplayTimeMillis(displayTimeMillis); - return this; - } - - /** - * Modifies the notification's width and returns itself to allow for method chaining - * - * @param width the width to set the notification to - * @return the current notification - */ - public Notification withWidth(double width) { - setWidth(width); - return this; - } - - /** - * Modifies the notification's height and returns itself to allow for method chaining - * - * @param height the height to set the notification to - * @return the current notification - */ - public Notification withHeight(double height) { - setHeight(height); - return this; - } - - /** - * Modifies the notification's height and returns itself to allow for method chaining - * - *

This will set the height to -1 to have it automatically determined by the dashboard - * - * @return the current notification - */ - public Notification withAutomaticHeight() { - setHeight(-1); - return this; - } - - /** - * Modifies the notification to disable the auto dismiss behavior - * - *

This sets the display time to 0 milliseconds - * - *

The auto dismiss behavior can be re-enabled by setting the display time to a number - * greater than 0 - * - * @return the current notification - */ - public Notification withNoAutoDismiss() { - setDisplayTimeMillis(0); - return this; - } - - /** - * Represents the possible levels of notifications for the Elastic dashboard. These levels are - * used to indicate the severity or type of notification. - */ - public enum NotificationLevel { - /** Informational Message */ - INFO, - /** Warning message */ - WARNING, - /** Error message */ - ERROR - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/additional_utilities/periodics/PeriodicScheduler.java b/src/main/java/frc/excalib/additional_utilities/periodics/PeriodicScheduler.java new file mode 100644 index 0000000..7eb757c --- /dev/null +++ b/src/main/java/frc/excalib/additional_utilities/periodics/PeriodicScheduler.java @@ -0,0 +1,39 @@ +package frc.excalib.additional_utilities.periodics; + +import java.util.ArrayList; + +public class PeriodicScheduler { + + private static final PeriodicScheduler instance = new PeriodicScheduler(); + + private PeriodicScheduler() {} + + public enum PERIOD { + MILLISECONDS_10(10), + MILLISECONDS_20(20), + MILLISECONDS_50(50), + MILLISECONDS_100(100), + MILLISECONDS_500(500), + SECOND(1000); + private final ArrayList tasks; + public final int milliseconds; + + PERIOD(int milliseconds) { + this.tasks = new ArrayList<>(); + this.milliseconds = milliseconds; + } + + public void run() { + for (PeriodicTask task : this.tasks) task.execute(); + } + + public void add(PeriodicTask task) { + this.tasks.add(task); + } + + } + + public static PeriodicScheduler getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/excalib/additional_utilities/periodics/PeriodicTask.java b/src/main/java/frc/excalib/additional_utilities/periodics/PeriodicTask.java new file mode 100644 index 0000000..b75305f --- /dev/null +++ b/src/main/java/frc/excalib/additional_utilities/periodics/PeriodicTask.java @@ -0,0 +1,19 @@ +package frc.excalib.additional_utilities.periodics; + +public class PeriodicTask { + private Runnable task; + PeriodicScheduler.PERIOD period; + + public PeriodicTask(Runnable toRun, PeriodicScheduler.PERIOD period){ + this.task = toRun; + this.period = period; + this.period.add(this); + } + + public void execute(){ + task.run(); + } + public void setTask(Runnable task){ + this.task = task; + } +} diff --git a/src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java b/src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java deleted file mode 100644 index a6855ba..0000000 --- a/src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.excalib.commands; - -import edu.wpi.first.util.ErrorMessages; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -import java.util.function.BooleanSupplier; - -/** - * - */ -public class ContinuouslyConditionalCommand extends Command { - private final Command m_onTrue; - private final Command m_onFalse; - private final BooleanSupplier m_condition; - private boolean negated; - - public ContinuouslyConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { - this.m_onTrue = ErrorMessages.requireNonNullParam(onTrue, "onTrue", "ConditionalCommand"); - this.m_onFalse = ErrorMessages.requireNonNullParam(onFalse, "onFalse", "ConditionalCommand"); - this.m_condition = ErrorMessages.requireNonNullParam(condition, "condition", "ConditionalCommand"); - CommandScheduler.getInstance().registerComposedCommands(onTrue, onFalse); - negated = condition.getAsBoolean(); - } - - private Command getCurrentCommand(){ - if (m_condition.getAsBoolean()) return m_onTrue; - return m_onFalse; - } - - private Command getOtherCommand(){ - if (m_condition.getAsBoolean()) return m_onFalse; - return m_onTrue; - } - - public void initialize() { - getCurrentCommand().schedule(); - } - - public void execute() { - if (negated != m_condition.getAsBoolean()) { - getOtherCommand().cancel(); - getCurrentCommand().schedule(); - negated = !negated; - } - } -} diff --git a/src/main/java/frc/excalib/commands/MapCommand.java b/src/main/java/frc/excalib/commands/MapCommand.java deleted file mode 100644 index d7dc995..0000000 --- a/src/main/java/frc/excalib/commands/MapCommand.java +++ /dev/null @@ -1,74 +0,0 @@ -package frc.excalib.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -import java.util.Map; -import java.util.function.Supplier; - -/** - * A command that maps a key of type T to a corresponding command. - * This allows dynamic selection and execution of commands based on a supplied key. - * - * @param The type of the key used to map to commands. - */ -public class MapCommand extends Command { - private final Map m_map; // A map that associates keys of type T with commands. - private Command m_command; // The currently selected command to execute. - private Supplier m_t; // A supplier that provides the key to select the command. - - /** - * Constructs a new MapCommand. - * - * @param map A map associating keys of type T with commands. - * @param t A supplier that provides the key to select the command. - */ - public MapCommand(Map map, Supplier t) { - m_map = map; - m_t = t; - // Add the requirements of all commands in the map to this command. - map.forEach((key, value) -> this.addRequirements(value.getRequirements())); - } - - /** - * Initializes the command by selecting the appropriate command from the map - * based on the key provided by the supplier. - * If no command is found for the key, a default "none" command is used. - */ - @Override - public void initialize() { - m_command = m_map.get(m_t.get()); - if (m_command == null) { - m_command = Commands.none(); // Use a default "none" command if no match is found. - } - m_command.initialize(); // Initialize the selected command. - } - - /** - * Executes the currently selected command. - */ - @Override - public void execute() { - m_command.execute(); - } - - /** - * Ends the currently selected command. - * - * @param inturupted Whether the command was interrupted. - */ - @Override - public void end(boolean inturupted) { - m_command.end(inturupted); - } - - /** - * Checks if the currently selected command has finished execution. - * - * @return True if the selected command is finished, false otherwise. - */ - @Override - public boolean isFinished() { - return this.m_command.isFinished(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/feedback/PID.java b/src/main/java/frc/excalib/control/feedback/PID.java new file mode 100644 index 0000000..fed2b8e --- /dev/null +++ b/src/main/java/frc/excalib/control/feedback/PID.java @@ -0,0 +1,57 @@ +package frc.excalib.control.feedback; + +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicTask; +import frc.excalib.control.gains.Gains; + +import java.util.function.DoubleSupplier; + +public class PID extends PeriodicTask { + private final double kp, ki, kd; + private final double periodSeconds; + + private double setpoint; + private double output = 0.0; + private double integral = 0.0; + private double lastError = 0.0; + private boolean firstRun = true; + + public PID(DoubleSupplier measurementSupplier, double setpoint, Gains gains, PeriodicScheduler.PERIOD period) { + super(() -> {}, period); + this.setpoint = setpoint; + this.kp = gains.kp; + this.ki = gains.ki; + this.kd = gains.kd; + this.periodSeconds = period.milliseconds / 1000.0; + + super.setTask(() -> { + double measurement = measurementSupplier.getAsDouble(); + double error = setpoint - measurement; + + integral += error * periodSeconds; + double derivative = firstRun ? 0.0 : (error - lastError) / periodSeconds; + + output = kp * error + ki * integral + kd * derivative; + lastError = error; + firstRun = false; + }); + } + + public double getOutput() { + return output; + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + } + + public double getSetpoint() { + return setpoint; + } + + public void reset() { + integral = 0.0; + lastError = 0.0; + firstRun = true; + } +} diff --git a/src/main/java/frc/excalib/control/math/filters/Derivative.java b/src/main/java/frc/excalib/control/math/filters/Derivative.java new file mode 100644 index 0000000..d200cf2 --- /dev/null +++ b/src/main/java/frc/excalib/control/math/filters/Derivative.java @@ -0,0 +1,28 @@ +package frc.excalib.control.math.filters; + +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicTask; + +import java.util.function.DoubleSupplier; + +public class Derivative extends PeriodicTask { + private double value = 0.0; + private double last; + private final double periodSeconds; + + public Derivative(DoubleSupplier f, PeriodicScheduler.PERIOD period) { + super(() -> {}, period); + this.last = f.getAsDouble(); + this.periodSeconds = period.milliseconds / 1000.0; + + super.setTask(() -> { + double current = f.getAsDouble(); + value = (current - last) / periodSeconds; + last = current; + }); + } + + public double getValue() { + return value; + } +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/filters/Integral.java b/src/main/java/frc/excalib/control/math/filters/Integral.java new file mode 100644 index 0000000..b06a208 --- /dev/null +++ b/src/main/java/frc/excalib/control/math/filters/Integral.java @@ -0,0 +1,29 @@ +package frc.excalib.control.math.filters; + +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicTask; + +import java.util.function.DoubleSupplier; + +public class Integral extends PeriodicTask { + private double value; + private double last; + + public Integral(double initialValue, DoubleSupplier f, PeriodicScheduler.PERIOD period) { + super(() -> {}, period); + this.last = f.getAsDouble(); + this.value = initialValue; + + double periodSeconds = period.milliseconds / 1000.0; + + super.setTask(() -> { + double current = f.getAsDouble(); + value += periodSeconds * (current + last) / 2.0; + last = current; + }); + } + + public double getValue() { + return value; + } +} diff --git a/src/main/java/frc/excalib/control/math/filters/data_fusion/EMAFilter.java b/src/main/java/frc/excalib/control/math/filters/data_fusion/EMAFilter.java new file mode 100644 index 0000000..4789086 --- /dev/null +++ b/src/main/java/frc/excalib/control/math/filters/data_fusion/EMAFilter.java @@ -0,0 +1,31 @@ +package frc.excalib.control.math.filters.data_fusion; + +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicTask; + +import java.util.function.DoubleSupplier; + +public class EMAFilter extends PeriodicTask { + private final double alpha; + private double filteredValue; + private boolean initialized = false; + + public EMAFilter(DoubleSupplier input, double alpha, PeriodicScheduler.PERIOD period) { + super(() -> {}, period); + this.alpha = alpha; + + super.setTask(() -> { + double raw = input.getAsDouble(); + if (!initialized) { + filteredValue = raw; + initialized = true; + } else { + filteredValue = alpha * raw + (1.0 - alpha) * filteredValue; + } + }); + } + + public double getValue() { + return filteredValue; + } +} diff --git a/src/main/java/frc/excalib/control/math/filters/data_fusion/SMAFilter.java b/src/main/java/frc/excalib/control/math/filters/data_fusion/SMAFilter.java new file mode 100644 index 0000000..8c9d7dc --- /dev/null +++ b/src/main/java/frc/excalib/control/math/filters/data_fusion/SMAFilter.java @@ -0,0 +1,35 @@ +package frc.excalib.control.math.filters.data_fusion; + +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicTask; + +import java.util.function.DoubleSupplier; +import java.util.ArrayDeque; +import java.util.Deque; + +public class SMAFilter extends PeriodicTask { + private final int windowSize; + private final Deque window; + private double sum; + + public SMAFilter(DoubleSupplier input, int windowSize, PeriodicScheduler.PERIOD period) { + super(() -> {}, period); + this.windowSize = windowSize; + this.window = new ArrayDeque<>(windowSize); + this.sum = 0; + + super.setTask(() -> { + double sample = input.getAsDouble(); + if (window.size() == windowSize) { + sum -= window.removeFirst(); + } + window.addLast(sample); + sum += sample; + }); + } + + public double getValue() { + if (window.isEmpty()) return 0; + return sum / window.size(); + } +} diff --git a/src/main/java/frc/excalib/control/math/filters/data_fusion/WMAFilter.java b/src/main/java/frc/excalib/control/math/filters/data_fusion/WMAFilter.java new file mode 100644 index 0000000..7efd462 --- /dev/null +++ b/src/main/java/frc/excalib/control/math/filters/data_fusion/WMAFilter.java @@ -0,0 +1,50 @@ +package frc.excalib.control.math.filters.data_fusion; + +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicTask; + +import java.util.function.DoubleSupplier; +import java.util.function.IntFunction; +import java.util.ArrayDeque; +import java.util.Deque; +import java.util.Iterator; + +public class WMAFilter extends PeriodicTask { + public final int windowSize; + private final Deque window; + private final IntFunction weightFunction; + + public WMAFilter(DoubleSupplier input, int windowSize, IntFunction weightFunction, PeriodicScheduler.PERIOD period) { + super(() -> {}, period); + this.windowSize = windowSize; + this.weightFunction = weightFunction; + this.window = new ArrayDeque<>(windowSize); + + super.setTask(() -> { + double sample = input.getAsDouble(); + if (window.size() == windowSize) { + window.removeFirst(); + } + window.addLast(sample); + }); + } + + public double getValue() { + if (window.isEmpty()) return 0; + + double weightedSum = 0; + double weightTotal = 0; + + Iterator iter = window.iterator(); + int index = 0; + while (iter.hasNext()) { + double sample = iter.next(); + double weight = weightFunction.apply(index); + weightedSum += sample * weight; + weightTotal += weight; + index++; + } + + return weightTotal == 0 ? 0 : weightedSum / weightTotal; + } +} diff --git a/src/main/java/frc/excalib/slam/mapper/AuroraClient.java b/src/main/java/frc/excalib/slam/mapper/AuroraClient.java deleted file mode 100644 index d9457bc..0000000 --- a/src/main/java/frc/excalib/slam/mapper/AuroraClient.java +++ /dev/null @@ -1,110 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.wpilibj.DriverStation; - -import java.io.DataInputStream; -import java.io.IOException; -import java.net.ServerSocket; -import java.net.Socket; - -public class AuroraClient { - private ServerSocket serverSocket; - private Thread serverThread; - private volatile boolean running = true; - - // Pose Data (volatile for thread safety) - private volatile float x = 0; - private volatile float y = 0; - private volatile float z = 0; - private volatile float roll = 0; - private volatile float pitch = 0; - private volatile float yaw = 0; - - public AuroraClient(int port) { - serverThread = new Thread(() -> { - try { - serverSocket = new ServerSocket(port); - DriverStation.reportWarning("Localization server started on port " + port, false); - - while (running) { - try (Socket clientSocket = serverSocket.accept(); - DataInputStream in = new DataInputStream(clientSocket.getInputStream())) { - - DriverStation.reportWarning("Localization client connected!", false); - - while (running) { - try { - x = in.readFloat(); - y = in.readFloat(); - z = in.readFloat(); - roll = in.readFloat(); - pitch = in.readFloat(); - yaw = in.readFloat(); - } catch (IOException e) { - DriverStation.reportError("Error reading localization data: " + e.getMessage(), false); - break; - } - } - } catch (IOException e) { - DriverStation.reportError("Client connection error: " + e.getMessage(), false); - } - } - } catch (IOException e) { - DriverStation.reportError("Localization server error: " + e.getMessage(), false); - } - }); - - serverThread.setDaemon(true); - serverThread.start(); - } - - // Getter methods for retrieving pose data - public float getX() { - return x; - } - - public float getY() { - return y; - } - - public float getZ() { - return z; - } - - public float getRoll() { - return roll; - } - - public float getPitch() { - return pitch; - } - - public float getYaw() { - return yaw; - } - - public Pose3d getPose3d() { - return new Pose3d(x, y, z, new Rotation3d(roll, pitch, yaw)); - } - - public Pose2d getPose2d() { - return new Pose2d(x, y, new Rotation2d(yaw)); - } - - // Stops the server - public void stop() { - running = false; - try { - if (serverSocket != null) { - serverSocket.close(); - } - } catch (IOException e) { - DriverStation.reportError("Failed to close localization server: " + e.getMessage(), false); - } - } -} - diff --git a/src/main/java/frc/excalib/slam/mapper/Odometry.java b/src/main/java/frc/excalib/slam/mapper/Odometry.java deleted file mode 100644 index 72e0427..0000000 --- a/src/main/java/frc/excalib/slam/mapper/Odometry.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; - -import java.util.function.Supplier; - -public class Odometry extends SwerveDrivePoseEstimator { - private final Supplier m_YAW_SUPPLIER; - private Pose2d m_robotPose; - - public Odometry( - SwerveDriveKinematics swerveDrive, - SwerveModulePosition[] modulesPositions, - Supplier angleSupplier, - Pose2d initialPose) { - super(swerveDrive, angleSupplier.get(), modulesPositions, initialPose); - m_YAW_SUPPLIER = angleSupplier; - m_robotPose = initialPose; - } - - public Pose2d getRobotPose() { - return m_robotPose; - } - - public void updateOdometry(SwerveModulePosition[] modulesPositions) { - m_robotPose = super.update( - m_YAW_SUPPLIER.get(), - modulesPositions - ); - } - - public void resetOdometry(SwerveModulePosition[] modulesPositions, Pose2d newInitialPose) { - super.resetPosition(m_YAW_SUPPLIER.get(), modulesPositions, newInitialPose); - } -} diff --git a/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java b/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java deleted file mode 100644 index ecbea3e..0000000 --- a/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java +++ /dev/null @@ -1,91 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import monologue.Logged; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.targeting.PhotonPipelineResult; - -import java.util.List; -import java.util.Optional; -import java.util.function.BiConsumer; - -import static org.photonvision.PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY; -import static org.photonvision.PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; - -public class PhotonAprilTagsCamera implements Logged { - private final PhotonCamera camera; - private final AprilTagFieldLayout fieldLayout; - private final PhotonPoseEstimator photonPoseEstimator; - - public PhotonAprilTagsCamera(String cameraName, AprilTagFields aprilTagField, Transform3d robotToCamera) { - camera = new PhotonCamera(cameraName); - camera.setDriverMode(false); - - fieldLayout = AprilTagFieldLayout.loadField(aprilTagField); - - photonPoseEstimator = new PhotonPoseEstimator(fieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); - photonPoseEstimator.setMultiTagFallbackStrategy(LOWEST_AMBIGUITY); - } - - public PhotonCamera getCamera() { - return camera; - } - - public void setDriverMode(boolean isDriverMode) { - camera.setDriverMode(isDriverMode); - } - - public void setPipeline(int index) { - camera.setPipelineIndex(index); - } - - public PhotonPipelineResult getFirstLatestResult() { - var result = camera.getAllUnreadResults(); - - if (result != null) return result.getFirst(); - return new PhotonPipelineResult(); - } - - public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { - List unreadResults = camera.getAllUnreadResults(); - - for (PhotonPipelineResult result : unreadResults) - if (result.hasTargets() && result.getBestTarget().getPoseAmbiguity() < 0.2) { - Translation2d targetTranslation = result.getBestTarget().getBestCameraToTarget().getTranslation().toTranslation2d(); - // m - double TOO_FAR = 3.5; - if (targetTranslation.getDistance(new Translation2d(0, 0)) < TOO_FAR) { - photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - return photonPoseEstimator.update(result); - } - } - - return Optional.empty(); - } - - public boolean updateFromAprilTagPose(BiConsumer toUpdate) { - var unreadResults = camera.getAllUnreadResults(); - for (PhotonPipelineResult result : unreadResults) - if (!result.hasTargets()) return false; - - for (PhotonPipelineResult result : unreadResults) { - var id = result.getBestTarget().getFiducialId(); - if (id == -1) return false; - var tag = fieldLayout.getTagPose(id); - if (tag.isEmpty()) return false; - toUpdate.accept( - tag.get().plus( - result.getBestTarget().getBestCameraToTarget()).toPose2d(), - result.getTimestampSeconds() - ); - } - return true; - } -} - diff --git a/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java b/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java deleted file mode 100644 index 03e377d..0000000 --- a/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; - -import java.util.function.Supplier; - -public class PoseEstimator extends SwerveDrivePoseEstimator { - private final Supplier m_YAW_SUPPLIER; - private Pose2d m_robotPose; - - public PoseEstimator(SwerveDriveKinematics kinematics, Supplier yawSupplier, SwerveModulePosition[] modulePositions, Pose2d initialPose) { - super(kinematics, yawSupplier.get(), modulePositions, initialPose); - m_YAW_SUPPLIER = yawSupplier; - } - - -} diff --git a/src/main/java/frc/excalib/swerve/ModulesHolder.java b/src/main/java/frc/excalib/swerve/ModulesHolder.java deleted file mode 100644 index 2cf7661..0000000 --- a/src/main/java/frc/excalib/swerve/ModulesHolder.java +++ /dev/null @@ -1,222 +0,0 @@ -package frc.excalib.swerve; - -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.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.excalib.control.math.Vector2D; -import monologue.Logged; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import static frc.robot.SwerveConstants.MAX_VEL; -import static monologue.Annotations.Log; - -public class ModulesHolder implements Logged { - public final SwerveModule m_frontLeft; - public final SwerveModule m_frontRight; - public final SwerveModule m_backLeft; - public final SwerveModule m_backRight; - - private final SwerveDriveKinematics m_swerveDriveKinematics; - - private final SwerveModulePosition[] m_modulePositions; - - /** - * A constructor that initialize the ModulesHolder. - * - * @param frontLeft A SwerveModule represents the front-left module. - * @param frontRight A SwerveModule represents the front-right module. - * @param backLeft A SwerveModule represents the back-left module. - * @param backRight A SwerveModule represents the back-right module. - */ - public ModulesHolder( - SwerveModule frontLeft, - SwerveModule frontRight, - SwerveModule backLeft, - SwerveModule backRight) { - this.m_frontLeft = frontLeft; - this.m_frontRight = frontRight; - this.m_backLeft = backLeft; - this.m_backRight = backRight; - - // Initialize SwerveDriveKinematics once since module locations are constant - this.m_swerveDriveKinematics = new SwerveDriveKinematics( - frontLeft.m_MODULE_LOCATION, - frontRight.m_MODULE_LOCATION, - backLeft.m_MODULE_LOCATION, - backRight.m_MODULE_LOCATION - ); - - m_modulePositions = new SwerveModulePosition[]{ - m_frontLeft.getModulePosition(), - m_frontRight.getModulePosition(), - m_backLeft.getModulePosition(), - m_backRight.getModulePosition() - }; - } - - /** - * Stops all swerve modules. - */ - public void stop() { - m_frontLeft.stopModule(); - m_frontRight.stopModule(); - m_backLeft.stopModule(); - m_backRight.stopModule(); - } - - /** - * Gets the robot's average velocity based on the velocities of all modules. - * - * @return a Vector2D representing the robot's velocity. - */ - public Vector2D getVelocity() { - // Sum the velocities of all modules - double totalX = m_frontLeft.getVelocity().getX() - + m_frontRight.getVelocity().getX() - + m_backLeft.getVelocity().getX() - + m_backRight.getVelocity().getX(); - - double totalY = m_frontLeft.getVelocity().getY() - + m_frontRight.getVelocity().getY() - + m_backLeft.getVelocity().getY() - + m_backRight.getVelocity().getY(); - - // Compute the average velocity - return new Vector2D(totalX * 0.25, totalY * 0.25); - } - - @Log.NT(key = "angular vel") - public double getOmegaRadPerSec() { - return new SwerveDriveKinematics( - m_frontLeft.m_MODULE_LOCATION, - m_frontRight.m_MODULE_LOCATION, - m_backLeft.m_MODULE_LOCATION, - m_backRight.m_MODULE_LOCATION - ).toChassisSpeeds(logStates()).omegaRadiansPerSecond; - } - - @Log.NT(key = "swerve velocity") - public double getVelocityDistance() { - return getVelocity().getDistance(); - } - - /** - * Calculates the minimum velocity ratio limit among all modules. - * - * @param translationVelocity The desired translation velocity. - * @param omegaRadPerSec The desired rotation rate in radians per second. - * @return The velocity ratio limit. - */ - private double calcVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) { - double flLimit = m_frontLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - double frLimit = m_frontRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - double blLimit = m_backLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - double brLimit = m_backRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - - double velocityRatioLimit = Math.min(Math.min(flLimit, frLimit), Math.min(blLimit, brLimit)); - return Math.min(1.0, velocityRatioLimit); // Ensure the limit does not exceed 1.0 - } - - /** - * Sets the velocities of all modules based on the desired translation and rotation velocities. - * - * @param omega The desired rotation rate supplier. - * @param translationalVel The desired translation velocity supplier. - * @return A command to set the velocities. - */ - public Command setVelocitiesCommand(Supplier translationalVel, DoubleSupplier omega) { - return new ParallelCommandGroup( - m_frontLeft.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ), - m_frontRight.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ), - m_backLeft.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ), - m_backRight.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ) - ); - } - - public Command coastCommand() { - return new ParallelCommandGroup( - m_frontLeft.coastCommand(), - m_frontRight.coastCommand(), - m_backLeft.coastCommand(), - m_backRight.coastCommand() - ); - } - - public void setModulesStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VEL); - m_frontLeft.setDesiredState(states[0]); - m_frontRight.setDesiredState(states[1]); - m_backLeft.setDesiredState(states[2]); - m_backRight.setDesiredState(states[3]); - } - - /** - * Logs the states of all modules. - * - * @return An array of SwerveModuleState representing the states of the modules. - */ - @Log.NT(key = "Swerve States") - public SwerveModuleState[] logStates() { - return new SwerveModuleState[]{ - m_frontLeft.logState(), - m_frontRight.logState(), - m_backLeft.logState(), - m_backRight.logState() - }; - } - - @Log.NT(key = "SetPoints") - public SwerveModuleState[] logSetPointStates() { - return new SwerveModuleState[]{ - m_frontLeft.logSetpointState(), - m_frontRight.logSetpointState(), - m_backLeft.logSetpointState(), - m_backRight.logSetpointState() - }; - } - - /** - * Gets the swerve drive kinematics. - * - * @return The SwerveDriveKinematics instance. - */ - public SwerveDriveKinematics getSwerveDriveKinematics() { - return m_swerveDriveKinematics; - } - - /** - * Gets the positions of all modules. - * - * @return An array of SwerveModulePosition representing the positions of the modules. - */ - public SwerveModulePosition[] getModulesPositions() { - return m_modulePositions; - } - - public void periodic() { - m_frontLeft.periodic(); - m_frontRight.periodic(); - m_backLeft.periodic(); - m_backRight.periodic(); - } -} diff --git a/src/main/java/frc/excalib/swerve/Swerve.java b/src/main/java/frc/excalib/swerve/Swerve.java deleted file mode 100644 index 3779bf5..0000000 --- a/src/main/java/frc/excalib/swerve/Swerve.java +++ /dev/null @@ -1,501 +0,0 @@ -package frc.excalib.swerve; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.excalib.additional_utilities.AllianceUtils; -import frc.excalib.additional_utilities.Elastic; -import frc.excalib.control.gains.SysidConfig; -import frc.excalib.control.imu.IMU; -import frc.excalib.control.math.Vector2D; -import frc.excalib.control.motor.controllers.SparkMaxMotor; -import frc.excalib.control.motor.controllers.TalonFXMotor; -import frc.excalib.slam.mapper.Odometry; -import frc.excalib.slam.mapper.PhotonAprilTagsCamera; -import monologue.Logged; -import org.json.simple.parser.ParseException; -import org.photonvision.EstimatedRobotPose; - -import java.io.IOException; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless; -import static edu.wpi.first.apriltag.AprilTagFields.*; -import static frc.excalib.additional_utilities.Elastic.Notification.NotificationLevel.WARNING; -import static frc.robot.SwerveConstants.*; -import static monologue.Annotations.Log; - -/** - * A class representing a swerve subsystem. - */ -public class Swerve extends SubsystemBase implements Logged { - private final ModulesHolder modules; - private final IMU imu; - private final Odometry odometry; - PhotonAprilTagsCamera exampleCamera; - private ChassisSpeeds desiredChassisSpeeds = new ChassisSpeeds(); - private final Trigger finishTrigger; - private final Rotation2d PI = new Rotation2d(Math.PI); - private final InterpolatingDoubleTreeMap velocityLimit = new InterpolatingDoubleTreeMap(); - - private final SwerveDriveKinematics swerveDriveKinematics; - - private final PIDController angleController = new PIDController(ANGLE_GAINS.kp, ANGLE_GAINS.ki, ANGLE_GAINS.kd); - private final PIDController xController = new PIDController(TRANSLATION_GAINS.kp, TRANSLATION_GAINS.ki, TRANSLATION_GAINS.kd); - private final PIDController yController = new PIDController(TRANSLATION_GAINS.kp, TRANSLATION_GAINS.ki, TRANSLATION_GAINS.kd); - public final Field2d field = new Field2d(); - private Supplier angleSetpoint = Rotation2d::new; - private Supplier translationSetpoint = Translation2d::new; - - /** - * A constructor that initialize the Swerve Subsystem - * - * @param modules The ModulesHolder containing all swerve modules. - * @param imu IMU sensor. - * @param initialPosition The initial position of the robot. - */ - public Swerve(ModulesHolder modules, IMU imu, Pose2d initialPosition) { - this.modules = modules; - this.imu = imu; - this.imu.resetIMU(); - - angleController.enableContinuousInput(-Math.PI, Math.PI); - angleController.setTolerance(ANGLE_CONTROLLER_TOLORANCE); - xController.setTolerance(X_CONTROLLER_TOLORANCE); - yController.setTolerance(Y_CONTROLLER_TOLORANCE); - - finishTrigger = new Trigger(xController::atSetpoint).and(yController::atSetpoint).and(angleController::atSetpoint).debounce(0.1); - this.odometry = new Odometry(modules.getSwerveDriveKinematics(), modules.getModulesPositions(), this.imu::getZRotation, initialPosition); - PhotonAprilTagsCamera m_exampleCamera = new PhotonAprilTagsCamera("example", k2025ReefscapeWelded, new Transform3d(0, 0, 0, new Rotation3d())); - - swerveDriveKinematics = this.modules.getSwerveDriveKinematics(); - velocityLimit.put(0.1, 0.4); - velocityLimit.put(0.7, 2.0); - velocityLimit.put(1.5, MAX_VEL); - - initAutoBuilder(); - } - - /** - * Creates a drive command for the swerve drive. - * - * @param velocityMPS Supplier for the desired linear velocity in meters per second. - * @param omegaRadPerSec Supplier for the desired rotational velocity in radians per second. - * @param fieldOriented Supplier indicating whether the control is field-oriented. - * @return A command that drives the robot. - */ - public Command driveCommand( - Supplier velocityMPS, - DoubleSupplier omegaRadPerSec, - BooleanSupplier fieldOriented) { - - // Precompute values to avoid redundant calculations - Supplier adjustedVelocitySupplier = () -> { - Vector2D velocity = velocityMPS.get(); -// Vector2D velocity = getSmartTranslationalVelocitySetPoint(getVelocity(), velocityMPS.get()); - if (fieldOriented.getAsBoolean()) { - Rotation2d yaw = getRotation2D().unaryMinus(); - if (!AllianceUtils.isBlueAlliance()) yaw = yaw.plus(PI); - return velocity.rotate(yaw); - } - return velocity; - }; - - Command driveCommand = new ParallelCommandGroup(modules.setVelocitiesCommand( - adjustedVelocitySupplier, - omegaRadPerSec - ), - new RunCommand( - () -> desiredChassisSpeeds = new ChassisSpeeds( - adjustedVelocitySupplier.get().getX(), - adjustedVelocitySupplier.get().getY(), - omegaRadPerSec.getAsDouble()) - ) - ); - driveCommand.setName("Drive Command"); - driveCommand.addRequirements(this); - return driveCommand; - } - - /** - * A non-command function that drives the robot (for pathplanner). - * - * @param speeds A ChassisSpeeds object represents ROBOT RELATIVE speeds desired speeds. - */ - public void driveRobotRelativeChassisSpeeds(ChassisSpeeds speeds) { - modules.setModulesStates(swerveDriveKinematics.toSwerveModuleStates(speeds)); - } - - /** - * A method that turns the robot to a desired angle. - * - * @param angleSetpoint The desired angle in radians. - * @return A command that turns the robot to the wanted angle. - */ - public Command turnToAngleCommand(Supplier velocityMPS, Supplier angleSetpoint) { - return new SequentialCommandGroup( - new InstantCommand(() -> this.angleSetpoint = angleSetpoint), - driveCommand( - velocityMPS, - () -> angleController.calculate(getRotation2D().getRadians(), angleSetpoint.get().getRadians()), - () -> true - ) - ).withName("Turn To Angle"); - } - - public Command pidToPoseCommand(Supplier poseSetpoint) { - return new SequentialCommandGroup( - new InstantCommand( - () -> { - xController.calculate(getPose2D().getX(), poseSetpoint.get().getX()); - yController.calculate(getPose2D().getY(), poseSetpoint.get().getY()); - angleController.calculate(getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians()); - translationSetpoint = () -> poseSetpoint.get().getTranslation(); - angleSetpoint = () -> poseSetpoint.get().getRotation(); - } - ), - driveCommand( - () -> { - Vector2D vel = new Vector2D( - xController.calculate(getPose2D().getX(), poseSetpoint.get().getX()), - yController.calculate(getPose2D().getY(), poseSetpoint.get().getY()) - ); - double distance = getPose2D().getTranslation().getDistance(poseSetpoint.get().getTranslation()); - vel.setMagnitude(Math.min(vel.getDistance(), velocityLimit.get(distance))); -// vel = vel.rotate(poseSetpoint.get().getRotation()); -// vel.setX(Math.signum(vel.getX()) * Math.min(Math.abs(vel.getX()), 1.2)); -// vel.setY(Math.signum(vel.getY()) * Math.min(Math.abs(vel.getY()), 0.4)); -// vel = vel.rotate(poseSetpoint.get().getRotation().unaryMinus()); - if (!AllianceUtils.isBlueAlliance()) return vel.rotate(PI); - return vel; - }, - () -> angleController.calculate(getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians()), - () -> true - ) - ).until(finishTrigger).withName("PID To Pose"); - } - - /** - * A method that drives the robot to a desired pose. - * - * @param setPoint The desired pose. - * @return A command that drives the robot to the wanted pose. - */ - public Command driveToPoseCommand(Pose2d setPoint) { - return AutoBuilder.pathfindToPose( - setPoint, - MAX_PATH_CONSTRAINTS - ).withName("Pathfinding Command"); - } - - public Command driveToPoseWithOverrideCommand( - Pose2d setPoint, - BooleanSupplier override, - Supplier velocityMPS, - DoubleSupplier omegaRadPerSec) { - Command driveToPoseCommand = driveToPoseCommand(setPoint); - return new SequentialCommandGroup( - driveToPoseCommand.until(() -> velocityMPS.get().getDistance() != 0 && override.getAsBoolean()), - driveCommand( - velocityMPS, - omegaRadPerSec, - () -> true - ).until(() -> velocityMPS.get().getDistance() == 0) - ).repeatedly().until(driveToPoseCommand::isFinished).withName("Pathfinding With Override Command"); - } - - /** - * A method that drives the robot to the starting pose of a path, then follows the path. - * - * @param pathName The path which the robot needs to follow. - * @return A command that turns the robot to the wanted angle. - */ - public Command pathfindThenFollowPathCommand(String pathName) { - PathPlannerPath path; - try { - path = PathPlannerPath.fromPathFile(pathName); - } catch (IOException | ParseException e) { - Elastic.sendNotification(new Elastic.Notification( - WARNING, - "Path Creating Error", - "the path file " + pathName + " doesn't exist") - ); - return new PrintCommand("this path file doesn't exist"); - } - - return AutoBuilder.pathfindThenFollowPath( - path, - MAX_PATH_CONSTRAINTS - ); - } - - public Command resetAngleCommand() { - return new InstantCommand(imu::resetIMU).ignoringDisable(true); - } - - public Command coastCommand() { - Command coastCommand = modules.coastCommand().ignoringDisable(true).withName("Coast Command"); - coastCommand.addRequirements(this); - return coastCommand; - } - - /** - * Updates the robot's odometry. - */ - public void updateOdometry() { - odometry.updateOdometry(modules.getModulesPositions()); - Optional backPose = exampleCamera.getEstimatedGlobalPose(odometry.getEstimatedPosition()); - backPose.ifPresent( - estimatedRobotPose -> odometry.addVisionMeasurement( - estimatedRobotPose.estimatedPose.toPose2d(), - estimatedRobotPose.timestampSeconds - ) - ); - } - - /** - * A method that restarts the odometry. - * - * @param newPose the wanted new Pose2d of the robot. - */ - public void resetOdometry(Pose2d newPose) { - odometry.resetOdometry(modules.getModulesPositions(), newPose); - } - - /** - * Gets the robot's rotation. - * - * @return The current rotation of the robot. - */ - @Log.NT(key = "Robot Rotation") - public Rotation2d getRotation2D() { - return getPose2D().getRotation(); - } - - @Log.NT(key = "Angle Setpoint") - public Rotation2d getAngleSetpoint() { - return angleSetpoint.get(); - } - - @Log.NT(key = "Translation Setpoint") - public Translation2d getTranslationSetpoint() { - return translationSetpoint.get(); - } - - /** - * Gets the robot's pose. - * - * @return The current pose of the robot. - */ - @Log.NT(key = "Robot Pose") - public Pose2d getPose2D() { - return odometry.getRobotPose(); - } - - /** - * Gets the current velocity of the robot. - * - * @return The robot's velocity as a Vector2D. - */ - public Vector2D getVelocity() { - return modules.getVelocity(); - } - - /** - * Gets the current acceleration of the robot. - * - * @return The robot's acceleration as a Vector2D. - */ - @Log.NT(key = "Acceleration") - public double getAccelerationDistance() { - return new Vector2D(imu.getAccX(), imu.getAccY()).getDistance(); - } - - /** - * Gets the current robot relative speed. - * - * @return The robot's speed as a ChassisSpeeds. - */ - @Log.NT(key = "Measured Chassis Speeds") - public ChassisSpeeds getRobotRelativeSpeeds() { - return swerveDriveKinematics.toChassisSpeeds(modules.logStates()); - } - - @Log.NT - public ChassisSpeeds getDesiredChassisSpeeds() { - return desiredChassisSpeeds; - } - - public Command stopCommand() { - return driveCommand(() -> new Vector2D(0, 0), () -> 0, () -> true); - } - - /** - * A function that initialize the AutoBuilder for pathplanner. - */ - private void initAutoBuilder() { - // Load the RobotConfig from the GUI settings. You should probably - // store this in your Constants file - RobotConfig config = null; - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - - // Configure AutoBuilder last - assert config != null; - AutoBuilder.configure( - this::getPose2D, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> driveRobotRelativeChassisSpeeds(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - TRANSLATION_PID_CONSTANTS, // Translation PID constants - ANGLE_PID_CONSTANTS // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - return alliance.filter(value -> value == DriverStation.Alliance.Red).isPresent(); - }, - this // Reference to this subsystem to set requirements - ); - } - - - /** - * Runs a system identification routine on a specific module's angle. - * - * @param module The module index (0-3). - * @param dir The direction of the sysid routine. - * @param dynamic Whether to perform a dynamic or quasistatic test. - * @return The command to perform the sysid routine. - */ - public Command driveSysId(int module, Direction dir, SysidConfig sysidConfig, boolean dynamic) { - SwerveModule selectedModule; - - switch (module) { - case 0 -> selectedModule = modules.m_frontLeft; - case 1 -> selectedModule = modules.m_frontRight; - case 2 -> selectedModule = modules.m_backLeft; - case 3 -> selectedModule = modules.m_backRight; - default -> { - throw new IllegalArgumentException("Invalid module index: " + module); - } - } - return dynamic ? - selectedModule.driveSysIdDynamic(dir, this, sysidConfig) - : selectedModule.driveSysIdQuas(dir, this, sysidConfig); - } - - /** - * Runs a system identification routine on a specific module's angle. - * - * @param module The module index (0-3). - * @param dir The direction of the sysid routine. - * @param dynamic Whether to perform a dynamic or quasistatic test. - * @return The command to perform the sysid routine. - */ - public Command angleSysId(int module, Direction dir, SysidConfig sysidConfig, boolean dynamic) { - SwerveModule selectedModule; - - switch (module) { - case 0 -> selectedModule = modules.m_frontLeft; - case 1 -> selectedModule = modules.m_frontRight; - case 2 -> selectedModule = modules.m_backLeft; - case 3 -> selectedModule = modules.m_backRight; - default -> { - throw new IllegalArgumentException("Invalid module index: " + module); - } - } - return dynamic ? - selectedModule.angleSysIdDynamic(dir, this, sysidConfig) - : selectedModule.angleSysIdQuas(dir, this, sysidConfig); - } - - public static Swerve configureSwerve(Pose2d initialPose) { - return new Swerve( - new ModulesHolder( - new SwerveModule( - new TalonFXMotor(FRONT_LEFT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(FRONT_LEFT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - FRONT_LEFT_TRANSLATION, - () -> FRONT_LEFT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - ), - new SwerveModule( - new TalonFXMotor(FRONT_RIGHT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(FRONT_RIGHT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - FRONT_RIGHT_TRANSLATION, - () -> FRONT_RIGHT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - ), - new SwerveModule( - new TalonFXMotor(BACK_LEFT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(BACK_LEFT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - BACK_LEFT_TRANSLATION, - () -> BACK_LEFT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - ), - new SwerveModule( - new TalonFXMotor(BACK_RIGHT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(BACK_RIGHT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - BACK_RIGHT_TRANSLATION, - () -> BACK_RIGHT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - )), - GYRO, - initialPose - ); - } - - @Override - public void periodic() { - modules.periodic(); - field.setRobotPose(getPose2D()); - } -} diff --git a/src/main/java/frc/excalib/swerve/SwerveAccUtils.java b/src/main/java/frc/excalib/swerve/SwerveAccUtils.java deleted file mode 100644 index a3c2e5a..0000000 --- a/src/main/java/frc/excalib/swerve/SwerveAccUtils.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.excalib.swerve; - -import frc.excalib.control.math.Vector2D; - -import static frc.excalib.control.math.MathUtils.minSize; -import static frc.robot.SwerveConstants.*; - -public class SwerveAccUtils { - private static final double CYCLE_TIME = 0.02; - - /** - * A function to get the translational velocity setpoint - * - * @param velocitySetPoint wanted velocity setpoint - * @return translational velocity setpoint - */ - public static Vector2D getSmartTranslationalVelocitySetPoint(Vector2D currentVel, Vector2D velocitySetPoint) { - Vector2D deltaVelocity = velocitySetPoint.plus( - currentVel.mul(-1)); - Vector2D actualDeltaVelocity = applyAccelerationLimits(currentVel, deltaVelocity); - return currentVel.plus(actualDeltaVelocity); - } - - /** - * A function to apply the acceleration limits - * - * @param velocityError wanted velocity - * @return velocity limited by acceleration limits - */ - private static Vector2D applyAccelerationLimits(Vector2D currentVel, Vector2D velocityError) { - Vector2D wantedAcceleration = velocityError.mul(1 / CYCLE_TIME); - -// wantedAcceleration = applyForwardLimit(currentVel, wantedAcceleration); -// wantedAcceleration = applyTiltLimit(wantedAcceleration); - wantedAcceleration = applySkidLimit(wantedAcceleration); - - return wantedAcceleration.mul(CYCLE_TIME); - } - - /** - * A function to apply the forward acceleration limit - * - * @param wantedAcceleration the wanted acceleration - * @return wanted acceleration limited by forward acceleration limit - */ - private static Vector2D applyForwardLimit(Vector2D currentVel, Vector2D wantedAcceleration) { - double maxAcceleration = - MAX_FORWARD_ACC * - (1 - (currentVel.getDistance() / MAX_VEL)); - - wantedAcceleration = wantedAcceleration.limit(new Vector2D(maxAcceleration, currentVel.getDirection())); - return wantedAcceleration; - } - - /** - * A function to apply the tilt acceleration limit - * - * @param wantedAcceleration the wanted acceleration - * @return wanted acceleration limited by tilt acceleration limit - */ - private static Vector2D applyTiltLimit(Vector2D wantedAcceleration) { - double frontAcceleration = minSize(wantedAcceleration.getX(), MAX_FRONT_ACC); - double sideAcceleration = minSize(wantedAcceleration.getY(), MAX_SIDE_ACC); - return new Vector2D(frontAcceleration, sideAcceleration); - } - - /** - * A function to apply the skid acceleration limit - * - * @param wantedAcceleration the wanted acceleration - * @return wanted acceleration limited by skid acceleration limit - */ - private static Vector2D applySkidLimit(Vector2D wantedAcceleration) { - return new Vector2D( - Math.min(wantedAcceleration.getDistance(), MAX_SKID_ACC), - wantedAcceleration.getDirection() - ); - } -} diff --git a/src/main/java/frc/excalib/swerve/SwerveModule.java b/src/main/java/frc/excalib/swerve/SwerveModule.java deleted file mode 100644 index e21768d..0000000 --- a/src/main/java/frc/excalib/swerve/SwerveModule.java +++ /dev/null @@ -1,231 +0,0 @@ -package frc.excalib.swerve; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.gains.SysidConfig; -import frc.excalib.control.limits.ContinuousSoftLimit; -import frc.excalib.control.math.Vector2D; -import frc.excalib.control.motor.controllers.Motor; -import frc.excalib.mechanisms.fly_wheel.FlyWheel; -import frc.excalib.mechanisms.turret.Turret; -import monologue.Logged; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import static frc.excalib.control.motor.motor_specs.DirectionState.FORWARD; -import static frc.excalib.control.motor.motor_specs.DirectionState.REVERSE; -import static frc.excalib.control.motor.motor_specs.IdleState.BRAKE; - -/** - * A class representing a swerve module - * - * @author Yoav Cohen & Itay Keller - */ -public class SwerveModule implements Logged { - private final FlyWheel m_driveWheel; - private final Turret m_turret; - public final Translation2d m_MODULE_LOCATION; - private final double m_MAX_VEL; - private final Rotation2d m_moduleAnglePlus90; - private final Vector2D m_setPoint = new Vector2D(0, 0); - private final SwerveModulePosition m_swerveModulePosition; - - /** - * A constructor for the SwerveModule - */ - public SwerveModule(Motor driveMotor, Motor rotationMotor, Gains angleGains, Gains velocityGains, - double PIDTolerance, Translation2d moduleLocation, DoubleSupplier angleSupplier, - double maxVel, double velocityConversionFactor, double positionConversionFactor, - double rotationVelocityConversionFactor) { - driveMotor.setInverted(FORWARD); - driveMotor.setVelocityConversionFactor(velocityConversionFactor); - driveMotor.setIdleState(BRAKE); - driveMotor.setPositionConversionFactor(positionConversionFactor); - driveMotor.setCurrentLimit(0, 60); - - rotationMotor.setIdleState(BRAKE); - rotationMotor.setVelocityConversionFactor(rotationVelocityConversionFactor); - rotationMotor.setInverted(REVERSE); - - m_driveWheel = new FlyWheel(driveMotor, 10, 10, velocityGains); - - m_turret = new Turret(rotationMotor, new ContinuousSoftLimit(() -> Double.NEGATIVE_INFINITY, () -> Double.POSITIVE_INFINITY), - angleGains, PIDTolerance, angleSupplier); - - m_MODULE_LOCATION = moduleLocation; - m_MAX_VEL = maxVel; - - // Precompute the rotated module angle (module angle + 90 degrees) - m_moduleAnglePlus90 = m_MODULE_LOCATION.getAngle().plus(new Rotation2d(Math.PI / 2)); - - m_swerveModulePosition = new SwerveModulePosition(m_driveWheel.logPosition(), m_turret.getPosition()); - } - - double getVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) { - Vector2D rotationVector = new Vector2D( - omegaRadPerSec, - m_moduleAnglePlus90 - ); - Vector2D sigmaVel = translationVelocity.plus(rotationVector); - double sigmaVelDistance = sigmaVel.getDistance(); - - // Avoid division by zero - if (sigmaVelDistance == 0) { - return 0; - } - return m_MAX_VEL / sigmaVelDistance; - } - - Vector2D getSigmaVelocity(Vector2D translationVelocity, double omegaRadPerSec, double velocityRatioLimit) { - Vector2D rotationVector = new Vector2D( - omegaRadPerSec, - m_moduleAnglePlus90 - ); - Vector2D sigmaVel = translationVelocity.plus(rotationVector); - sigmaVel = sigmaVel.mul(velocityRatioLimit); - return sigmaVel; - } - - public boolean isOptimizable(Vector2D moduleVelocitySetPoint) { - Rotation2d setPointDirection = moduleVelocitySetPoint.getDirection(); - Rotation2d currentDirection = m_turret.getPosition(); - double deltaDirection = Math.cos(setPointDirection.minus(currentDirection).getRadians()); - - // If the dot product is negative, reversing the wheel direction may be beneficial - return deltaDirection < 0; - } - - public Command setVelocityCommand(Supplier moduleVelocity) { - return new ParallelCommandGroup( - m_driveWheel.setDynamicVelocityCommand(() -> { - Vector2D velocity = moduleVelocity.get(); - double speed = velocity.getDistance(); - - if (speed < 0.1) { - speed = 0; - } - - boolean optimize = isOptimizable(velocity); - return optimize ? -speed : speed; - }), - m_turret.setPositionCommand(() -> { - Vector2D velocity = moduleVelocity.get(); - double speed = velocity.getDistance(); - - if (speed < 0.1) { - return m_turret.getPosition(); - } - - boolean optimize = isOptimizable(velocity); - Rotation2d direction = velocity.getDirection(); - return optimize ? direction.rotateBy(Rotation2d.fromRadians(Math.PI)) : direction; - }), - new RunCommand(() -> { - m_setPoint.setY(moduleVelocity.get().getY()); - m_setPoint.setX(moduleVelocity.get().getX()); - }) - ); - } - - public Command setVelocityCommand( - Supplier translationVelocity, - DoubleSupplier omegaRadPerSec, - DoubleSupplier velocityRatioLimit) { - - return setVelocityCommand(() -> getSigmaVelocity( - translationVelocity.get(), - omegaRadPerSec.getAsDouble(), - velocityRatioLimit.getAsDouble())); - } - - public Command coastCommand() { - return new ParallelCommandGroup( - m_driveWheel.coastCommand(), - m_turret.coastCommand() - ); - } - - public void setDesiredState(SwerveModuleState wantedState) { - Vector2D velocity = new Vector2D(wantedState.speedMetersPerSecond, wantedState.angle); - double speed = velocity.getDistance(); - Rotation2d direction = velocity.getDirection(); - - if (speed < 0.1) { - speed = 0.0; - direction = m_turret.getPosition(); - } - - boolean optimize = isOptimizable(velocity); - - m_driveWheel.setDynamicVelocity(optimize ? -speed : speed); - m_turret.setPosition(optimize ? direction.rotateBy(Rotation2d.fromRadians(Math.PI)) : direction); - } - - /** - * Stops the module by setting the drive wheel output to zero. - */ - void stopModule() { - m_driveWheel.setOutput(0); - } - - /** - * Gets the module's velocity. - * - * @return a Vector2D representing the velocity. - */ - Vector2D getVelocity() { - return new Vector2D(m_driveWheel.getVelocity(), getPosition()); - } - - /** - * Gets the turret's position. - * - * @return the current position of the turret. - */ - Rotation2d getPosition() { - return m_turret.getPosition(); - } - - public SwerveModulePosition getModulePosition() { - return m_swerveModulePosition; - } - - public SwerveModuleState logState() { - Vector2D velocity = getVelocity(); - return new SwerveModuleState(velocity.getDistance(), velocity.getDirection()); - } - - public SwerveModuleState logSetpointState() { - return new SwerveModuleState(m_setPoint.getDistance(), m_setPoint.getDirection()); - } - - - public Command driveSysIdDynamic(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_driveWheel.sysIdDynamic(direction, swerve, m_driveWheel::logPosition, sysidConfig, false); - } - - public Command driveSysIdQuas(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_driveWheel.sysIdQuasistatic(direction, swerve, m_driveWheel::logPosition, sysidConfig, false); - } - - public Command angleSysIdDynamic(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_turret.sysIdDynamic(direction, swerve, m_turret::logPosition, sysidConfig, false); - } - - public Command angleSysIdQuas(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_turret.sysIdQuasistatic(direction, swerve, m_turret::logPosition, sysidConfig, false); - } - - public void periodic() { - m_swerveModulePosition.distanceMeters = m_driveWheel.logPosition(); - m_swerveModulePosition.angle = m_turret.getPosition(); - } -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3abceca..11ab060 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.excalib.additional_utilities.periodics.PeriodicScheduler; import monologue.Logged; import monologue.Monologue; @@ -15,10 +16,11 @@ public class Robot extends TimedRobot implements Logged { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; + public Robot() { m_robotContainer = new RobotContainer(); - // you need to change the Logged implementation acourding to the base class you create your subsystems Monologue.setupMonologue(this, "Robot", false, true); + PeriodicScheduler scheduler = PeriodicScheduler.getInstance(); } @Override