diff --git a/src/main/java/frc/lib/Elastic.java b/src/main/java/frc/lib/Elastic.java new file mode 100644 index 0000000..8d0cc95 --- /dev/null +++ b/src/main/java/frc/lib/Elastic.java @@ -0,0 +1,390 @@ +// Copyright (c) 2023-2026 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.lib; + +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(); + + /** + * 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 + } + + /** + * 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; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/FieldConstants.java b/src/main/java/frc/lib/FieldConstants.java new file mode 100644 index 0000000..37158be --- /dev/null +++ b/src/main/java/frc/lib/FieldConstants.java @@ -0,0 +1,354 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.lib; + +import com.fasterxml.jackson.databind.ObjectMapper; +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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; + +/** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ +public class FieldConstants { + public static final FieldType fieldType = FieldType.WELDED; + + // AprilTag related constants + public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); + public static final double aprilTagWidth = Units.inchesToMeters(6.5); + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + // Field dimensions + public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); + public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - Units.inchesToMeters(120); + public static final double neutralZoneFar = center + Units.inchesToMeters(120); + public static final double oppHubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + + // Dimensions + public static final double width = Units.inchesToMeters(47.0); + public static final double height = + Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = Units.inchesToMeters(41.7); + public static final double innerHeight = Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + } + + public static class RightTrench { + + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + public static final double width = Units.inchesToMeters(49.25); + public static final double depth = Units.inchesToMeters(45.0); + public static final double height = Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = Units.inchesToMeters(32.250); + public static final double frontFaceX = Units.inchesToMeters(43.51); + + public static final double uprightHeight = Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = Units.inchesToMeters(27.0); + public static final double midRungHeight = Units.inchesToMeters(45.0); + public static final double highRungHeight = Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d( + frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + } + + public static class Depot { + // Dimensions + public static final double width = Units.inchesToMeters(42.0); + public static final double depth = Units.inchesToMeters(27.0); + public static final double height = Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + } + + public static class Outpost { + // Dimensions + public static final double width = Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); + public static final double height = Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); + } + + public enum FieldType { + ANDYMARK("andymark"), + WELDED("welded"); + + private final String name; + + FieldType(String name) { + this.name = name; + } + } + + public enum AprilTagLayoutType { + OFFICIAL("2026-official"), + NONE("2026-none"); + + private final String name; + private volatile AprilTagFieldLayout layout; + private volatile String layoutString; + + AprilTagLayoutType(String name) { + this.name = name; + } + + public AprilTagFieldLayout getLayout() { + if (layout == null) { + synchronized (this) { + if (layout == null) { + try { + layout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + } + } + return layout; + } + + public String getLayoutString() { + if (layoutString == null) { + getLayout(); + } + return layoutString; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/LowPassFilter.java b/src/main/java/frc/lib/LowPassFilter.java new file mode 100644 index 0000000..2f0c966 --- /dev/null +++ b/src/main/java/frc/lib/LowPassFilter.java @@ -0,0 +1,63 @@ +package frc.lib; + +public class LowPassFilter { + + private double filteredValue; + private boolean initialized; + private double alpha; // smoothing factor (0..1) + + /** + * @param alpha smoothing factor. + * smaller = smoother but more lag. + * recommended starting range: 0.1 - 0.2 + */ + public LowPassFilter(double alpha) { + setAlpha(alpha); + this.initialized = false; + } + + /** + * Update filter with new measurement. + * + * @param input new raw value + * @return filtered value + */ + public double calculate(double input) { + + if (!initialized) { + filteredValue = input; + initialized = true; + return filteredValue; + } + + filteredValue += alpha * (input - filteredValue); + + return filteredValue; + } + + /** + * Reset filter to a known value. + */ + public void reset(double value) { + filteredValue = value; + initialized = true; + } + + /** + * Change smoothing strength dynamically. + */ + public void setAlpha(double alpha) { + if (alpha <= 0 || alpha > 1) { + throw new IllegalArgumentException("Alpha must be in (0, 1]"); + } + this.alpha = alpha; + } + + public boolean isInitialized() { + return initialized; + } + + public double get() { + return filteredValue; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java new file mode 100644 index 0000000..ed8ae3b --- /dev/null +++ b/src/main/java/frc/robot/BuildConstants.java @@ -0,0 +1,17 @@ +package frc.robot; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "Rebuilt"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 732; + public static final String GIT_SHA = "0ca4d8de9c3cec5e033dfc971b4b5edf0dc9b177"; + public static final String GIT_DATE = "2024-04-04 08:16:11 EDT"; + public static final String GIT_BRANCH = "develop"; + public static final String BUILD_DATE = "2025-01-05 14:16:50 EST"; + public static final long BUILD_UNIX_TIME = 1736104610571L; + public static final int DIRTY = 1; + + private BuildConstants() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/CanID.java b/src/main/java/frc/robot/CanID.java index 60d3d2f..7896199 100644 --- a/src/main/java/frc/robot/CanID.java +++ b/src/main/java/frc/robot/CanID.java @@ -4,9 +4,18 @@ * Holder for all CAN device IDs besides drivetrain devices */ public enum CanID { + INDEXER_MOTOR(10), + + FLYWHEEL_MOTOR(20), + HOOD_MOTOR(12), + HOOD_ENCODER(14), + + TURRET_MOTOR(13), + INTAKE_MOTOR(9), INTAKE_DEPLOY_MOTOR(11), - INDEXER(10); + + ; private final int deviceID; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..59770c8 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,37 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final Mode currentMode = Mode.REAL; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a079969..a916faa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,75 +1,232 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. package frc.robot; -import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -public class Robot extends TimedRobot { - private Command m_autonomousCommand; +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.util.Arrays; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends LoggedRobot { + private static final double LOOP_PERIOD_S = 0.02; // 20 ms loop period + private static double tickStart = 0; // start time of current tick, updated for each tick, in seconds + + private Command autonomousCommand; + private RobotContainer robotContainer; + + private static final boolean IS_PRACTICE = true; + private static final String LOG_DIRECTORY = "/home/lvuser/logs"; + private static final long MIN_FREE_SPACE = + IS_PRACTICE + ? 100000000 + : // 100 MB + 1000000000; // 1 GB + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Setup log directory and free up space if needed + SetupLog(); + //Pathfinding.setPathfinder(new LocalADStarAK()); + + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (Constants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter(LOG_DIRECTORY)); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() - private final RobotContainer m_robotContainer; + // Start AdvantageKit logger + Logger.start(); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = new RobotContainer(); + } + + void SetupLog() { + // Check if the log directory exists + var directory = new File(LOG_DIRECTORY); + if (!directory.exists()) { + directory.mkdir(); + } - public Robot() { - m_robotContainer = new RobotContainer(); + // ensure that there is enough space on the roboRIO to log data + if (directory.getFreeSpace() < MIN_FREE_SPACE) { + System.out.println("ERROR: out of space!"); + var files = directory.listFiles(); + if (files == null) { + System.out.println("ERROR: Cannot delete, Files are NULL!"); + } else { + // Sorting the files by name will ensure that the oldest files are deleted first + files = Arrays.stream(files).sorted().toArray(File[]::new); + + long bytesToDelete = MIN_FREE_SPACE - directory.getFreeSpace(); + + for (File file : files) { + if (file.getName().endsWith(".wpilog")) { + try { + bytesToDelete -= Files.size(file.toPath()); + } catch (IOException e) { + System.out.println("Failed to get size of file " + file.getName()); + continue; + } + if (file.delete()) { + System.out.println("Deleted " + file.getName() + " to free up space"); + } else { + System.out.println("Failed to delete " + file.getName()); + } + if (bytesToDelete <= 0) { + break; + } + } + } + } + } } + /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); + tickStart = Timer.getFPGATimestamp(); + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. + CommandScheduler.getInstance().run(); } + /** This function is called once when the robot is disabled. */ @Override public void disabledInit() {} + /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() {} - @Override - public void disabledExit() {} - + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + autonomousCommand = robotContainer.getAutonomousCommand(); - if (m_autonomousCommand != null) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); + // schedule the autonomous command (example) + if (autonomousCommand != null) { + CommandScheduler.getInstance().schedule(autonomousCommand); } } + /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() {} - @Override - public void autonomousExit() {} - + /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } + /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() {} - @Override - public void teleopExit() {} - + /** This function is called once when test mode is enabled. */ @Override public void testInit() { + // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } + /** This function is called periodically during test mode. */ @Override public void testPeriodic() {} + /** This function is called once when the robot is first started up. */ @Override - public void testExit() {} + public void simulationInit() {} + /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} -} + + /** + * Utility function to check if a given timestamp is within the current tick. + * @param timestamp timestamp to check, in seconds + * @return true if the timestamp is within the current tick, false otherwise + */ + public static boolean isTimestampInCurrentTick(double timestamp) { + return timestamp >= tickStart && timestamp < tickStart + LOOP_PERIOD_S; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8ff5129..dc13d42 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,24 +7,28 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.pathplanner.lib.auto.AutoBuilder; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.targeting.Targeting; +import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.vision.Vision; +import java.util.HashSet; +import java.util.Set; + public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity @@ -32,7 +36,7 @@ public class RobotContainer { private final SendableChooser autoChooser; /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + private final SwerveRequest.FieldCentric driveCmd = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); @@ -43,30 +47,40 @@ public class RobotContainer { private final CommandXboxController driverController = new CommandXboxController(0); private final CommandXboxController operatorController = new CommandXboxController(1); - - public final CommandSwerveDrivetrain drivetrain; + public static CommandSwerveDrivetrain drive; private final Vision vision; + private final Turret turret; private final Intake intake; private final Indexer indexer; + private final Shooter shooter; + private final Targeting targeting; public RobotContainer() { - drivetrain = TunerConstants.createDrivetrain(); - autoChooser = drivetrain.getAutoChooser(); + drive = TunerConstants.createDrivetrain(); + this.turret = new Turret(); + this.vision = new Vision(drive); + this.intake = new Intake(drive); + this.shooter = new Shooter(); + this.targeting = new Targeting(drive); + this.indexer = new Indexer(2, shooter, turret); + + autoChooser = drive.getAutoChooser(); SmartDashboard.putData("Auto Path", autoChooser); - - this.vision = new Vision(drivetrain); - this.intake = new Intake(drivetrain); - this.indexer = new Indexer(2); + configureBindings(); } + public static CommandSwerveDrivetrain getDrive() { + return drive; + } + private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( + drive.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> - drive.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + drive.applyRequest(() -> + driveCmd.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left) .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) ) @@ -76,29 +90,32 @@ private void configureBindings() { // neutral mode is applied to the drive motors while disabled. final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue( - drivetrain.applyRequest(() -> idle).ignoringDisable(true) + drive.applyRequest(() -> idle).ignoringDisable(true) ); - driverController.a().whileTrue(drivetrain.applyRequest(() -> brake)); - driverController.b().whileTrue(drivetrain.applyRequest(() -> + driverController.a().whileTrue(drive.applyRequest(() -> brake)); + driverController.b().whileTrue(drive.applyRequest(() -> point.withModuleDirection(new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX())) )); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - driverController.back().and(driverController.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - driverController.back().and(driverController.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - driverController.start().and(driverController.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - driverController.start().and(driverController.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + driverController.back().and(driverController.y()).whileTrue(drive.sysIdDynamic(Direction.kForward)); + driverController.back().and(driverController.x()).whileTrue(drive.sysIdDynamic(Direction.kReverse)); + driverController.start().and(driverController.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); + driverController.start().and(driverController.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press - driverController.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + driverController.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); operatorController.a().onTrue(intake.run(() -> intake.deploy())); operatorController.b().onTrue(intake.run(() -> intake.retract())); + + operatorController.x().onTrue(targeting.run(() -> targeting.setTargetingHub())); + operatorController.y().onTrue(targeting.run(() -> targeting.setTargetingShuttle())); } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 45af73b..41ad6f8 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -55,6 +55,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + private double robotOmegaDegPerSec = 0.0; + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -250,6 +252,9 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + + this.robotOmegaDegPerSec = this.getState().Speeds.omegaRadiansPerSecond + * 180.0 / Math.PI; } private void startSimThread() { @@ -336,7 +341,6 @@ private void configureAutoBuilder() { } catch (Exception e) { DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", e.getStackTrace()); } - } public Pose2d getPose() { @@ -350,4 +354,8 @@ public Rotation2d getRotation() { public SendableChooser getAutoChooser() { return AutoBuilder.buildAutoChooser(); } + + public double getRobotOmegaDegPerSec() { + return robotOmegaDegPerSec; + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index dbc46fb..9a931ad 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,31 +1,68 @@ package frc.robot.subsystems.indexer; import frc.lib.subsystem.SpikeSystem; -import frc.robot.subsystems.indexer.IndexerIO; -import frc.robot.subsystems.indexer.IndexerIOTalonFX; import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.turret.Turret; public class Indexer extends SpikeSystem { private final static double indexerSpeed = 10.0; // Rotations per second + private final Shooter shooter; + private final Turret turret; + private IndexerIO indexerIO; private DigitalInput proximitySensor; - public Indexer(int channel) { + public Indexer(int channel, Shooter shooter, Turret turret) { super("Indexer", new IndexerIO.IndexerIOInputs()); proximitySensor = new DigitalInput(channel); + + this.shooter = shooter; + this.turret = turret; } // Activate motor if proximity sensor detects a ball in the indexer @Override public void onPeriodic() { - if (proximitySensor.get()) { - indexerIO.setSpeed(0.0); - } else { + if (mechanismReadyForBalls()) { + // run the indexer if the mechanisms are ready for balls + // run it regardless of ball in indexer, so that it can feed a ball in if there is one queued up indexerIO.setSpeed(indexerSpeed); + } else if (needsFeeding()) { + // bring the ball to the indexer and stop once we see a ball + indexerIO.setSpeed(indexerSpeed); + } else { + // stop the indexer if the mechanisms aren't ready and we have a ball queued + indexerIO.setSpeed(0.0); } } + /** + * Checks the proximity sensor to see if there is a ball currently queued up in the indexer. + * @return true if there is a ball in the indexer, false otherwise + */ + private boolean hasBallQueued() { + return proximitySensor.get(); + } + + /** + * Determines if the shooter and turret are ready to receive a ball. + * @return true if both the shooter is at target RPS and the turret is at target angle, false otherwise + */ + private boolean mechanismReadyForBalls() { + return shooter.isAtTargetRPS() && turret.isAtTargetAngle(); + } + + /** + * Determines if the indexer needs a ball fed into it. + * True if the mechanisms need a ball and there isn't one already queued up, false otherwise. + * @return true if the indexer needs a ball, false otherwise + */ + public boolean needsFeeding() { + return !hasBallQueued(); + } + @Override protected Runnable setupDataRefresher() { this.indexerIO = new IndexerIOTalonFX(); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java index fd3154e..b50ba7d 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java @@ -10,7 +10,7 @@ public class IndexerIOTalonFX implements IORefresher, IndexerIO { private final BaseStatusSignal motorRps; // Rotations per second public IndexerIOTalonFX() { - this.motor = new TalonFX(CanID.INDEXER.getID()); + this.motor = new TalonFX(CanID.INDEXER_MOTOR.getID()); this.motorRps = motor.getRotorVelocity(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..5e71404 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.shooter; + +import frc.lib.subsystem.SpikeSystem; +import frc.robot.subsystems.targeting.Targeting; +import frc.robot.subsystems.turret.calc.ShotCompensation; + +public class Shooter extends SpikeSystem { + private static final double SHOOTER_READY_THRESHOLD_RPS = 0.5; // RPS threshold to consider the shooter ready + + private ShooterIO shooterIO; + private double targetRPS = 0.0; // Target rotations per second + + public Shooter() { + super("Shooter", new ShooterIO.ShooterIOInputs()); + } + + /** + * Set rps and hood angle from adjusted shot data + */ + @Override + public void onPeriodic() { + ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); + + if (shotData != null) { + double newTargetRPS = shotData.rpm() / 60.0; + this.targetRPS = newTargetRPS; + + shooterIO.setHoodAngle(shotData.hoodAngleDeg()); + shooterIO.setFlywheelVelocity(newTargetRPS); + } + } + + /** + * Sets up the data refresher for Shooter + */ + @Override + protected Runnable setupDataRefresher() { + shooterIO = new ShooterIOTalonFX(); + return useAsyncDataRefresher(shooterIO); + } + + /** + * Checks if current rps of the motor is within the allowed error bounds + * @return if the target is within error bounds + */ + public boolean isAtTargetRPS() { + return Math.abs(super.io.motorRPS - targetRPS) < SHOOTER_READY_THRESHOLD_RPS; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 0000000..7adbc44 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.shooter; + +import frc.lib.subsystem.BaseIO; +import frc.lib.subsystem.BaseInputClass; +import frc.lib.subsystem.IORefresher; +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO extends BaseIO, IORefresher { + @AutoLog + public static class ShooterIOInputs extends BaseInputClass { + public double motorRPS = 0.0; // Motor rotations per second + public double hoodAngle = 15; // Hood angle in degrees from + public double hoodMotorPosition = 0.0; // TODO: position is in relation to what? + public double flywheelSetPointRPS = 0.0; // Flywheel set point rotations per second + public double hoodSetPointAngle = 0.0; // TODO: Hood setpoint in relation to what? + } + + void setFlywheelVelocity(double rps); + void setHoodAngle(double angle); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java new file mode 100644 index 0000000..1ff338c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -0,0 +1,147 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.CanID; + +public class ShooterIOTalonFX implements ShooterIO { + private static final double HOOD_GEAR_RATIO = 1.0/1.0; // hood pulley teeth / motor pulley teeth (motor revs per hood rev) + + private final TalonFX flywheelMotor; + private final TalonFXS hoodMotor; + private final CANcoder hoodEncoder; // using wcp throughbore; you interface through 'CANcoder' class + + private final double hoodMotorPositionOffset; // offset in motor rotations, calculated at startup, units in motor rotations + + private final VelocityVoltage flywheelVelocityControl = new VelocityVoltage(0); + private final PositionVoltage hoodPositionControl = new PositionVoltage(0); + + private final StatusSignal motorVelocity; // rps + private final StatusSignal hoodMotorPosition; // motor rotations + private final StatusSignal hoodAngle; // degrees + + private double hoodAngleSetPoint = 0.0; + private double flywheelRPSSetPoint = 0.0; + + public ShooterIOTalonFX() { + // hood configs + this.hoodMotor = new TalonFXS(CanID.HOOD_MOTOR.getID()); + + var hoodSlot0 = new Slot0Configs(); + hoodSlot0.kP = 0.1; + hoodSlot0.kI = 0.0; + hoodSlot0.kD = 0.0; + hoodSlot0.kS = 0.0; + hoodSlot0.kV = 0.0; + + this.hoodMotor.getConfigurator().apply(hoodSlot0); + + // flywheel configs + this.flywheelMotor = new TalonFX(CanID.FLYWHEEL_MOTOR.getID()); + + var flywheelSlot0 = new Slot0Configs(); + flywheelSlot0.kP = 0.1; + flywheelSlot0.kI = 0.0; + flywheelSlot0.kD = 0.0; + flywheelSlot0.kS = 0.0; + flywheelSlot0.kV = 0.0; + + this.flywheelMotor.getConfigurator().apply(flywheelSlot0); + + this.hoodEncoder = new CANcoder(CanID.HOOD_ENCODER.getID()); + + CANcoderConfiguration hoodEncoderConfig = new CANcoderConfiguration(); + // constrains the range to [0, 1) + hoodEncoderConfig.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(1.0); + + this.hoodEncoder.getConfigurator().apply(hoodEncoderConfig); + + this.motorVelocity = flywheelMotor.getVelocity(); + this.hoodAngle = hoodEncoder.getAbsolutePosition(); + this.hoodMotorPosition = hoodMotor.getPosition(); + + // force refresh before zero calculations + BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition); + + // convert hood angle to motor rotations and calculate offset + this.hoodMotorPositionOffset = this.hoodMotorPosition.getValueAsDouble() - angleToMotorRotations(this.hoodAngle.getValueAsDouble()); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + motorVelocity, + hoodAngle, + hoodMotorPosition + ); + + flywheelMotor.optimizeBusUtilization(); + hoodMotor.optimizeBusUtilization(); + } + + + /** + * Periodically refreshes encoder signal + * @note This is called automatically + */ + @Override + public void refreshData() { + BaseStatusSignal.refreshAll(motorVelocity, hoodAngle, hoodMotorPosition); + } + + /** + * Periodically called to update the shooter information for logging + * @param inputs ShooterIOInputs object to update + */ + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.motorRPS = this.motorVelocity.getValueAsDouble(); + inputs.hoodAngle = this.hoodAngle.getValueAsDouble() * 360; // convert rotations to degrees + inputs.hoodMotorPosition = this.hoodMotorPosition.getValueAsDouble(); + + inputs.flywheelSetPointRPS = this.flywheelRPSSetPoint; + inputs.hoodSetPointAngle = this.hoodAngleSetPoint; + } + + /** + * Set the target flywheel velocity + * @param rps - target rotations per second + */ + @Override + public void setFlywheelVelocity(double rps) { + this.flywheelRPSSetPoint = rps; + this.flywheelVelocityControl.withVelocity(rps); + + flywheelMotor.setControl(this.flywheelVelocityControl); + } + + /** + * Set the target hood angle + * @param angle target angle TODO: relative to what + */ + @Override + public void setHoodAngle(double angle) { + this.hoodAngleSetPoint = angle; + double motorRotations = angleToMotorRotations(angle); + this.hoodPositionControl.withPosition(hoodMotorPositionOffset + motorRotations); + + this.hoodMotor.setControl(this.hoodPositionControl); + } + + /** + * Converts angle in degrees to motor rotations per second + * @param angle Input angle in degrees + * @return double motor rotations per second + */ + private static double angleToMotorRotations(double angle) { + // rotations = (angle_deg * gear_ratio) / 360 + return angle * HOOD_GEAR_RATIO / 360.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java new file mode 100644 index 0000000..86596dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -0,0 +1,73 @@ +package frc.robot.subsystems.targeting; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.Elastic; +import frc.lib.Elastic.Notification; +import frc.lib.Elastic.NotificationLevel; +import frc.lib.FieldConstants; +import frc.robot.subsystems.drive.CommandSwerveDrivetrain; +import frc.robot.subsystems.turret.calc.ShotCompensation; + +public class Targeting extends SubsystemBase { + private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23) + private static ShotCompensation.AdjustedShot shotData = new ShotCompensation.AdjustedShot(0.0, 0.0, 0.0, 0.0, 0.0); + + private Translation2d targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + private final CommandSwerveDrivetrain drive; + + public Targeting(CommandSwerveDrivetrain drive) { + this.drive = drive; + setTargetingHub(); + Logger.recordOutput("HubTarget", FieldConstants.Hub.innerCenterPoint); + Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); + } + + /** + * Periodically calculate the shot data given the target position and robot movement + */ + @Override + public void periodic() { + // calculate the adjusted shot parameters based on the current robot movement and the turret's target position + shotData = ShotCompensation.compensateForMovement( + drive.getPose(), + drive.getState().Speeds, + new Pose2d(targetPos, new Rotation2d()), + NOMINAL_SHOT_TIME_S + ); + } + + /** + * Set the target location to center of the hub + */ + public void setTargetingHub() { + Elastic.sendNotification( + new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SCORING mode") + ); + Elastic.selectTab("Scoring Mode"); + targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + } + + /** + * Set the target location to 0, 0 + */ + public void setTargetingShuttle() { + Elastic.sendNotification( + new Notification(NotificationLevel.INFO, "Switched Modes", "Switched modes to SHUTTLING mode") + ); + Elastic.selectTab("Shuttling Mode"); + targetPos = new Translation2d(0, 0); + } + + /** + * Returns the current shot data + * @return ShotCompensation.AdjustedShot shot data to use + */ + public static ShotCompensation.AdjustedShot getShotData() { + return shotData; + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java new file mode 100644 index 0000000..3e79448 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.turret; + +import frc.lib.subsystem.SpikeSystem; +import frc.robot.RobotContainer; + +import frc.robot.subsystems.targeting.Targeting; +import frc.robot.subsystems.turret.calc.ShotCompensation; + +public class Turret extends SpikeSystem { + private static final double TURRET_ANGLE_THRESHOLD_DEG = 3.0; // degrees within target angle to be considered "at target" + + private TurretIO turretIO; + private double targetAngleDeg = 0.0; + + public Turret() { + super("Turret", new TurretIO.TurretIOInputs()); + } + + /** + * Continuously sets the turret angle to point at the target position + */ + @Override + public void onPeriodic() { + // compensate for robot movement + ShotCompensation.AdjustedShot shotData = Targeting.getShotData(); + + if (shotData != null) { + double newTargetAngleDeg = shotData.turretAngleDeg(); + this.targetAngleDeg = newTargetAngleDeg; + + this.turretIO.setTurretAngle(newTargetAngleDeg); + } + } + + /** + * Sets up the data refresher for + */ + @Override + protected Runnable setupDataRefresher() { + this.turretIO = new TurretIOTalonFX(RobotContainer.getDrive()); + return useAsyncDataRefresher(this.turretIO); + } + + /** + * Determines if turret is at target angle within error bounds + * @return if turret is at angle within bounds + */ + public boolean isAtTargetAngle() { + double angleError = Math.abs(super.io.turretAngleDegrees - targetAngleDeg); + return angleError < TURRET_ANGLE_THRESHOLD_DEG; + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java new file mode 100644 index 0000000..3074ba8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.lib.subsystem.BaseIO; +import frc.lib.subsystem.BaseInputClass; +import frc.lib.subsystem.IORefresher; +import org.littletonrobotics.junction.AutoLog; + +public interface TurretIO extends BaseIO, IORefresher { + + @AutoLog + public static class TurretIOInputs extends BaseInputClass { + public double turretAngleDegrees = 0.0; + public double pinionEncoder = 0.0; + public double followerEncoder = 0.0; + public double turretSetPointDegrees = 0.0; + public Pose2d turretPosition = new Pose2d(); + public double robotOmegaDegPerSec = 0.0; + } + + void setTurretAngle(double angle); +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java new file mode 100644 index 0000000..1048a8a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -0,0 +1,152 @@ +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc.robot.CanID; +import frc.robot.subsystems.drive.CommandSwerveDrivetrain; +import frc.robot.subsystems.turret.calc.TurretMath; + +public class TurretIOTalonFX implements TurretIO { + private final TalonFX turretMotor; + private final DutyCycleEncoder pinionEncoder; + private final DutyCycleEncoder followerEncoder; + private final CommandSwerveDrivetrain drive; + + private final MotionMagicVoltage mmVoltage = new MotionMagicVoltage(0); + private final StatusSignal encoderSignal; + + private static final double kTurretGearRatio = 140/10; // turret ring: 140 teeth, motor pinion: 10 teeth + + private final double turretRotationOffset; // offset in motor rotations, calculated at startup, units in motor rotations + private final double turretDegreesOffset = 246.7; // offset in degrees to align turret with front of robot + + private double targetAngleDeg = 0.0; // target angle of the turret in degrees + + public TurretIOTalonFX(CommandSwerveDrivetrain drive) { + this.drive = drive; + this.turretMotor = new TalonFX(CanID.TURRET_MOTOR.getID()); + + MotionMagicConfigs mm = new MotionMagicConfigs(); + mm.MotionMagicAcceleration = 60; // rot/sec^2 + mm.MotionMagicCruiseVelocity = 30; // rot/sec + + // Motor configuration + Slot0Configs config = new Slot0Configs(); + config.kP = 1; + config.kI = 0.01; + config.kD = 0.3; + config.kS = 0.194; + config.kV = 0.1167; + this.turretMotor.getConfigurator().apply(config); + this.turretMotor.getConfigurator().apply(mm); + + // Setup encoders + this.pinionEncoder = new DutyCycleEncoder(0); + this.followerEncoder = new DutyCycleEncoder(1); + this.encoderSignal = this.turretMotor.getPosition(); + + double pinionEncoderValue = this.pinionEncoder.get(); + double followerEncoderValue = this.followerEncoder.get(); + + // set offset of the turret on startup + double turretRotations = TurretMath.getTurretAngleRevs(pinionEncoderValue, followerEncoderValue); + + double turretDegrees = TurretMath.normalizeTurretHeading( + TurretMath.toDegreesWrapped(turretRotations), + this.turretDegreesOffset + ); + + double currentMotorRevs = this.turretMotor.getPosition().getValueAsDouble(); + double toZeroRevs = TurretMath.degreesToMotorPosition(turretDegrees); + + this.turretRotationOffset = currentMotorRevs + toZeroRevs; + } + + /** + * Set the turret angle to a target field angle + * @param fieldTargetHeadingDeg field relative angle to set turret to + */ + @Override + public void setTurretAngle(double fieldTargetHeadingDeg) { + this.targetAngleDeg = fieldTargetHeadingDeg; + fieldTargetHeadingDeg = MathUtil.inputModulus(fieldTargetHeadingDeg, 0.0, 360.0); + + // convert robot heading to [0, 360) range + double robotHeadingDeg = + MathUtil.inputModulus( + drive.getPose().getRotation().getDegrees(), + 0.0, 360.0 + ); + + // dt = time we expect turret to reach commanded angle, tunable + double dt = 0.025; + double predictedHeadingDeg = + robotHeadingDeg + this.drive.getRobotOmegaDegPerSec() * dt; + + // turret angle in (-180, 180] range, where positive is counterclockwise relative to the robot's forward direction, and negative is clockwise + double turretAngleDeg = + MathUtil.inputModulus( + fieldTargetHeadingDeg - predictedHeadingDeg, + -180.0, 180.0 + ); + + // convert turret angle to motor rotations, accounting for gear ratio and offset + double rotations = (turretAngleDeg / 360.0) * kTurretGearRatio; + rotations = -rotations + this.turretRotationOffset; + + // calculate feedforward to counteract robot rotation, using a simple linear model with gain determined empirically + double motorRps = + -(this.drive.getRobotOmegaDegPerSec() / 360.0) * kTurretGearRatio; + + // set the motor to the desired position with feedforward to counteract robot rotation + this.turretMotor.setControl( + this.mmVoltage + .withPosition(rotations) + // 0.1167 is an empirically determined gain to convert from motor RPS to voltage needed to hold position against rotation + .withFeedForward(motorRps * 0.1167) + ); + } + + + /** + * Periodically called to update the Turret information for logging + * @param inputs TurretIOInputs object to update + */ + @Override + public void updateInputs(TurretIOInputs inputs) { + inputs.pinionEncoder = this.pinionEncoder.get(); + inputs.followerEncoder = this.followerEncoder.get(); + inputs.turretSetPointDegrees = this.targetAngleDeg; + + double turretRotations = TurretMath.getTurretAngleRevs(inputs.pinionEncoder, inputs.followerEncoder); + + inputs.robotOmegaDegPerSec = this.drive.getRobotOmegaDegPerSec(); + + // convert raw encoder readings to turret angle in degrees, accounting for gear ratio and offset + inputs.turretAngleDegrees = TurretMath.normalizeTurretHeading( + TurretMath.toDegreesWrapped(turretRotations), + turretDegreesOffset + ); + + // creates a position for the turret based on robot position, rotated by turret angle for logging + inputs.turretPosition = new Pose2d(drive.getPose().getTranslation(), new Rotation2d(TurretMath.toRad(inputs.turretAngleDegrees))); + } + + /** + * Periodically refreshes encoder signal + * @note This is called automatically + */ + @Override + public void refreshData() { + BaseStatusSignal.refreshAll(encoderSignal); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java b/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java new file mode 100644 index 0000000..f19f80e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/calc/ShotCompensation.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.turret.calc; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class ShotCompensation { + + public record AdjustedShot(double rpm, double hoodAngleDeg, double turretAngleDeg, + double turretFF_deg_s, double rangeFF_m_s) {} + + /** + * Calculate adjustments to shot parameters to compensate for robot movement during the shot, using a simple linear prediction of target motion based on current velocity. + * @param robotPose current robot pose on the field + * @param fieldRelVel current robot velocity in field-relative coordinates (x is forward, y is left, omega is CCW positive), used to predict where the target will be when the shot arrives + * @param targetPose current target pose on the field, used to calculate initial aiming angle and distance for compensation calculations + * @param nominalShotTimeS estimated time from shot release to target impact at the nominal shot speed, used to predict where the target will be when the shot arrives. This should be based on empirical measurements of the actual shot time for the given shot parameters, and is critical for accurate compensation. + * @return adjusted shot parameters including feedforward terms for turret angle and shot speed to compensate for robot movement, as well as the effective turret angle to aim at after compensation. The caller should apply the feedforward terms to the turret control and shot speed commands to achieve the desired compensation. + */ + public static AdjustedShot compensateForMovement( + Pose2d robotPose, + ChassisSpeeds fieldRelVel, + Pose2d targetPose, + double nominalShotTimeS + ) { + + // vector from robot to target in field coords + Translation2d toTarget = targetPose.getTranslation().minus(robotPose.getTranslation()); + // straight line distance to target (meters) + double rangeM = toTarget.getNorm(); + + // angle from robot to goal, in field/world space + Rotation2d robotToGoalRot = toTarget.getAngle(); + + // raw turret angle pointing directly at the goal (no movement compensation) + double uncompensatedTurretDeg = robotToGoalRot.getDegrees(); + + // convert chassis velocities into a 2d translation for easier math + Translation2d velTrans = new Translation2d(fieldRelVel.vxMetersPerSecond, fieldRelVel.vyMetersPerSecond); + // rotate the velocity vector into the frame that points toward the goal + // this makes the x component be the radial (toward/away) speed and y be tangential (sideways) + Translation2d velRelative = velTrans.rotateBy(robotToGoalRot.unaryMinus()); + + // radial: how fast robot is moving toward/away from the target (m/s) + double radialM_s = velRelative.getX(); + // tangential: sideways speed relative to the target (m/s) + double tangentialM_s = velRelative.getY(); + + // convert chassis angular velocity to degrees per second so it's easier to reason about + double angularDeg_s = Math.toDegrees(fieldRelVel.omegaRadiansPerSecond); + + double effectiveRangeM; + // start with the angle you'd aim if the robot were stationary + double effectiveTurretDeg = uncompensatedTurretDeg; + + // estimate horizontal shot speed needed to reach target in nominalShotTimeS, then remove radial robot motion + double shotHorizontalSpeed = rangeM / nominalShotTimeS - radialM_s; + // if robot is moving faster than the shot would need, clamp so we don't divide by negative speeds + if (shotHorizontalSpeed < 0) { + shotHorizontalSpeed = 0; + } + + // lead angle: how far to lead the shot based on sideways motion vs horizontal shot speed + // atan2(-tangential, shotSpeed) gives the angular correction to point into the moving target + double leadAdjustmentDeg = Math.toDegrees( + Math.atan2(-tangentialM_s, shotHorizontalSpeed) + ); + + // apply the lead so turret points where the target will be + effectiveTurretDeg += leadAdjustmentDeg; + + // recompute the effective distance the ball will travel taking the actual shot speed and tangential motion into account + effectiveRangeM = nominalShotTimeS * + Math.hypot(tangentialM_s, shotHorizontalSpeed); + + // lookup base rpm and hood angle from empirical shot table for the effective range + double baseRPM = ShotData.distanceToRPM.get(effectiveRangeM); + double hoodDeg = ShotData.distanceToHoodAngle.get(effectiveRangeM); + + // feedforward for turret angular motion: combine robot yaw and the apparent angular rate due to tangential motion + // note: tangential/range gives radians/sec approx, convert to deg/s + double turretFF_deg_s = -(angularDeg_s + Math.toDegrees(tangentialM_s / rangeM)); + // feedforward for range: negative radial means robot moving toward target, so subtract + double rangeFF_m_s = -radialM_s; + + return new AdjustedShot(baseRPM, hoodDeg, effectiveTurretDeg, + turretFF_deg_s, rangeFF_m_s); + } + + /** + * Calculate a feedforward term to compensate for the robot's velocity toward the target, which reduces effective range and thus shot time. + * @param velocity current robot velocity in field-relative coordinates + * @param directionToTarget unit vector pointing from robot to target in field-relative coordinates + * @return feedforward term to add to shot speed (in whatever units the caller expects, legacy scaling applies) + */ + private static double calculateVelocityCompensation( + ChassisSpeeds velocity, + Translation2d directionToTarget) { + + // project robot velocity onto the direction to target to get speed toward target + double robotSpeedTowardTarget = + velocity.vxMetersPerSecond * Math.cos(directionToTarget.getAngle().getRadians()) + + velocity.vyMetersPerSecond * Math.sin(directionToTarget.getAngle().getRadians()); + + // multiply by 100 to convert to whatever unit the caller expects (legacy scaling) + return robotSpeedTowardTarget * 100.0; + } + + private static double rpmToVelocity(double rpm) { + // convert wheel rpm to linear m/s + // wheel diameter 4 inches => 0.1016 m + return rpm * (Math.PI * 0.1016) / 60.0; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java b/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java new file mode 100644 index 0000000..7b71344 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/calc/ShotData.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.turret.calc; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +public class ShotData { + public static final InterpolatingDoubleTreeMap distanceToRPM = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap(); + + static { + // TODO: get data and fill these in + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java b/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java new file mode 100644 index 0000000..dbd66e8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/calc/TurretMath.java @@ -0,0 +1,189 @@ +package frc.robot.subsystems.turret.calc; + +/** + * Utility class for computing absolute turret position using two geared absolute encoders + * with a CRT-like search approach. + * + * Hardware setup (example): + * - Turret gear: 140 teeth + * - Encoder A: e.g. 13-tooth gear driving an absolute encoder (reads 0–1 rev) + * - Encoder B: e.g. 11-tooth gear driving an absolute encoder (reads 0–1 rev) + * + * The combination gives unique positions over (encoderA_teeth × encoderB_teeth) teeth. + * Beyond that range, continuity tracking (unwrapping) is used. + */ +public class TurretMath { + + // Hardware tooth counts + private static final double TURRET_GEAR_TEETH = 140.0; + private static final double ENCODER_A_TEETH = 13.0; + private static final double ENCODER_B_TEETH = 11.0; + + // Derived encoder combination values + private static final double ENCODER_COMBINED_TEETH = ENCODER_A_TEETH * ENCODER_B_TEETH; // 143 (example) + private static final double ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_TEETH / TURRET_GEAR_TEETH; // ≈1.0214 (example) + private static final double HALF_ENCODER_COMBINED_PERIOD_REV = ENCODER_COMBINED_PERIOD_REV / 2.0; + + private static final double NORMALIZED_REV = 1.0; // encoder reading range (0..1) + private static final double HALF_NORMALIZED_REV = NORMALIZED_REV / 2.0; + private static final double DEGREES_PER_REV = 360.0; + private static final double RAD_PER_REV = 2.0 * Math.PI; + private static final double MOTOR_UNITS_PER_REV = 14.0; // scale used in degreesToMotorPosition + + // Defaults / initial values + private static final double DEFAULT_OFFSET_DEGREES = 0.0; + private static final double DEFAULT_LAST_POSITION_REVS = 0.0; + + // State + private static double offsetDegrees = DEFAULT_OFFSET_DEGREES; // Calibration offset (degrees) + private static double lastPositionRevs = DEFAULT_LAST_POSITION_REVS; // Unwrapped continuous position + private static boolean initialized = false; + + /** + * Helper method to perform modulus that always returns a positive result, wrapping x into [0, m). + * @param x value to wrap + * @param m modulus (e.g. 1.0 for normalized encoder readings) + * @return wrapped value in [0, m) + */ + private static double mod(double x, double m) { + return ((x % m) + m) % m; + } + + /** + * Computes the raw absolute turret position in revolutions of the encoder-A gear + * (i.e. position in "encoder-A-gear-equivalent revolutions"). + * Range is approximately 0 to (combined_teeth / encoderA_teeth) revolutions of the encoder-A gear. + * + * This uses a search-based approach across the possible integer wraps of the encoder-A reading + * to find the branch that best matches the encoder-B reading. + * + * @param encoderAReading Absolute encoder A reading (normalized [0, 1)) + * @param encoderBReading Absolute encoder B reading (normalized [0, 1)) + * @return Raw turret position in encoder-A-gear revolutions + */ + public static double getRawPositionPrimaryRevs(double encoderAReading, double encoderBReading) { + double offsetRevs = offsetDegrees / DEGREES_PER_REV; + + // Apply offset and wrap to [0,1) + encoderAReading = mod(encoderAReading - offsetRevs, NORMALIZED_REV); + encoderBReading = mod(encoderBReading - offsetRevs, NORMALIZED_REV); + + double bestError = Double.POSITIVE_INFINITY; + double bestPosition = 0.0; + + // Search over the possible integer wraps of the secondary/primary relationship + int searchCount = (int) ENCODER_B_TEETH; // number of distinct branches to check (encoder B teeth) + for (int k = 0; k < searchCount; k++) { + double assumedPrimaryRevs = encoderAReading + k; + // Predict what the encoder-B reading should be if this is the correct branch + double predictedEncoderB = mod(assumedPrimaryRevs * (ENCODER_A_TEETH / ENCODER_B_TEETH), NORMALIZED_REV); + + double error = Math.abs(predictedEncoderB - encoderBReading); + // Handle wrap-around distance + if (error > HALF_NORMALIZED_REV) { + error = NORMALIZED_REV - error; + } + + if (error < bestError) { + bestError = error; + bestPosition = assumedPrimaryRevs; + } + } + + return bestPosition; + } + + /** + * Returns the turret angle in revolutions, unwrapped for continuous multi-turn motion. + * Applies offset and continuity tracking. + * + * @param encoderAReading Absolute encoder A reading [0,1) + * @param encoderBReading Absolute encoder B reading [0,1) + * @return Continuous turret position in revolutions (can be >1 or <0) + */ + public static double getTurretAngleRevs(double encoderAReading, double encoderBReading) { + double rawPrimaryRevs = getRawPositionPrimaryRevs(encoderAReading, encoderBReading); + double rawTurretRevs = rawPrimaryRevs * (ENCODER_A_TEETH / TURRET_GEAR_TEETH); + + // Apply offset again (in turret space) + rawTurretRevs -= offsetDegrees / DEGREES_PER_REV; + + // Wrap raw reading into one encoded period for comparison + double wrapped = mod(rawTurretRevs, ENCODER_COMBINED_PERIOD_REV); + + if (!initialized) { + lastPositionRevs = wrapped; + initialized = true; + return wrapped; + } + + // Compute shortest path delta (assuming small motion between calls) + double delta = wrapped - lastPositionRevs; + + // Unwrap using the known encoder combined period + if (delta > HALF_ENCODER_COMBINED_PERIOD_REV) { + delta -= ENCODER_COMBINED_PERIOD_REV; + } else if (delta < -HALF_ENCODER_COMBINED_PERIOD_REV) { + delta += ENCODER_COMBINED_PERIOD_REV; + } + + lastPositionRevs += delta; + return lastPositionRevs; + } + + /** + * Convert turret revolutions to degrees, wrapped to [0, 360). + * Use this when you only care about single-turn angle. + */ + public static double toDegreesWrapped(double turretRevs) { + return mod(turretRevs, NORMALIZED_REV) * DEGREES_PER_REV; + } + + /** + * Convert turret revolutions to radians, wrapped to [0, 2π). + */ + public static double toRadiansWrapped(double turretRevs) { + return mod(turretRevs, NORMALIZED_REV) * RAD_PER_REV; + } + + public static double degreesToMotorPosition(double turretDegrees) { + return (turretDegrees * MOTOR_UNITS_PER_REV) / DEGREES_PER_REV; + } + + public static double normalizeTurretHeading(double turretHeading, double zeroDegrees) { + double newHeading = turretHeading - zeroDegrees; + + if (newHeading < 0) { + newHeading = DEGREES_PER_REV - Math.abs(newHeading); + } + + return newHeading; + } + + /** + * Convert turret revolutions to total accumulated degrees (can be >360 or <0). + * Use this when you want continuous angle for PID or motion profiling. + */ + public static double toDegreesContinuous(double turretRevs) { + return turretRevs * DEGREES_PER_REV; + } + + // Calibration / zeroing + public static void setOffsetDegrees(double degrees) { + offsetDegrees = degrees; + } + + public static double getOffsetDegrees() { + return offsetDegrees; + } + + // Reset continuity tracker + public static void resetContinuity() { + initialized = false; + lastPositionRevs = DEFAULT_LAST_POSITION_REVS; + } + + public static double toRad(double angle) { + return angle * (Math.PI / DEGREES_PER_REV); + } +}