diff --git a/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java b/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java index 85e92a6..7458227 100644 --- a/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java +++ b/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java @@ -17,6 +17,7 @@ /** * the purpose of this class is too supply different utility functions for functionality * that depends on the robot alliance. + * * @author Shai Grossman */ public class AllianceUtils { @@ -43,6 +44,7 @@ public static boolean isRedAlliance() { /** * Converts a pose to the pose relative to the current driver station alliance. + * * @param bluePose the current blue alliance pose * @return the converted Pose2d pose */ @@ -69,7 +71,7 @@ public static Pose2d mirrorAlliance(Pose2d pose) { public static class AlliancePose { private Pose2d pose; - public AlliancePose(double x, double y, double degrees){ + public AlliancePose(double x, double y, double degrees) { this.pose = new Pose2d(x, y, Rotation2d.fromDegrees(degrees)); } @@ -77,7 +79,7 @@ public AlliancePose(Translation2d translation, Rotation2d rotation) { this.pose = new Pose2d(translation, rotation); } - public AlliancePose(double degrees){ + public AlliancePose(double degrees) { this.pose = new Pose2d(0, 0, Rotation2d.fromDegrees(degrees)); } diff --git a/src/main/java/frc/excalib/additional_utilities/Color.java b/src/main/java/frc/excalib/additional_utilities/Color.java deleted file mode 100644 index 74ced9b..0000000 --- a/src/main/java/frc/excalib/additional_utilities/Color.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.excalib.additional_utilities; - -/** - * This Class represents a single RGB color for led usage - */ -public class Color extends edu.wpi.first.wpilibj.util.Color { - public Color(double green, double red, double blue) { - super(green, red, blue); - } - - public Color(int green, int red, int blue) { - super(red, green, blue); - } - - public Color(){ - super(0, 0, 0); - } - - public static Color balance(Color color) { - double v = Math.max(Math.max(color.red, color.green), color.blue); - Color newColor = new Color(color.red, color.green / 2, color.blue / 4); - double newV = Math.max(Math.max(newColor.red, newColor.green), newColor.blue); - double ratio = v / newV; - return new Color(newColor.red * ratio, newColor.green * ratio, newColor.blue * ratio); - } - - public enum Colors { - OFF(new Color(0, 0, 0)), - - TEAM_GOLD(new Color(255, 170, 0)), - TEAM_BLUE(new Color(1, 30, 202)), - - RED(new Color(0, 255, 0)), - GREEN(new Color(0, 255, 0)), - BLUE(new Color(0, 0, 255)), - - WHITE(new Color(255, 255, 255)), - - YELLOW(new Color(255, 255, 0)), - CYAN(new Color(0, 255, 255)), - PINK(new Color(255, 0, 255)), - - ORANGE(new Color(255, 50, 0)), - PURPLE(new Color(75, 0, 130)); - - public final Color color; - - Colors(Color color) { - this.color = color; - } - } -} \ No newline at end of file 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/LEDs.java b/src/main/java/frc/excalib/additional_utilities/LEDs.java deleted file mode 100644 index 34d6284..0000000 --- a/src/main/java/frc/excalib/additional_utilities/LEDs.java +++ /dev/null @@ -1,308 +0,0 @@ -package frc.excalib.additional_utilities; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -import java.util.Arrays; -import java.util.Random; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.concurrent.atomic.AtomicInteger; -import java.util.concurrent.atomic.AtomicReference; - -import static frc.excalib.additional_utilities.Color.Colors.*; - -/** - * The LEDs class is responsible for controlling an addressable LED strip. - * It provides various patterns and commands to manipulate the LED strip's colors and animations. - */ -public class LEDs extends SubsystemBase { - private final AddressableLED LedStrip = new AddressableLED(LEDS_PORT); // LED strip object - private final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LENGTH); // Buffer to hold LED colors - - private static LEDs instance = null; // Singleton instance of the LEDs class - private final Random rnd = new Random(); // Random generator for random patterns - - private int tailIndex = 0; // Index for train patterns - private double offset = 0; // Offset for animations - - // PWM port for the LED strip - public static final int LEDS_PORT = 9; - // The length of the LED strip - public static final int LENGTH = 50; - - // Predefined color arrays - Color[] orange = new Color[LENGTH]; - Color[] black = new Color[LENGTH]; - - /** - * Private constructor to initialize the LED strip and set the default command. - */ - private LEDs() { - LedStrip.setLength(LENGTH); - LedStrip.start(); - - Arrays.fill(orange, ORANGE.color); - Arrays.fill(black, OFF.color); - - setDefaultCommand(setPattern(LEDPattern.EXPAND, BLUE.color, WHITE.color)); - } - - /** - * Returns the singleton instance of the LEDs class. - * - * @return The LEDs instance. - */ - public static LEDs getInstance() { - if (instance == null) { - instance = new LEDs(); - } - return instance; - } - - /** - * Creates a command to set a specific LED pattern with main and accent colors. - * - * @param pattern The LED pattern to set. - * @param mainColor The main color for the pattern. - * @param accentColor The accent color for the pattern. - * @return A command to execute the pattern. - */ - public Command setPattern(LEDPattern pattern, Color mainColor, Color accentColor) { - Command command = new InstantCommand(); // Default command - Color[] colors = new Color[LENGTH]; // Array to hold LED colors - int trainLength = (int) (LENGTH / 5.0); // Length of the train pattern - final AtomicBoolean invert = new AtomicBoolean(false); // Direction for train patterns - - switch (pattern) { - case OFF: - // Turns off all LEDs - Arrays.fill(colors, OFF.color); - command = new RunCommand(() -> setLedStrip(colors), this).withName("OFF"); - break; - - case SOLID: - // Sets all LEDs to the main color - Arrays.fill(colors, mainColor); - command = new RunCommand(() -> setLedStrip(colors), this).withName("SOLID: " + mainColor.toString()); - break; - - case ALTERNATING_STATIC: - // Creates a static alternating pattern of main and accent colors - for (int i = 0; i < LENGTH; i++) { - colors[i] = mainColor; - colors[i + 1] = accentColor; - i++; - } - command = new RunCommand(() -> setLedStrip(colors), this) - .withName("ALTERNATING, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - case ALTERNATING_MOVING: - // Creates a moving alternating pattern of main and accent colors - AtomicReference mainAlternatingColor = new AtomicReference<>(mainColor); - AtomicReference accentAlternatingColor = new AtomicReference<>(accentColor); - AtomicReference tempAlternatingColor = new AtomicReference<>(mainColor); - - command = this.runOnce(() -> { - for (int i = 0; i < LENGTH - 1; i++) { - colors[i] = mainAlternatingColor.get(); - colors[i + 1] = accentAlternatingColor.get(); - i++; - } - setLedStrip(colors); - - tempAlternatingColor.set(mainAlternatingColor.get()); - mainAlternatingColor.set(accentAlternatingColor.get()); - accentAlternatingColor.set(tempAlternatingColor.get()); - }) - .andThen(new WaitCommand(0.25)).repeatedly() - .withName("ALTERNATING_MOVING, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - case RANDOM: - // Sets LEDs to random colors - command = this.runOnce(() -> { - Arrays.fill(colors, new Color(rnd.nextInt(255), rnd.nextInt(255), rnd.nextInt(255))); - setLedStrip(colors); - }).andThen(new WaitCommand(1)) - .repeatedly().withName("RANDOM"); - break; - - case BLINKING: - // Creates a blinking pattern between main and accent colors - command = Commands.repeatingSequence( - new InstantCommand(() -> { - Arrays.fill(colors, mainColor); - setLedStrip(colors); - }, this), - new WaitCommand(0.15), - new InstantCommand(() -> { - Arrays.fill(colors, accentColor); - setLedStrip(colors); - }, this), - new WaitCommand(0.15) - ).withName("BLINKING, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - case EXPAND: - // Creates an expanding pattern from the center - final AtomicInteger expandStep = new AtomicInteger(0); - command = new InstantCommand(() -> { - Color[] LedColors = new Color[LENGTH]; - Arrays.fill(LedColors, mainColor); - int LEDS_LENGTH = 5; - - int step = expandStep.get(); - - if (LENGTH % 2 == 0) { - int leftCenter = (LENGTH / 2) - 1; - int rightCenter = LENGTH / 2; - int leftIndex = leftCenter - step; - int rightIndex = rightCenter + step; - - for (int i = 0; i < LEDS_LENGTH; i++) { // Light up 3 LEDs per side - if (leftIndex - i >= 0) { - LedColors[leftIndex - i] = accentColor; - } - if (rightIndex + i < LENGTH) { - LedColors[rightIndex + i] = accentColor; - } - } - } else { - int center = LENGTH / 2; - int leftIndex = center - step; - int rightIndex = center + step; - - for (int i = 0; i < LEDS_LENGTH; i++) { // Light up 3 LEDs per side - if (leftIndex - i >= 0) { - LedColors[leftIndex - i] = accentColor; - } - if (rightIndex + i < LENGTH) { - LedColors[rightIndex + i] = accentColor; - } - } - } - - int newStep = expandStep.get() + 1; - if (newStep > (LENGTH / 2)) { - newStep = 0; - } - expandStep.set(newStep); - - setLedStrip(LedColors); - }, this) - .andThen(new WaitCommand(0.01)) - .repeatedly() - .withName("EXPAND, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - // Additional cases omitted for brevity - default: - break; - } - - return command.ignoringDisable(true); - } - - public Command setPattern(LEDPattern pattern, Color color) { - return setPattern(pattern, color, OFF.color); - } - - - public Command setLEDsCommand(Color[] colors) { - return this.runOnce(() -> setLedStrip(colors)).ignoringDisable(true); - } - - public Command twoStatesCommand(LEDPattern firstPattern, Color firstColor, LEDPattern secondPattern, Color secondColor, Trigger switchStates) { - return setPattern(firstPattern, firstColor).until(switchStates).andThen(setPattern(secondPattern, secondColor)); - } - - public Command deadLineLEDcommand(Command ledCommand) { - return new StartEndCommand( - ledCommand::schedule, - restoreLEDs()::schedule); - } - - public Command scheduleLEDcommand(Command ledCommand) { - return new InstantCommand(ledCommand::schedule); - } - - public Command circleLEDs(Color[] colors) { - return Commands.repeatingSequence( - setLEDsCommand(colors), - new WaitCommand(0.1), - new InstantCommand(() -> shiftLeds(colors))) - .ignoringDisable(true); - } - - public Command restoreLEDs() { - return new InstantCommand(() -> - CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(this))); - } - - public enum LEDPattern { - OFF, - RAINBOW, - SOLID, - ALTERNATING_STATIC, - ALTERNATING_MOVING, - TRAIN_CIRCLE, - TRAIN, - RANDOM, - EXPAND, - BLINKING, - RSL; - } - - public Color getAllianceColor() { - if (DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)) return Color.Colors.TEAM_BLUE.color; - else if (DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) return Color.Colors.RED.color; - else return Color.Colors.WHITE.color; - } - - private void setLedStrip(Color[] colors) { - for (int i = 0; i < colors.length; i++) buffer.setLED(i, colors[i]); - LedStrip.setData(buffer); - } - - private void shiftTrain(Color[] colors, Color mainColor, Color trainColor, int trainLength, int offset) { - tailIndex = findTailIndex(colors, trainColor); - Arrays.fill(colors, mainColor); - for (int i = 0; i < trainLength; i++) { - colors[stayInBounds(i + tailIndex + offset, colors.length)] = trainColor; - } - } - - private int findHeadIndex(Color[] colors, Color trainColor) { - for (int i = colors.length - 1; i >= 0; i--) { - if (colors[i].equals(trainColor)) return i; - } - return -1; - } - - private int findTailIndex(Color[] colors, Color trainColor) { - for (int i = 0; i < colors.length; i++) { - if (colors[i].equals(trainColor)) return i; - } - return -1; - } - - private void shiftLeds(Color[] colors) { - Color lastColor = colors[colors.length - 1]; - - for (int i = colors.length - 2; i >= 0; i--) { - colors[i + 1] = colors[i]; - } - - colors[0] = lastColor; - } - - private int stayInBounds(int value, int length) { - if (value >= length) return stayInBounds(value - length, length); - if (value < 0) return stayInBounds(value + length, length); - return value; - } -} \ No newline at end of file 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/encoder/DutyCycleEncoder.java b/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java new file mode 100644 index 0000000..3ad5dfa --- /dev/null +++ b/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java @@ -0,0 +1,35 @@ +package frc.excalib.control.encoder; + +public class DutyCycleEncoder extends edu.wpi.first.wpilibj.DutyCycleEncoder implements Encoder { + private double positionConversionFactor, velocityConversionFactor; + private double position; + + public DutyCycleEncoder(int port){ + super(port); + } + + @Override + public void setPositionConversionFactor(double conversionFactor) { + this.positionConversionFactor = conversionFactor; + } + + @Override + public void setVelocityConversionFactor(double conversionFactor) { + this.velocityConversionFactor = conversionFactor; + } + + @Override + public double getPosition() { + return super.get() * this.positionConversionFactor + this.position; + } + + @Override + public double getVelocity() { + return 0; + } + + @Override + public void setPosition(double position, boolean offset) { + this.position = offset? -position : position; + } +} diff --git a/src/main/java/frc/excalib/control/encoder/Encoder.java b/src/main/java/frc/excalib/control/encoder/Encoder.java new file mode 100644 index 0000000..b529322 --- /dev/null +++ b/src/main/java/frc/excalib/control/encoder/Encoder.java @@ -0,0 +1,14 @@ +package frc.excalib.control.encoder; + +public interface Encoder { + + void setPositionConversionFactor(double positionConversionFactor); + + void setVelocityConversionFactor(double velocityConversionFactor); + + double getPosition(); + + double getVelocity(); + + void setPosition(double position, boolean offset); +} diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index a5be5ed..66b9296 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -1,5 +1,8 @@ package frc.excalib.control.gains; +import edu.wpi.first.math.controller.PIDController; + + /** * This class bundles together multiple types of gains, including PID gains and feedforward gains. */ @@ -9,6 +12,7 @@ public class Gains { // Feedforward gains: static, gravity, velocity, and acceleration public double ks, kg, kv, ka; + /** * Constructs a Gains object with specified PID and feedforward gains. * @@ -30,6 +34,7 @@ public Gains(double kp, double ki, double kd, double ks, double kv, double ka, d this.ka = ka; } + /** * Constructs a Gains object with only PID gains. * Feedforward gains are set to 0. @@ -42,6 +47,7 @@ public Gains(double kp, double ki, double kd) { this(kp, ki, kd, 0, 0, 0, 0); } + /** * Constructs a Gains object with only feedforward gains. * PID gains are set to 0. @@ -55,6 +61,7 @@ public Gains(double ks, double kg, double kv, double ka) { this(0, 0, 0, ks, kv, ka, kg); } + /** * Constructs a Gains object by combining PID gains and feedforward gains from two separate Gains objects. * @@ -65,6 +72,7 @@ public Gains(Gains PIDgains, Gains FFgains) { this(PIDgains.kp, PIDgains.ki, PIDgains.kd, FFgains.ks, FFgains.kv, FFgains.ka, FFgains.kg); } + /** * Constructs a Gains object with all gains set to 0. */ @@ -72,6 +80,7 @@ public Gains() { this(0, 0, 0, 0, 0, 0, 0); } + /** * Constructs a Gains object by copying the values from another Gains object. * @@ -81,6 +90,7 @@ public Gains(Gains gains) { this(gains.kp, gains.ki, gains.kd, gains.ks, gains.kg, gains.kv, gains.ka); } + /** * Sets the PID gains. * @@ -94,6 +104,7 @@ public void setPIDgains(double kp, double ki, double kd) { this.kd = kd; } + /** * Sets the feedforward gains. * @@ -108,4 +119,17 @@ public void setFeedForwardGains(double ks, double kg, double kv, double ka) { this.kv = kv; this.ka = ka; } -} \ No newline at end of file + + + public PIDController getPIDcontroller(){ + return new PIDController(this.kp, this.ki, this.kd); + } + + public GenericFeedFoward applyGains(GenericFeedFoward feedForward){ + feedForward.setKv(this.kv); + feedForward.setKs(this.ks); + feedForward.setKa(this.ka); + feedForward.setKg(this.kg); + return feedForward; + } +} diff --git a/src/main/java/frc/excalib/control/gains/GenericFF.java b/src/main/java/frc/excalib/control/gains/GenericFF.java new file mode 100644 index 0000000..fbfe810 --- /dev/null +++ b/src/main/java/frc/excalib/control/gains/GenericFF.java @@ -0,0 +1,105 @@ +package frc.excalib.control.gains; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + +public class GenericFF { + public interface GenericFeedForward { + void setKs(double Ks); + void setKv(double Kv); + void setKa(double Ka); + void setKg(double Kg); + } + + public static class ArmFF extends ArmFeedforward implements GenericFeedForward { + public ArmFF() { + super(0, 0, 0, 0); + } + + @Override + public void setKs(double Ks) { + super.setKs(Ks); + } + + + @Override + public void setKv(double Kv) { + super.setKv(Kv); + } + + + @Override + public void setKa(double Ka) { + super.setKa(Ka); + } + + + @Override + public void setKg(double Kg) { + super.setKg(Kg); + } + } + + public static class SimpleFF extends SimpleMotorFeedforward implements GenericFeedForward { + public SimpleFF() { + super(0, 0, 0); + } + + + @Override + public void setKs(double Ks) { + super.setKs(Ks); + } + + + @Override + public void setKv(double Kv) { + super.setKv(Kv); + } + + + @Override + public void setKa(double Ka) { + super.setKa(Ka); + } + + + @Override + public void setKg(double Kg) { + if (Kg != 0) throw new IllegalArgumentException("SimpleMotorFeedforward does not support gravity gain (Kg), please make it zero!"); + + return; + } + } + + public static class ElevatorFF extends ElevatorFeedforward implements GenericFeedForward { + public ElevatorFF() { + super(0, 0, 0, 0); + } + + + @Override + public void setKs(double Ks) { + super.setKs(Ks); + } + + + @Override + public void setKv(double Kv) { + super.setKv(Kv); + } + + + @Override + public void setKa(double Ka) { + super.setKa(Ka); + } + + + @Override + public void setKg(double Kg) { + super.setKg(Kg); + } + } +} diff --git a/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java b/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java index dce51ed..ae36a95 100644 --- a/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java +++ b/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java @@ -1,42 +1,51 @@ package frc.excalib.control.limits; import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; // Import the MathUtil class public class ContinuousSoftLimit extends SoftLimit { - /** - * A constructor that takes two DoubleSuppliers representing the dynamic limits: - * - * @param minLimit the minimal limit of the represented range - * @param maxLimit the maximal limit of the represented range - */ + private static final double TWO_PI = 2 * Math.PI; + public ContinuousSoftLimit(DoubleSupplier minLimit, DoubleSupplier maxLimit) { super(minLimit, maxLimit); } - public double getSetPoint(double measurement, double wantedSetPoint) { - double upperSetPoint, lowerSetPoint; - if (wantedSetPoint > measurement) { - upperSetPoint = wantedSetPoint; - while ((upperSetPoint - 2 * Math.PI) > measurement) { - upperSetPoint -= 2 * Math.PI; - } - lowerSetPoint = upperSetPoint - 2 * Math.PI; - } else if (wantedSetPoint < measurement) { - lowerSetPoint = wantedSetPoint; - while ((lowerSetPoint + 2 * Math.PI) < measurement) { - lowerSetPoint += 2 * Math.PI; - } - upperSetPoint = lowerSetPoint + 2 * Math.PI; - } else { - return wantedSetPoint; - } - if (upperSetPoint > super.getMaxLimit()) { - return lowerSetPoint; - } else if (lowerSetPoint < super.getMinLimit()) { - return upperSetPoint; - } - return - Math.abs(measurement - upperSetPoint) < Math.abs(measurement - lowerSetPoint) ? - upperSetPoint : lowerSetPoint; + public double getSetpoint(double measurement, double wantedSetPoint) { + double minLimit = super.getMinLimit(); + double maxLimit = super.getMaxLimit(); + + // Calculate the difference and normalize it to (-PI, PI] + // This ensures 'candidate1' is the shortest angular path from 'measurement' + double angleDifference = MathUtil.angleModulus(wantedSetPoint - measurement); + + // Candidate 1: The target reached by the shortest angular path + double candidate1 = measurement + angleDifference; + + // Candidate 2: The target reached by taking the "other way around" + // If angleDifference was positive (clockwise), candidate2 is counter-clockwise. + // If angleDifference was negative (counter-clockwise), candidate2 is clockwise. + double candidate2 = (angleDifference > 0) ? (candidate1 - TWO_PI) : (candidate1 + TWO_PI); + + // Check if candidates are within the soft limits + boolean candidate1Valid = candidate1 >= minLimit && candidate1 <= maxLimit; + boolean candidate2Valid = candidate2 >= minLimit && candidate2 <= maxLimit; + + if (candidate1Valid && candidate2Valid) + // Both paths lead to a valid angle, choose the one truly closer to the measurement + return Math.abs(measurement - candidate1) < Math.abs(measurement - candidate2) ? candidate1 : candidate2; + if (candidate1Valid) + // Only candidate1 is valid + return candidate1; + if (candidate2Valid) + // Only candidate2 is valid + return candidate2; + + // Neither of the primary cyclic candidates are valid. + // This means the 'wantedSetPoint' is unreachable given the current `measurement` + // and the `minLimit`/`maxLimit` constraints. + // In this case, the most robust action is to clamp the desired setpoint + // to the absolute bounds of the soft limit. This ensures the returned value + // is always within the safe range. + return super.limit(wantedSetPoint); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/limits/SoftLimit.java b/src/main/java/frc/excalib/control/limits/SoftLimit.java index e2c3bd6..3bd080e 100644 --- a/src/main/java/frc/excalib/control/limits/SoftLimit.java +++ b/src/main/java/frc/excalib/control/limits/SoftLimit.java @@ -1,5 +1,6 @@ package frc.excalib.control.limits; +import edu.wpi.first.math.MathUtil; import monologue.Annotations.Log; import monologue.Logged; @@ -44,13 +45,7 @@ public boolean within(double val) { */ @Log.NT public double limit(double val) { - if (within(val)) { - return val; - } - if (val > m_maxLimit.getAsDouble()) { - return m_maxLimit.getAsDouble(); - } - return m_minLimit.getAsDouble(); + return MathUtil.clamp(val, m_minLimit.getAsDouble(), m_maxLimit.getAsDouble()); } /** diff --git a/src/main/java/frc/excalib/control/math/Circle.java b/src/main/java/frc/excalib/control/math/Circle.java deleted file mode 100644 index 30c3f7b..0000000 --- a/src/main/java/frc/excalib/control/math/Circle.java +++ /dev/null @@ -1,102 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Represents a circle in a 2D plane, defined by its center and radius. - */ -public class Circle { - // The radius of the circle - public final double r; - // The center of the circle as a Translation2d object - public final Translation2d center; - - /** - * Constructs a Circle object with the specified center coordinates and radius. - * - * @param a The x-coordinate of the circle's center. - * @param b The y-coordinate of the circle's center. - * @param r The radius of the circle. - */ - public Circle(double a, double b, double r) { - this.center = new Translation2d(a, b); - this.r = r; - } - - /** - * Calculates the tangent line to the circle at a given point on its circumference. - * - * @param pointOnCircle A point on the circle's circumference. - * @return A Line object representing the tangent line. - */ - public Line getTangent(Translation2d pointOnCircle) { - return new Line( - pointOnCircle.getX() - center.getX(), - pointOnCircle.getY() - center.getY(), - -center.getX() * (pointOnCircle.getX() - center.getX()) - - center.getY() * (pointOnCircle.getY() - center.getY()) - - this.r * this.r - ); - } - - /** - * Calculates the tangent lines from a given external point to the circle. - * - * @param point A point outside or on the circle. - * @return An array of Line objects representing the tangent lines. - * Returns an empty array if the point is inside the circle. - */ - public Line[] getTangents(Translation2d point) { - if (point.getDistance(this.center) < this.r) { - return new Line[0]; - } else if (point.getDistance(this.center) == this.r) { - return new Line[]{ - getTangent(point) - }; - } - double centersDistance = this.center.getDistance(point); - double newRad = Math.sqrt(Math.pow(centersDistance, 2) - Math.pow(this.r, 2)); - Circle newCircle = new Circle(point.getX(), point.getY(), newRad); - Translation2d[] intersections = getInterSections(newCircle); - Translation2d firstTanPoint = intersections[0]; - Translation2d secondTanPoint = intersections[1]; - - return new Line[]{ - getTangent(firstTanPoint), - getTangent(secondTanPoint) - }; - } - - /** - * Calculates the intersection points between this circle and another circle. - * - * @param other The other circle to find intersections with. - * @return An array of Translation2d objects representing the intersection points. - * Returns an empty array if there are no intersections. - */ - public Translation2d[] getInterSections(Circle other) { - - if (other.center.getDistance(this.center) > other.r + r) return new Translation2d[0]; - if (other.center.getDistance(this.center) < Math.abs(other.r - this.r)) return new Translation2d[0]; - - if (other.center.getDistance(this.center) == other.r + r) { - return new Translation2d[]{ - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle())) - }; - } - if (other.center.getDistance(this.center) < Math.abs(other.r - this.r)) { - return new Translation2d[]{ // Check for edge case - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle())) - }; - } - Rotation2d alpha = new Rotation2d(Math.acos( - (Math.pow(other.r, 2) - Math.pow(this.r, 2) - Math.pow(this.center.getDistance(other.center), 2)) - / (-2 * this.center.getDistance(other.center) * this.r) - )); - return new Translation2d[]{ - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle().plus(alpha))), - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle().minus(alpha))) - }; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/Line.java b/src/main/java/frc/excalib/control/math/Line.java deleted file mode 100644 index 1613b96..0000000 --- a/src/main/java/frc/excalib/control/math/Line.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Represents a line in a 2D plane, defined by the equation ax + by + c = 0. - */ -public class Line { - // Coefficient of x in the line equation - public final double a; - // Coefficient of y in the line equation - public final double b; - // Constant term in the line equation - public final double c; - - /** - * Constructs a Line object with the specified coefficients. - * - * @param a The coefficient of x in the line equation. - * @param b The coefficient of y in the line equation. - * @param c The constant term in the line equation. - */ - public Line(double a, double b, double c) { - this.a = a; - this.b = b; - this.c = c; - } - - /** - * Finds the intersection point of this line with another line. - * - * @param other The other line to find the intersection with. - * @return A Translation2d object representing the intersection point. - * If the lines are parallel, the result may be undefined. - */ - public Translation2d findIntersection(Line other) { - return new Translation2d( - (other.b * c - b * other.c) / (b * other.a - other.b * a), - (other.a * c - a * other.c) / (a * other.b - other.a * b) - ); - } - - /** - * Returns a string representation of the line in the format "a: [a], b: [b], c: [c]". - * - * @return A string representation of the line. - */ - @Override - public String toString() { - return "a: " + a + ", b: " + b + ", c: " + c; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/MathUtils.java b/src/main/java/frc/excalib/control/math/MathUtils.java deleted file mode 100644 index a6c993e..0000000 --- a/src/main/java/frc/excalib/control/math/MathUtils.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Utility class for mathematical operations and geometry-related calculations. - */ -public class MathUtils { - - /** - * Ensures that the absolute value of the given number does not exceed the specified limit. - * - * @param val The value to be limited. - * @param sizeLimit The maximum allowable absolute value. - * @return The value limited to the specified maximum absolute value, preserving its sign. - */ - public static double minSize(double val, double sizeLimit) { - return Math.min(sizeLimit, Math.abs(val)) * Math.signum(val); - } - - /** - * Limits the given value to the specified limit. If the value exceeds the limit, it is clamped to the limit. - * - * @param limit The maximum allowable value (positive or negative). - * @param value The value to be limited. - * @return The value clamped to the specified limit. - */ - public static double limitTo(double limit, double value) { - if ((limit > 0 && limit < value) || (limit < 0 && limit > value)) { - return limit; - } - return value; - } - - /** - * Calculates the optimal target position for a robot to reach a target while avoiding a reef. - * - * @param robot The current position of the robot as a Translation2d. - * @param target The target position as a Translation2d. - * @param reefCenter The center of the reef as a Translation2d. - * @return The optimal target position as a Translation2d. - */ - public static Translation2d getTargetPose(Translation2d robot, Translation2d target, Translation2d reefCenter) { - double radius = reefCenter.getDistance(target); - Circle c = new Circle(reefCenter.getX(), reefCenter.getY(), radius); - Line[] tangents = c.getTangents(robot); - Rotation2d alpha = target.minus(reefCenter).getAngle(); - Rotation2d theta = robot.minus(reefCenter).getAngle(); - - // If the angle between the target and robot is less than 60 degrees, return the target directly. - if (Math.abs(alpha.minus(theta).getRadians()) < Math.PI / 3) { - return target; - } - - // If no tangents exist, calculate a point on the reef perimeter and find the intersection. - if (tangents.length == 0) { - Translation2d onPerimeter = reefCenter.plus(new Translation2d(radius, robot.minus(reefCenter).getAngle())); - Line tangent = c.getTangent(onPerimeter); - return tangent.findIntersection(c.getTangent(target)); - } - - // If tangents exist, find the intersection of the target tangent with the robot tangents. - Line targetTangent = c.getTangent(target); - if (tangents.length == 1) { - return targetTangent.findIntersection(tangents[0]); - } - - // Calculate the two possible intersection points and return the closer one to the target. - Translation2d translation1 = targetTangent.findIntersection(tangents[0]); - Translation2d translation2 = targetTangent.findIntersection(tangents[1]); - - if (target.getDistance(translation1) < target.getDistance(translation2)) { - return translation1; - } - return translation2; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/Vector2D.java b/src/main/java/frc/excalib/control/math/Vector2D.java deleted file mode 100644 index 9680aa4..0000000 --- a/src/main/java/frc/excalib/control/math/Vector2D.java +++ /dev/null @@ -1,164 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * A class representing a Vector in two dimensions; - * the Vector is defined by x value and y value - * - * @author Itay - */ -public class Vector2D { - private double m_x, m_y; - - /** - * A constructor that takes two doubles representing the x and y components: - * - * @param x The x component of the Vector - * @param y The y component of the Vector - */ - public Vector2D(double x, double y) { - this.m_x = x; - this.m_y = y; - } - - /** - * A constructor that takes a double representing the distance, - * and a Rotation2D representing the angle: - * - * @param magnitude The distance from the origin - * @param direction The angle of the Vector - */ - public Vector2D(double magnitude, Rotation2d direction) { - this.m_x = magnitude * direction.getCos(); - this.m_y = magnitude * direction.getSin(); - } - - /** - * A function to get the x component of the Vector - * - * @return x component - */ - public double getX() { - return m_x; - } - - /** - * A function to get the y component of the Vector - * - * @return y component - */ - public double getY() { - return m_y; - } - - /** - * A function to get the distance from the origin - * - * @return distance - */ - public double getDistance() { - return Math.hypot(m_x, m_y); - } - - /** - * A function to get the direction of the Vector - * - * @return direction - */ - public Rotation2d getDirection() { - return new Rotation2d(Math.atan2(m_y, m_x)); - } - - /** - * A function that adds another Vector to this Vector - * - * @param other The other vector to add - * @return a new Vector2D that represents the sum of the Vectors - */ - public Vector2D plus(Vector2D other) { - return new Vector2D(m_x + other.m_x, m_y + other.m_y); - } - - /** - * A function that multiplies the Vector by a scalar - * - * @param scalar The scalar to multiply by - * @return a new Vector2D that represents the scaled Vector - */ - public Vector2D mul(double scalar) { - return new Vector2D(m_x * scalar, m_y * scalar); - } - - /** - * A function that rotates the Vector by a given angle - * - * @param deltaDirection The angle to rotate by - * @return a new Vector2D that represents the rotated Vector - */ - public Vector2D rotate(Rotation2d deltaDirection) { - double cosTheta = deltaDirection.getCos(); - double sinTheta = deltaDirection.getSin(); - - double newX = m_x * cosTheta - m_y * sinTheta; - double newY = m_x * sinTheta + m_y * cosTheta; - - return new Vector2D(newX, newY); - } - - /** - * Sets the x component of the Vector - * - * @param x The new x component - */ - public void setX(double x) { - this.m_x = x; - } - - /** - * Sets the y component of the Vector - * - * @param y The new y component - */ - public void setY(double y) { - this.m_y = y; - } - - /** - * Sets the magnitude of the Vector while keeping its direction - * - * @param magnitude The new magnitude - */ - public void setMagnitude(double magnitude) { - double currentMagnitude = getDistance(); - if (currentMagnitude != 0) { - double scale = magnitude / currentMagnitude; - this.m_x *= scale; - this.m_y *= scale; - } else { - // When the current magnitude is zero, direction is undefined. - // Default to setting x to the magnitude and y to zero. - this.m_x = magnitude; - this.m_y = 0; - } - } - - /** - * Sets the direction of the Vector while keeping its magnitude - * - * @param direction The new direction - */ - public void setDirection(Rotation2d direction) { - double magnitude = getDistance(); - this.m_x = magnitude * direction.getCos(); - this.m_y = magnitude * direction.getSin(); - } - - public Vector2D limit(Vector2D limit) { - Vector2D output = new Vector2D(m_x, m_y); - output.rotate(limit.getDirection().unaryMinus()); - output.setX(MathUtils.limitTo(limit.getDistance(), output.m_x)); - output.setDirection(this.getDirection()); - return output; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/physics/Mass.java b/src/main/java/frc/excalib/control/math/physics/Mass.java deleted file mode 100644 index 5dde935..0000000 --- a/src/main/java/frc/excalib/control/math/physics/Mass.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.excalib.control.math.physics; - -import edu.wpi.first.math.geometry.Translation2d; - -import java.util.function.DoubleSupplier; - -/** - * This Class represents the mass of an object including size, center - * and utility functions. - */ -public class Mass { - private final DoubleSupplier m_xSupplier; - private final DoubleSupplier m_ySupplier; - private final double mass; - - /** - * returns a new Mass object - * - * @param xSupplier the x position of the Mass (m) - * @param ySupplier the y position of the Mass (m) - * @param mass the size value of the mass (kg) - */ - public Mass(DoubleSupplier xSupplier, DoubleSupplier ySupplier, double mass) { - this.m_xSupplier = xSupplier; - this.m_ySupplier = ySupplier; - this.mass = mass; - } - - /** - * @return the center of the mass as a Translation2d - */ - public Translation2d getCenterOfMass() { - return new Translation2d(m_xSupplier.getAsDouble(), m_ySupplier.getAsDouble()); - } - - /** - * @param other the mass to add - * @return a new Mass representing the mass of the system the includes both masses. - */ - public Mass add(Mass other) { - return new Mass( - () -> - (other.mass * other.m_xSupplier.getAsDouble() + - this.mass * this.m_xSupplier.getAsDouble()) / (other.mass + this.mass), - () -> - (other.mass * other.m_ySupplier.getAsDouble() + - this.mass * this.m_ySupplier.getAsDouble()) / (other.mass + this.mass), - other.mass + this.mass - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/motor/controllers/Motor.java b/src/main/java/frc/excalib/control/motor/Motor.java similarity index 98% rename from src/main/java/frc/excalib/control/motor/controllers/Motor.java rename to src/main/java/frc/excalib/control/motor/Motor.java index 278dd45..4e9a712 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/Motor.java +++ b/src/main/java/frc/excalib/control/motor/Motor.java @@ -1,4 +1,4 @@ -package frc.excalib.control.motor.controllers; +package frc.excalib.control.motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/Piston.java b/src/main/java/frc/excalib/control/motor/Piston.java new file mode 100644 index 0000000..a7ea87a --- /dev/null +++ b/src/main/java/frc/excalib/control/motor/Piston.java @@ -0,0 +1,27 @@ +package frc.excalib.control.motor; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import frc.excalib.control.motor.motor_specs.DirectionState; + +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; + +public class Piston { + private final DoubleSolenoid piston; + private DirectionState direction; + + public Piston(int fwdID, int revID, PneumaticsModuleType moduleType){ + this.piston = new DoubleSolenoid(moduleType, fwdID, revID); + this.direction = DirectionState.REVERSE; + } + + public void setPiston(DirectionState direction){ + this.piston.set(direction == DirectionState.FORWARD? kForward : kReverse); + this.direction = direction; + } + + public void togglePiston(){ + if (direction == DirectionState.FORWARD) setPiston(DirectionState.REVERSE); + else setPiston(DirectionState.FORWARD); + } +} diff --git a/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java b/src/main/java/frc/excalib/control/motor/controllers/SparkFlexMotor.java similarity index 95% rename from src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java rename to src/main/java/frc/excalib/control/motor/controllers/SparkFlexMotor.java index 2d1a4c3..91885b7 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/SparkFlexMotor.java @@ -2,6 +2,7 @@ import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.config.SparkFlexConfig; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; @@ -17,10 +18,10 @@ * This class represents a implemantation of a motor controller * using the Vortex Motor. */ -public class FlexMotor extends SparkFlex implements Motor { +public class SparkFlexMotor extends SparkFlex implements Motor { private final SparkFlexConfig m_config = new SparkFlexConfig(); - public FlexMotor(int deviceId, MotorType type) { + public SparkFlexMotor(int deviceId, MotorType type) { super(deviceId, type); } diff --git a/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java b/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java index 9db6826..3530c41 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java @@ -2,6 +2,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkFlexConfig; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java b/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java index a5dd60b..eca0de0 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java b/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java new file mode 100644 index 0000000..efb398d --- /dev/null +++ b/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java @@ -0,0 +1,50 @@ +package frc.excalib.control.motor.motorType; + +import edu.wpi.first.math.Pair; +import frc.excalib.control.motor.Motor; +import frc.excalib.control.motor.motorType.MotorGroup; + +import static frc.excalib.control.motor.motor_specs.DirectionState.*; + +public class DifferentialMotor extends MotorGroup { + private final Motor motorA, motorB; + + /** + * + * @param motorA first motor in the differential mechanism + * @param motorB second motor in the differential mechanism + * @param negate make sure that applying the same voltage causes rotation and not translation. + */ + public DifferentialMotor(Motor motorA, Motor motorB, boolean negate) { + super(motorA, motorB); + + this.motorA = motorA; + this.motorB = motorB; + + motorB.setInverted(negate? REVERSE : FORWARD); + } + + public void setDifferentialVoltage(double motorAvoltage, double motorBvoltage){ + super.setDifferentialVoltage(motorAvoltage, motorBvoltage); + } + + public Pair getMotorPositions(){ + return Pair.of(motorA.getMotorPosition(), motorB.getMotorPosition()); + } + + /** + * computes the avg of the two motors + * @return the rotation of the differential axle + */ + public double getMotorAvgPosition(){ + return (motorA.getMotorPosition() + motorB.getMotorPosition()) / 2.0; + } + + public double getMotorDifference(){ + return motorA.getMotorPosition() - motorB.getMotorPosition(); + } + + public double getMotorSum(){ + return motorA.getMotorPosition() + motorB.getMotorPosition(); + } +} diff --git a/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java b/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java similarity index 94% rename from src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java rename to src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java index 90bbc74..7af497c 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java +++ b/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java @@ -1,5 +1,6 @@ -package frc.excalib.control.motor.controllers; +package frc.excalib.control.motor.motorType; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; @@ -8,7 +9,6 @@ * Implements the Motor interface to provide unified control over multiple motors. */ public class MotorGroup implements Motor { - private final Motor[] m_motors; /** @@ -173,7 +173,7 @@ public double getTemperature() { * Sets a soft limit for all motors in the group. * * @param directionState The direction state for the limit. - * @param limit The soft limit value. + * @param limit The soft limit value. */ @Override public void setSoftLimit(DirectionState directionState, float limit) { @@ -222,7 +222,7 @@ public void setVelocityConversionFactor(double conversionFactor) { * Sets the current limits for all motors in the group. * * @param stallLimit The stall current limit. - * @param freeLimit The free current limit. + * @param freeLimit The free current limit. */ @Override public void setCurrentLimit(int stallLimit, int freeLimit) { @@ -242,4 +242,9 @@ public void setMotorPosition(double position) { motor.setMotorPosition(position); } } + + protected void setDifferentialVoltage(double voltageA, double voltageB) { + m_motors[0].setVoltage(voltageA); + m_motors[1].setVoltage(voltageB); + } } \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanism/Mechanism.java similarity index 85% rename from src/main/java/frc/excalib/mechanisms/Mechanism.java rename to src/main/java/frc/excalib/mechanism/Mechanism.java index 6db6e89..7959512 100644 --- a/src/main/java/frc/excalib/mechanisms/Mechanism.java +++ b/src/main/java/frc/excalib/mechanism/Mechanism.java @@ -5,13 +5,15 @@ * learn more at - https://github.com/ExaliburFRC/ExcaLIb */ -package frc.excalib.mechanisms; +package frc.excalib.mechanism; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.excalib.control.encoder.Encoder; import frc.excalib.control.gains.SysidConfig; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; +import frc.excalib.control.motor.motorType.DifferentialMotor; import frc.excalib.control.motor.motor_specs.IdleState; import monologue.Logged; @@ -26,12 +28,15 @@ */ public class Mechanism implements Logged { protected final Motor m_motor; + private final IdleState m_DEFAULT_IDLE_STATE; + + private Encoder m_externalEncoder; + protected final MutVoltage m_appliedVoltage = Volts.mutable(0); protected final MutAngle m_radians = Radians.mutable(0); protected final MutDistance m_meter = Meters.mutable(0); protected final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0); protected final MutLinearVelocity m_Linearvelocity = MetersPerSecond.mutable(0); - private final IdleState m_DEFAULT_IDLE_STATE; /** * @param motor the motor controlling the mechanism @@ -55,6 +60,18 @@ public void setVoltage(double voltage) { m_motor.setVoltage(voltage); } + protected void setDifferentialVoltage(double voltageA, double voltageB){ + if (!(this.m_motor instanceof DifferentialMotor)) { + System.out.println("setDifferentialVoltage only works with a DifferentialMotor"); + return; + } + ((DifferentialMotor) this.m_motor).setDifferentialVoltage(voltageA, voltageB); + } + + public void addExternalEncoder(Encoder externalEncoder){ + m_externalEncoder = externalEncoder; + } + /** * @param outputSupplier the dynamic setpoint for the mechanism (voltage) * @return a command which controls the mechanism manually @@ -70,7 +87,7 @@ public Command manualCommand(DoubleSupplier outputSupplier, SubsystemBase... req /** * @return an instant command to stop the motor */ - public Command stopMotorCommand(SubsystemBase... requirements) { + public Command stopMechnismCommand(SubsystemBase... requirements) { return new InstantCommand(() -> setOutput(0), requirements); } @@ -78,16 +95,18 @@ public Command stopMotorCommand(SubsystemBase... requirements) { * @return the velocity */ @Log.NT - public double logVelocity() { - return m_motor.getMotorVelocity(); + public double logMechanismVelocity() { + if (m_externalEncoder == null) return m_motor.getMotorVelocity(); + return m_externalEncoder.getVelocity(); } /** * @return the position */ @Log.NT - public double logPosition() { - return m_motor.getMotorPosition(); + public double logMechanismPosition() { + if (m_externalEncoder == null) return m_motor.getMotorPosition(); + return m_externalEncoder.getPosition(); } @Log.NT @@ -117,7 +136,7 @@ private SysIdRoutine getLinearSysIdRoutine(SubsystemBase subsystem, DoubleSuppli .voltage(m_appliedVoltage.mut_replace( m_motor.getVoltage(), Volts)) .linearPosition(m_meter.mut_replace(sensorInput.getAsDouble(), Meters)) - .linearVelocity(m_Linearvelocity.mut_replace(logVelocity(), MetersPerSecond)), + .linearVelocity(m_Linearvelocity.mut_replace(logMechanismVelocity(), MetersPerSecond)), subsystem ) ); @@ -140,7 +159,7 @@ private SysIdRoutine getAngularSysIdRoutine(SubsystemBase subsystem, DoubleSuppl .voltage(m_appliedVoltage.mut_replace( m_motor.getVoltage(), Volts)) .angularPosition(m_radians.mut_replace(sensorInput.getAsDouble(), Radians)) - .angularVelocity(m_velocity.mut_replace(logVelocity(), RadiansPerSecond)), + .angularVelocity(m_velocity.mut_replace(logMechanismVelocity(), RadiansPerSecond)), subsystem ) ); diff --git a/src/main/java/frc/excalib/mechanism/PositionMechanism.java b/src/main/java/frc/excalib/mechanism/PositionMechanism.java new file mode 100644 index 0000000..d8814a3 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/PositionMechanism.java @@ -0,0 +1,79 @@ +package frc.excalib.mechanism; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.encoder.Encoder; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.limits.SoftLimit; +import frc.excalib.control.motor.Motor; + +import java.util.function.BiFunction; +import java.util.function.DoubleSupplier; + +public class PositionMechanism extends Mechanism { + private double m_setpoint; + private PIDController m_pidController; + private BiFunction m_feedforwardCalculator; + private SoftLimit m_limit; + + public Trigger m_atSetpointTrigger; + + public PositionMechanism(Motor motor, Gains gains) { + super(motor); + m_pidController = gains.getPIDcontroller(); + + m_atSetpointTrigger = new Trigger(()-> false); + m_feedforwardCalculator = ((a, b)-> 0.0); + } + + public PositionMechanism withTolerance(double tolerance) { + m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), tolerance)); + return this; + } + + public PositionMechanism withFeedforwardCalculator(BiFunction calculator) { + m_feedforwardCalculator = calculator; + return this; + } + + public PositionMechanism withLimit(SoftLimit limit) { + m_limit = limit; + return this; + } + + public PositionMechanism withExternalEncoder(Encoder encoder) { + super.addExternalEncoder(encoder); + return this; + } + + public Command setPositionCommand(DoubleSupplier positionSupplier, TrapezoidProfile profile, SubsystemBase... requirements) { + return new RunCommand(() -> { + State state = profile.calculate( + 0.02, + new State(super.logMechanismPosition(), super.logMechanismVelocity()), + new State(positionSupplier.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); + } + + public void setPosition(double position) { + m_setpoint = position; + + if (m_limit != null) { + if (m_limit instanceof ContinuousSoftLimit) m_setpoint = ((ContinuousSoftLimit) m_limit).getSetpoint(super.logMechanismPosition(), position); + else m_setpoint = m_limit.limit(position); + } + + double pid = m_pidController.calculate(super.logMechanismPosition(), m_setpoint); + double ff = m_feedforwardCalculator.apply(super.logMechanismPosition(), super.logMechanismVelocity()); + super.setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/excalib/mechanism/VelocityMechanism.java b/src/main/java/frc/excalib/mechanism/VelocityMechanism.java new file mode 100644 index 0000000..9461607 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/VelocityMechanism.java @@ -0,0 +1,72 @@ +package frc.excalib.mechanism; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.encoder.Encoder; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.SoftLimit; +import frc.excalib.control.motor.Motor; + +import java.util.function.BiFunction; +import java.util.function.DoubleSupplier; + +public class VelocityMechanism extends Mechanism { + private double m_setpoint; + private final PIDController m_pidController; + private BiFunction m_feedforwardCalculator; + + public Trigger m_atSetpointTrigger; + + /** + * @param motor the Mechanism motor + * @param gains PID gains + */ + public VelocityMechanism(Motor motor, Gains gains) { + super(motor); + this.m_pidController = gains.getPIDcontroller(); + + m_atSetpointTrigger = new Trigger(()-> false); + m_feedforwardCalculator = ((a, b)-> 0.0); + } + + public VelocityMechanism withTolerance(double tolerance) { + m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismVelocity(), tolerance)); + return this; + } + + public VelocityMechanism withFeedforwardCalculator(BiFunction calculator) { + m_feedforwardCalculator = calculator; + return this; + } + + public VelocityMechanism withExternalEncoder(Encoder encoder) { + super.addExternalEncoder(encoder); + return this; + } + + /** + * sets the Mechanism to a Dynamic velocity + * + * @param velocity velocity supplier + * @param requirements subsystem requirements + * @return the command to set the FlyWheel velocity + */ + public Command setVelocityCommand(DoubleSupplier velocity, SubsystemBase... requirements) { + return new RunCommand(() -> setVelocity(velocity.getAsDouble()), requirements); + } + + /** + * @param velocity the velocity to set to the Mechanism + */ + public void setVelocity(double velocity) { + this.m_setpoint = velocity; + + double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), m_setpoint); + double ff = m_feedforwardCalculator.apply(super.logMechanismPosition(), super.logMechanismVelocity()); + super.setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java b/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java new file mode 100644 index 0000000..1761ef4 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java @@ -0,0 +1,61 @@ +package frc.excalib.mechanism.mechanisms; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.motor.Motor; +import frc.excalib.mechanism.Mechanism; + +import java.util.function.DoubleSupplier; + +/** + * This class represents an Arm Mechanism + */ +public class Arm extends Mechanism { + private final PIDController m_PIDController; + private final ArmFeedforward m_ffController; + + private final TrapezoidProfile m_profile; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + + public Arm(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { + super(motor); + m_PIDController = gains.getPIDcontroller(); + m_ffController = gains.applyGains(new GenericFF.ArmFF()); + + m_profile = new TrapezoidProfile(constraints); + + m_tolerance = tolerance; + m_setpoint = 0; + } + + public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements) { + return new RunCommand(() -> { + TrapezoidProfile.State state = m_profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); + } + + public void setPosition(double position){ + this.m_setpoint = position; + + double pid = m_PIDController.calculate(super.logMechanismPosition(), position); + double ff = m_ffController.calculate(super.logMechanismPosition(), super.logMechanismVelocity()); + setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/Differential.java b/src/main/java/frc/excalib/mechanism/mechanisms/Differential.java new file mode 100644 index 0000000..5ee45df --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Differential.java @@ -0,0 +1,99 @@ +package frc.excalib.mechanism.mechanisms; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.*; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.motor.motorType.DifferentialMotor; +import frc.excalib.control.motor.motor_specs.IdleState; +import frc.excalib.mechanism.Mechanism; +import monologue.Annotations.Log; + +import java.util.function.DoubleSupplier; + +public class Differential extends Mechanism { + private final PIDController pidController_A, pidController_B; + + private final DifferentialMotor motor; + + private final double differentialMul; + + private double deltaA, deltaB; + private double motorApos, motorBpos; + +// private DoubleSupplier angleSetpointSupplier, linearSetpointSupplier; +// DoubleSupplier angleSetpointSupplier, DoubleSupplier linearSetpointSupplier + + /** + * @param differentialMotor a differential motor object of both mechanism motors + * @param gains gains for pid & ff for the two motors + * @param differentialMul an optional multiplier to correct acc error in certain types of differential mechanism + */ + public Differential(DifferentialMotor differentialMotor, Gains gains, double differentialMul) { + super(differentialMotor); + + this.motor = differentialMotor; + this.motor.setIdleState(IdleState.BRAKE); + + this.pidController_A = gains.getPIDcontroller(); + this.pidController_B = gains.getPIDcontroller(); + + this.differentialMul = differentialMul; + } + + /** + * constructor for a manual diff, w/o advanced control capabilities + * @param differentialMotor + */ + public Differential(DifferentialMotor differentialMotor) { + this(differentialMotor, new Gains(),1); + } + + public Command moveToStateCommand(double angle, double position, double ff, SubsystemBase... requirements){ + return Commands.startRun( + ()-> { + double deltaTheta = angle - getMechanismAngle(); + double deltaPos = position - getMechanismPosition(); + + deltaA = deltaTheta + (deltaPos / (2 * differentialMul)); + deltaB = deltaTheta - (deltaPos / (2 * differentialMul)); + }, + ()-> { + this.motorApos = this.motor.getMotorPositions().getFirst(); + this.motorBpos = this.motor.getMotorPositions().getSecond(); + + setDifferentialVoltage( + pidController_A.calculate(motorApos, this.deltaA) + ff, + pidController_B.calculate(motorBpos, this.deltaB) + ff + ); + }, + requirements); + } + + public Command setDifferentialVoltageCommand(DoubleSupplier voltageA, DoubleSupplier voltageB, SubsystemBase... requirements){ + return new RunCommand( + ()-> setDifferentialVoltage(voltageA.getAsDouble(), voltageB.getAsDouble()), + requirements); + } + + public void move(double voltage) { + super.setDifferentialVoltage(voltage, -voltage); + } + + public void rotate(double voltage) { + super.setDifferentialVoltage(voltage, voltage); + } + + public void setDifferentialVoltage(double voltageA, double voltageB) { + super.setDifferentialVoltage(voltageA, voltageB); + } + + @Log.NT + public double getMechanismAngle() { + return this.motor.getMotorDifference(); + } + + @Log.NT + public double getMechanismPosition(){ + return this.differentialMul * this.motor.getMotorDifference(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java b/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java new file mode 100644 index 0000000..c0be1f5 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java @@ -0,0 +1,60 @@ +package frc.excalib.mechanism.mechanisms; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.motor.Motor; +import frc.excalib.mechanism.Mechanism; + +import java.util.function.DoubleSupplier; + +/** + * A class the represents A FlyWheel Mechanism. + */ +public class FlyWheel extends Mechanism { + private final PIDController m_pidController; + private final SimpleMotorFeedforward m_ffController; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + + /** + * @param motor the FlyWheel Motor + * @param gains the FF and PID gains + */ + public FlyWheel(Motor motor, Gains gains, double tolerance) { + super(motor); + this.m_pidController = gains.getPIDcontroller(); + this.m_ffController = gains.applyGains(new GenericFF.SimpleFF()); + + m_tolerance = tolerance; + m_setpoint = 0; + } + + /** + * sets the FlyWheel to a Dynamic velocity + * @param velocity velocity supplier + * @param requirements subsystem requirements + * @return the command to set the FlyWheel velocity + */ + public Command setDynamicVelocityCommand(DoubleSupplier velocity, SubsystemBase... requirements) { + return new RunCommand(() -> setVelocity(velocity.getAsDouble()), requirements); + } + + /** + * @param velocity the velocity to set to the FlyWheel Dynamically + */ + public void setVelocity(double velocity) { + this.m_setpoint = velocity; + + double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), velocity); + double ff = m_ffController.calculate(velocity); + super.setVoltage(pid + ff); + } +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java b/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java new file mode 100644 index 0000000..8e90281 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java @@ -0,0 +1,78 @@ +package frc.excalib.mechanism.mechanisms; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.motor.Motor; +import frc.excalib.mechanism.Mechanism; + +import java.util.function.DoubleSupplier; + +public class LinearExtension extends Mechanism { + private final PIDController m_pidController; + private final ElevatorFeedforward m_ffController; + + private final TrapezoidProfile m_profile; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + //--- + + private DoubleSupplier m_angleSupplier = ()-> 0; + private final Gains m_angleGains; + + public LinearExtension(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, DoubleSupplier angleSupplier, double tolerance) { + super(motor); + m_angleGains = gains; + m_pidController = gains.getPIDcontroller(); + m_ffController = gains.applyGains(new GenericFF.ElevatorFF()); + + m_profile = new TrapezoidProfile(constraints); + + m_tolerance = tolerance; + m_setpoint = 0; + } + + /** + * moves the LinearExtension to a Dynamic position with a trapezoid profile + * @param position position supplier + * @param requirements subsystem requirements + * @return the command to move the LinearExtension + */ + public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements){ + return new RunCommand(() -> { + TrapezoidProfile.State state = m_profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); + } + + /** + * allows for non floor-tangent linear extensions to add the angle for the FF calculations + * @param angleSupplier supplier for the angle between the mechanism and the ground + */ + public void setMechanismAngle(DoubleSupplier angleSupplier){ + this.m_angleSupplier = angleSupplier; + } + + public void setPosition(double position){ + this.m_setpoint = position; + + double pid = m_pidController.calculate(super.logMechanismPosition(), position); + double ff = (m_ffController.calculate(super.logMechanismVelocity()) - m_angleGains.kg) + + m_angleGains.kg * Math.sin(m_angleSupplier.getAsDouble()); + setVoltage(pid + ff); + } +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java b/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java new file mode 100644 index 0000000..76be490 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java @@ -0,0 +1,84 @@ +package frc.excalib.mechanism.mechanisms; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.motor.Motor; +import frc.excalib.mechanism.Mechanism; + +import java.util.function.DoubleSupplier; + +/** + * A class representing a Turret Mechanism + */ +public final class Turret extends Mechanism { + private final PIDController m_pidController; + private final SimpleMotorFeedforward m_ffController; + + private final TrapezoidProfile m_profile; + private final ContinuousSoftLimit m_rotationLimit; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + + /** + * @param motor the turret's motor + * @param rotationLimit the rotational boundary for the turret (radians) + * @param angleGains pid gains for the turret + * @param constraints constraints for the TrapezoidProfile + */ + public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, TrapezoidProfile.Constraints constraints, double tolerance) { + super(motor); + m_rotationLimit = rotationLimit; + + m_pidController = angleGains.getPIDcontroller(); + m_ffController = angleGains.applyGains(new GenericFF.SimpleFF()); + + m_pidController.enableContinuousInput(-180, 180); + + m_profile = new TrapezoidProfile(constraints); + + m_tolerance = tolerance; + m_setpoint = 0; + } + + /** + * moves the Turret to a Dynamic position with a Trapezoid profile + * @param position position supplier + * @param requirements subsystem requirements + * @return the command to move the Turret + */ + public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements){ + return new RunCommand(() -> { + TrapezoidProfile.State state = m_profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); + } + + /** + * moves the turret to the desired position + * @param setpoint the wanted position of the turret. + */ + public void setPosition(double setpoint) { + this.m_setpoint = setpoint; + + double limitedSetpoint = m_rotationLimit.getSetpoint(super.logMechanismPosition(), setpoint); + double pid = m_pidController.calculate(super.logMechanismPosition(), limitedSetpoint); + double ff = m_ffController.getKs() * Math.signum(pid); + super.setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/excalib/mechanisms/arm/Arm.java b/src/main/java/frc/excalib/mechanisms/arm/Arm.java deleted file mode 100644 index 4354282..0000000 --- a/src/main/java/frc/excalib/mechanisms/arm/Arm.java +++ /dev/null @@ -1,73 +0,0 @@ -package frc.excalib.mechanisms.arm; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.limits.SoftLimit; -import frc.excalib.control.math.physics.Mass; -import frc.excalib.control.motor.controllers.Motor; -import frc.excalib.mechanisms.Mechanism; - -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; - -/** - * This class represents an Arm Mechanism - */ -public class Arm extends Mechanism { - private final Mass m_mass; - private final PIDController m_PIDController; - public final DoubleSupplier ANGLE_SUPPLIER; - public final double m_kv, m_ks, m_kg; - public final SoftLimit m_VELOCITY_LIMIT; - - public Arm(Motor motor, DoubleSupplier angleSupplier, - SoftLimit velocityLimit, Gains gains, Mass mass) { - super(motor); - ANGLE_SUPPLIER = angleSupplier; - m_VELOCITY_LIMIT = velocityLimit; - m_kg = gains.kg; - m_kv = gains.kv; - m_ks = gains.ks; - m_PIDController = new PIDController(gains.kp, gains.ki, gains.kd); - m_mass = mass; - } - - /** - * @param setPointSupplier the dynamic angle setpoint to go to (radians) - * @param toleranceConsumer gets updated if the measurement is at tolerance. - * @return a command that moves the arm to the specified dynamic setpoint. - */ - public Command anglePositionControlCommand( - DoubleSupplier setPointSupplier, Consumer toleranceConsumer, - double maxOffSet, SubsystemBase... requirements) { - final double dutyCycle = 0.02; - return new RunCommand( - () -> { - double error = setPointSupplier.getAsDouble() - ANGLE_SUPPLIER.getAsDouble(); - double velocitySetpoint = error / dutyCycle; - velocitySetpoint = m_VELOCITY_LIMIT.limit(velocitySetpoint); - double phyOutput = - m_ks * Math.signum(velocitySetpoint) + - m_kg * m_mass.getCenterOfMass().getX(); - double pid = m_PIDController.calculate( - ANGLE_SUPPLIER.getAsDouble(), - setPointSupplier.getAsDouble() - ); - double output = phyOutput + pid; - super.setVoltage(m_VELOCITY_LIMIT.limit(output)); - toleranceConsumer.accept(Math.abs(error) < maxOffSet); - }, requirements); - } - - /** - * @param angle the angle setpoint to go to (radians) - * @param toleranceConsumer gets updated if the measurement is at tolerance. - * @return a command that moves the arm to the specified setpoint. - */ - public Command goToAngleCommand(double angle, Consumer toleranceConsumer, double maxOffSet, SubsystemBase... requirements) { - return anglePositionControlCommand(() -> angle, toleranceConsumer, maxOffSet, requirements); - } -} diff --git a/src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java b/src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java deleted file mode 100644 index 9942f23..0000000 --- a/src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.excalib.mechanisms.diffrential; - -import frc.excalib.control.motor.controllers.MotorGroup; -import frc.excalib.control.motor.motor_specs.IdleState; -import frc.excalib.mechanisms.Mechanism; - -import java.util.function.DoubleSupplier; - -public class Diffrential extends Mechanism { - private double setpoint; - public Diffrential(MotorGroup motorGroup, DoubleSupplier angleSetpointSupplier, DoubleSupplier angleSupplier, DoubleSupplier setpointSupplier) { - super(motorGroup); - motorGroup.setIdleState(IdleState.BRAKE); - } - - public void setVoltage(DoubleSupplier firstMotorVoltage, DoubleSupplier secondMotorVoltage) { - super.setVoltage((firstMotorVoltage.getAsDouble() + secondMotorVoltage.getAsDouble()) /2); - } - - public void setAngleSupplier(DoubleSupplier angleSupplier){ - - } - - - -} diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java deleted file mode 100644 index 28160d8..0000000 --- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java +++ /dev/null @@ -1,100 +0,0 @@ -package frc.excalib.mechanisms.fly_wheel; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.motor.controllers.Motor; -import frc.excalib.mechanisms.Mechanism; - -import java.util.function.DoubleSupplier; - -/** - * A class the represents A FlyWheel Mechanism. - */ -public class FlyWheel extends Mechanism { - private double lastTime, lastVelocity; - private final PIDController m_pidController; - private final double maxAcceleration; - private final double maxJerk; - private final Gains m_gains; - private final SimpleMotorFeedforward m_FF_CONTROLLER; - - /** - * @param motor the FlyWheel Motor - * @param maxAcceleration the max acceleration of the FlyWheel - * @param maxJerk the max jerk of the FlyWheel - * @param gains the FF and PID gains - */ - public FlyWheel(Motor motor, double maxAcceleration, double maxJerk, Gains gains) { - super(motor); - m_gains = gains; - this.maxAcceleration = maxAcceleration; - this.maxJerk = maxJerk; - this.m_pidController = new PIDController(m_gains.kp, m_gains.ki, m_gains.kd); - this.m_FF_CONTROLLER = new SimpleMotorFeedforward(gains.ks, gains.kv, gains.ka); - } - - /** - * @param velocitySupplier a dynamic velocity setpoint. - * @return a command that controls the FlyWheels velocity with high precision - */ - public Command smartVelocityCommand(DoubleSupplier velocitySupplier, SubsystemBase... requirements) { - return new RunCommand( - () -> { - TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAcceleration, maxJerk)); - TrapezoidProfile.State state = profile.calculate( - 0.02, - new TrapezoidProfile.State(super.m_motor.getMotorVelocity(), getAcceleration()), - new TrapezoidProfile.State(velocitySupplier.getAsDouble(), 0)); - double ff = m_gains.ks * Math.signum(state.position) + - m_gains.kv * state.position + - m_gains.ka * state.velocity; - double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), state.position); - setVoltage(pid + ff); - }); - } - - /** - * @return the FlyWheels current acceleration - */ - private double getAcceleration() { - //TODO: move to current based calculations - double currentTime = Timer.getFPGATimestamp(); - double currentVelocity = super.m_motor.getMotorVelocity(); - return (currentVelocity - lastVelocity) / (currentTime - lastTime); - } - - /** - * @param velocity the dynamic velocity setpoint - * @return a command which controls the FlyWheels velocity - */ - public Command setDynamicVelocityCommand(DoubleSupplier velocity, SubsystemBase... requirements) { - return new RunCommand(() -> setDynamicVelocity(velocity.getAsDouble()), requirements); - } - - /** - * @param velocity the velocity to set to the FlyWheel Dynamically - */ - public void setDynamicVelocity(double velocity) { - double ff = m_FF_CONTROLLER.calculate(velocity); - double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), velocity); - super.setVoltage(pid + ff); - } - - public void periodic() { - lastTime = Timer.getFPGATimestamp(); - lastVelocity = super.m_motor.getMotorVelocity(); - } - - /** - * @return the current velocity of the FlyWheel. - */ - public double getVelocity() { - return super.m_motor.getMotorVelocity(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java deleted file mode 100644 index f6d4892..0000000 --- a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java +++ /dev/null @@ -1,71 +0,0 @@ -package frc.excalib.mechanisms.linear_extension; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.motor.controllers.Motor; -import frc.excalib.mechanisms.Mechanism; - -import java.util.function.DoubleSupplier; - -public class LinearExtension extends Mechanism { - private final DoubleSupplier m_positionSupplier; - private final DoubleSupplier m_angleSupplier; - private final PIDController m_PIDController; - private final double m_tolerance; - private final Gains m_gains; - - private final TrapezoidProfile.Constraints m_constraints; - - public LinearExtension(Motor motor, DoubleSupplier positionSupplier, DoubleSupplier angleSupplier, - Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { - super(motor); - m_positionSupplier = positionSupplier; - m_angleSupplier = angleSupplier; - m_gains = gains; - m_PIDController = new PIDController(gains.kp, gains.ki, gains.kd); - m_constraints = constraints; - m_tolerance = tolerance; - } - - public Command extendCommand(DoubleSupplier lengthSetPoint, SubsystemBase... requirements) { - return new RunCommand(() -> { - TrapezoidProfile profile = new TrapezoidProfile(m_constraints); - TrapezoidProfile.State state = - profile.calculate( - 0.02, - new TrapezoidProfile.State( - m_positionSupplier.getAsDouble(), - super.m_motor.getMotorVelocity()), - new TrapezoidProfile.State(lengthSetPoint.getAsDouble(), 0) - ); - double pidValue = m_PIDController.calculate(m_positionSupplier.getAsDouble(), state.position); - double ff = - (Math.abs(m_positionSupplier.getAsDouble() - lengthSetPoint.getAsDouble()) > m_tolerance) ? - - m_gains.ks * Math.signum(state.velocity) + - m_gains.kv * state.velocity + - m_gains.kg * Math.sin(m_angleSupplier.getAsDouble()) : - - m_gains.kg * Math.sin(m_angleSupplier.getAsDouble()); - double output = ff + pidValue; - setVoltage(output); - }, requirements); - } - - public double logVoltage() { - return m_motor.getVoltage(); - } - - public double logVelocity() { - return m_motor.getMotorVelocity(); - } - - public double logPosition() { - return m_positionSupplier.getAsDouble(); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/turret/Turret.java deleted file mode 100644 index 7803b86..0000000 --- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.excalib.mechanisms.turret; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.limits.ContinuousSoftLimit; -import frc.excalib.control.motor.controllers.Motor; -import frc.excalib.mechanisms.Mechanism; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -/** - * A class representing a Turret Mechanism - */ -public final class Turret extends Mechanism { - private final ContinuousSoftLimit m_rotationLimit; - private final PIDController m_anglePIDcontroller; - private final SimpleMotorFeedforward m_angleFFcontroller; - private final DoubleSupplier m_POSITION_SUPPLIER; - - /** - * @param motor the turrets motor - * @param rotationLimit the rotational boundary for the turret (radians) - * @param angleGains pid gains for the turret - * @param PIDtolerance pid tolerance for the turret (radians) - * @param positionSupplier the position measurement - */ - public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, double PIDtolerance, DoubleSupplier positionSupplier) { - super(motor); - m_rotationLimit = rotationLimit; - - m_anglePIDcontroller = new PIDController(angleGains.kp, angleGains.ki, angleGains.kd); - m_angleFFcontroller = new SimpleMotorFeedforward(angleGains.ks, angleGains.kv, angleGains.ka); - - m_anglePIDcontroller.setTolerance(PIDtolerance); - m_anglePIDcontroller.enableContinuousInput(-Math.PI, Math.PI); - - m_POSITION_SUPPLIER = positionSupplier; - } - - /** - * @param wantedPosition a Rotation2d dynamic setpoint - * @return a Command that moves the turret tho the given setpoint - */ - public Command setPositionCommand(Supplier wantedPosition, SubsystemBase... requirements){ - return new RunCommand(()-> setPosition(wantedPosition.get()), requirements); - } - - /** - * moves the turret to the desired position - * @param wantedPosition the wanted position of the turret. - */ - public void setPosition(Rotation2d wantedPosition) { - double smartSetPoint = m_rotationLimit.getSetPoint(getPosition().getRadians(), wantedPosition.getRadians()); - double pid = m_anglePIDcontroller.calculate(m_POSITION_SUPPLIER.getAsDouble(), smartSetPoint); -// double ff =m_angleFFcontroller.getKs() * Math.signum(pid); - super.setVoltage(pid); - } - - /** - * @return get the position if the turret - */ - public Rotation2d getPosition() { - return new Rotation2d(m_POSITION_SUPPLIER.getAsDouble()); - } - - /** - * @return an Instant Command to stop the turret - */ - public Command stopTurret(SubsystemBase... requirements) { - return new InstantCommand(super.m_motor::stopMotor, requirements); - } -} 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/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..e105301 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,35 @@ +package frc.robot; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.limits.SoftLimit; + +public class Constants { + public static final class CannonConstants{ + + // turret constants + public static final double UPPER_LIMIT = 450; + public static final double LOWER_LIMIT = 0; + + public static final double TURRET_POSITION_CONVERSION_FACTOR = 1.0 / 150.0 * 360; + public static final double TURRET_VELOCITY_CONVERSION_FACTOR = 1.0 / 150.0 * 360; + + public static final ContinuousSoftLimit TURRET_SOFT_LIMIT = new ContinuousSoftLimit(()-> UPPER_LIMIT, ()-> LOWER_LIMIT); + public static final Gains TURRET_GAINS = new Gains(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7); + public static final TrapezoidProfile.Constraints TURRET_CONSTRAINTS = new TrapezoidProfile.Constraints(6.28, 3.14); + + public static final double TURRET_TOLERANCE = 3; // deg + + // arm constants + public static final Gains ARM_GAINS = new Gains(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7); + public static final TrapezoidProfile.Constraints ARM_CONSTRAINTS = new TrapezoidProfile.Constraints(6.28, 3.14); + + public static final SoftLimit ARM_SOFT_LIMITS = new SoftLimit(()-> 0, ()-> 90); + + public static final double ARM_POSITION_CONVERSION_FACTOR = 1.0 / 452.0 * 360; + public static final double ENCODER_OFFSET = 18.75; // deg + + public static final double ARM_TOLERANCE = 3; // deg + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 31f6d16..e8817f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,17 +5,45 @@ package frc.robot; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import frc.robot.subsystems.AdjustableShooter; +import frc.robot.subsystems.Cannon; +import frc.robot.subsystems.Swerve; public class RobotContainer { - public RobotContainer() { - configureBindings(); - } + Cannon cannon = new Cannon(); + AdjustableShooter adjShooter = new AdjustableShooter(); + Swerve swerve = new Swerve(); - private void configureBindings() {} + InterpolatingDoubleTreeMap velInterpolate = new InterpolatingDoubleTreeMap(); + InterpolatingDoubleTreeMap angleInterpolate = new InterpolatingDoubleTreeMap(); - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + CommandPS5Controller controller = new CommandPS5Controller(0); + + public RobotContainer() { + configureBindings(); + } + + private void configureBindings() { + + // move and shoot to target when cross is pressed + controller.cross().onTrue(cannon.shootToPositionCommand(swerve::getRotationToTarget, swerve::getPitchToTarget)); + + // toggle manual control when pressing square + controller.square().toggleOnTrue(cannon.manualControlCommand( + controller::getRightX, controller::getLeftY, controller.L1() //move the turret with joysticks, shoot with L1 + )); + + controller.triangle().onTrue(adjShooter.setShooterStateCommand( + () -> velInterpolate.get(swerve.getDistanceFromTarget()), + () -> angleInterpolate.get(swerve.getDistanceFromTarget()) + )); + } + + public Command getAutonomousCommand() { + return Commands.print("No autonomous command configured"); + } } diff --git a/src/main/java/frc/robot/SwerveConstants.java b/src/main/java/frc/robot/SwerveConstants.java deleted file mode 100644 index 999bc70..0000000 --- a/src/main/java/frc/robot/SwerveConstants.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.hardware.CANcoder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.imu.IMU; -import frc.excalib.control.imu.Pigeon; - -public class SwerveConstants { - public static final int FRONT_LEFT_DRIVE_ID = 0; - public static final int FRONT_RIGHT_DRIVE_ID = 0; - public static final int BACK_RIGHT_DRIVE_ID = 0; - public static final int BACK_LEFT_DRIVE_ID = 0; - - public static final int FRONT_LEFT_ROTATION_ID = 0; - public static final int FRONT_RIGHT_ROTATION_ID = 0; - public static final int BACK_RIGHT_ROTATION_ID = 0; - public static final int BACK_LEFT_ROTATION_ID = 0; - - public static final int GYRO_ID = 0; - public static final String SWERVE_CANBUS = "Swerve"; - - public static final double PID_TOLERANCE = 0; - - public static final double TRACK_WIDTH = 0; // Meters - public static final Translation2d FRONT_LEFT_TRANSLATION = new Translation2d(TRACK_WIDTH / 2, TRACK_WIDTH / 2); - public static final Translation2d FRONT_RIGHT_TRANSLATION = new Translation2d(TRACK_WIDTH / 2, -TRACK_WIDTH / 2); - public static final Translation2d BACK_LEFT_TRANSLATION = new Translation2d(-TRACK_WIDTH / 2, TRACK_WIDTH / 2); - public static final Translation2d BACK_RIGHT_TRANSLATION = new Translation2d(-TRACK_WIDTH / 2, -TRACK_WIDTH / 2); - - public static final double MAX_MODULE_VEL = 0; - public static final double MAX_FRONT_ACC = 0; - public static final double MAX_SIDE_ACC = 0; - public static final double MAX_SKID_ACC = 0; - public static final double MAX_FORWARD_ACC = 0; - public static final double MAX_VEL = 0; - public static final double MAX_OMEGA_RAD_PER_SEC = 0; - public static final double MAX_OMEGA_RAD_PER_SEC_SQUARE = 0; - - public static final double ANGLE_CONTROLLER_TOLORANCE = 0; - public static final double X_CONTROLLER_TOLORANCE = 0; - public static final double Y_CONTROLLER_TOLORANCE = 0; - - public static final PathConstraints MAX_PATH_CONSTRAINTS = new PathConstraints( - MAX_VEL, - MAX_SKID_ACC, - MAX_OMEGA_RAD_PER_SEC, - MAX_OMEGA_RAD_PER_SEC_SQUARE, - 12.0, - false - ); - - public static final CANcoder FRONT_LEFT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - public static final CANcoder FRONT_RIGHT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - public static final CANcoder BACK_RIGHT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - public static final CANcoder BACK_LEFT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - - public static final double VELOCITY_CONVERSION_FACTOR = 0; - public static final double POSITION_CONVERSION_FACTOR = 0; - public static final double ROTATION_VELOCITY_CONVERSION_FACTOR = 0; - - public static final PIDConstants TRANSLATION_PID_CONSTANTS = new PIDConstants(0, 0, 0); - public static final PIDConstants ANGLE_PID_CONSTANTS = new PIDConstants(0, 0, 0); - public static final Gains ANGLE_GAINS = new Gains(0, 0, 0); - public static final Gains TRANSLATION_GAINS = new Gains(0, 0, 0); - - public static final Gains SWERVE_DRIVE_MODULE_GAINS = new Gains(0, 0, 0, 0, 0, 0,0); - public static final Gains SWERVE_ROTATION_MODULE_GAINS = new Gains(0, 0, 0, 0, 0, 0,0); - - public static final IMU GYRO = new Pigeon(GYRO_ID, SWERVE_CANBUS, new Rotation3d()); - - -} - diff --git a/src/main/java/frc/robot/subsystems/AdjustableShooter.java b/src/main/java/frc/robot/subsystems/AdjustableShooter.java new file mode 100644 index 0000000..7aaf258 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AdjustableShooter.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.*; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.motor.controllers.SparkFlexMotor; +import frc.excalib.control.motor.controllers.TalonFXMotor; +import frc.excalib.control.motor.motorType.MotorGroup; +import frc.excalib.control.motor.motor_specs.DirectionState; +import frc.excalib.mechanism.PositionMechanism; +import frc.excalib.mechanism.VelocityMechanism; + +import java.util.function.DoubleSupplier; + +import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless; +import static frc.robot.Constants.CannonConstants.*; + +public class AdjustableShooter extends SubsystemBase{ + // Subsystem Hardware components + private final SparkFlexMotor flywheelMotorA, flywheelMotorB; + private final TalonFXMotor hoodMotor; + + // Subsystem mechanisms + private final PositionMechanism hood; + private final VelocityMechanism shooter; + + public AdjustableShooter() { + // initialize hardware + this.flywheelMotorA = new SparkFlexMotor(1, kBrushless); + this.flywheelMotorB = new SparkFlexMotor(2, kBrushless); + + this.hoodMotor = new TalonFXMotor(3); + + // reverse one of the motors in the gearbox + this.flywheelMotorB.setInverted(DirectionState.REVERSE); + + // setup shooterMotors conversion factors + MotorGroup shooterMotors = new MotorGroup(flywheelMotorA, flywheelMotorB); + shooterMotors.setPositionConversionFactor(TURRET_POSITION_CONVERSION_FACTOR); + shooterMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); + + // create turret and arm mechanisms + this.hood = new PositionMechanism(hoodMotor, TURRET_GAINS) + .withTolerance(TURRET_TOLERANCE).withLimit(TURRET_SOFT_LIMIT) + .withFeedforwardCalculator((position, velocity)-> TURRET_GAINS.ks * Math.signum(velocity)); + + this.shooter = new VelocityMechanism(shooterMotors, ARM_GAINS) + .withTolerance(ARM_TOLERANCE) + .withFeedforwardCalculator((position, velocity)-> ARM_GAINS.applyGains(new GenericFF.SimpleFF()).calculate(velocity)); + + setDefaultCommand(setShooterStateCommand(() -> 0, ()-> 0)); + } + + private Command moveHoodToPositionCommand(DoubleSupplier position) { + return this.hood.setPositionCommand(position, new TrapezoidProfile(ARM_CONSTRAINTS)); + } + + private Command setShooterVelocityCommand(DoubleSupplier velocity) { + return this.shooter.setVelocityCommand(velocity); + } + + public Command setShooterStateCommand(DoubleSupplier hoodPosition, DoubleSupplier velocity) { + return new ParallelCommandGroup( + new RunCommand(()-> {}, this), + moveHoodToPositionCommand(hoodPosition), + setShooterVelocityCommand(velocity) + ); + } + + public double getHoodPosition() { + return this.hood.logMechanismPosition(); + } + + public double getShooterVelocity() { + return this.shooter.logMechanismVelocity(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java new file mode 100644 index 0000000..b4527e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.encoder.DutyCycleEncoder; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.motor.Piston; +import frc.excalib.control.motor.controllers.SparkFlexMotor; +import frc.excalib.control.motor.controllers.TalonFXMotor; +import frc.excalib.control.motor.motorType.MotorGroup; +import frc.excalib.control.motor.motor_specs.DirectionState; +import frc.excalib.mechanism.mechanisms.Arm; +import frc.excalib.mechanism.mechanisms.LinearExtension; +import frc.excalib.mechanism.mechanisms.Turret; + +import java.util.function.DoubleSupplier; + +import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless; +import static frc.robot.Constants.CannonConstants.*; + +public class Cannon extends SubsystemBase { + + // Subsystem Hardware components + private final SparkFlexMotor rotationMotorA, rotationMotorB; + private final TalonFXMotor pitchMotor; + private final Piston primer; + + private final DutyCycleEncoder angleEncoder; + + // Subsystem mechanisms + private final Turret turret; + private final Arm arm; + + public Cannon() { + // initialize hardware + this.rotationMotorA = new SparkFlexMotor(1, kBrushless); + this.rotationMotorB = new SparkFlexMotor(2, kBrushless); + + this.pitchMotor = new TalonFXMotor(3); + this.primer = new Piston(1, 2, PneumaticsModuleType.REVPH); + + this.angleEncoder = new DutyCycleEncoder(3); + + // reverse one of the motors in the gearbox + this.rotationMotorB.setInverted(DirectionState.REVERSE); + + // set the encoder's offset and conversion factor + this.angleEncoder.setPositionConversionFactor(ARM_POSITION_CONVERSION_FACTOR); + this.angleEncoder.setPosition(ENCODER_OFFSET, true); + + // setup rotationMotors conversion factors + MotorGroup rotationMotors = new MotorGroup(rotationMotorA, rotationMotorB); + rotationMotors.setPositionConversionFactor(TURRET_POSITION_CONVERSION_FACTOR); + rotationMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); + + // create turret and arm mechanisms + this.turret = new Turret(rotationMotors, TURRET_SOFT_LIMIT, TURRET_GAINS, TURRET_CONSTRAINTS, TURRET_TOLERANCE); + this.arm = new Arm(this.pitchMotor, ARM_GAINS, ARM_CONSTRAINTS, ARM_TOLERANCE); + + // set the arm mechanism to use an external encoder rather than the motor's built in + this.arm.addExternalEncoder(this.angleEncoder); + + setDefaultCommand(moveToPositionCommand(() -> 0, () -> 0)); + } + + private Command moveToPositionCommand(DoubleSupplier rotation, DoubleSupplier pitch) { + return new ParallelCommandGroup( + new RunCommand(() -> { + }, this), // add requirements to the command + this.turret.setDynamicPositionCommand(rotation), + this.arm.setDynamicPositionCommand(pitch) + ); + } + + private Command shootCommand() { + return new StartEndCommand( + () -> this.primer.setPiston(DirectionState.FORWARD), + () -> this.primer.setPiston(DirectionState.REVERSE) + ); + } + + public Command shootToPositionCommand(DoubleSupplier rotation, DoubleSupplier pitch) { + return moveToPositionCommand(rotation, pitch) + .until(turret.atSetpointTrigger.and(arm.atSetpointTrigger)) + .andThen(shootCommand() + ); + } + + public Command manualControlCommand(DoubleSupplier rotation, DoubleSupplier pitch, Trigger shoot) { + return new ParallelCommandGroup( + moveToPositionCommand( + () -> getCannonRotation() + rotation.getAsDouble(), // rotate up to 5 deg a sec + () -> getCannonPitch() + pitch.getAsDouble()), // pitch up to 5 deg a sec + new WaitUntilCommand(shoot).andThen(shootCommand()) + ); + } + + + public double getCannonRotation() { + return this.turret.logMechanismPosition(); + } + + public double getCannonPitch() { + return this.arm.logMechanismPosition(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java new file mode 100644 index 0000000..dc0cbf8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems; + +public class Swerve { + public double getRotationToTarget(){ + return 0; + } + + public double getPitchToTarget(){ + return 0; + } + + public double getDistanceFromTarget(){ + return 0; + } +}