diff --git a/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto b/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto new file mode 100644 index 0000000..3eeb7bf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "deployIntake" + } + }, + { + "type": "named", + "data": { + "name": "enableIntake" + } + }, + { + "type": "named", + "data": { + "name": "startFlywheel" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Depot + Middle" + } + }, + { + "type": "named", + "data": { + "name": "requestShooting20S" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Depot Score.auto b/src/main/deploy/pathplanner/autos/Center Depot Score.auto new file mode 100644 index 0000000..73e1970 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Depot Score.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "startFlywheel" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Depot Path" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Depot + Middle.path b/src/main/deploy/pathplanner/paths/Center Depot + Middle.path new file mode 100644 index 0000000..d21b2e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot + Middle.path @@ -0,0 +1,200 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.660794487847223, + "y": 6.020440321180556 + }, + "prevControl": null, + "nextControl": { + "x": 2.9266887417218546, + "y": 5.8209437086092715 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.777748344370861, + "y": 5.762864238410596 + }, + "prevControl": { + "x": 0.7617654964670305, + "y": 6.012352812443321 + }, + "nextControl": { + "x": 0.7937311922746916, + "y": 5.51337566437787 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.961341059602649, + "y": 5.486986754966887 + }, + "prevControl": { + "x": 5.933165026697712, + "y": 5.8851734191363185 + }, + "nextControl": { + "x": 6.050950997385588, + "y": 4.2206096775903506 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.961341059602649, + "y": 2.633832781456953 + }, + "prevControl": { + "x": 6.143094347740103, + "y": 2.805488664697883 + }, + "nextControl": { + "x": 5.779587771465196, + "y": 2.4621768982160233 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2316059602649005, + "y": 2.633832781456953 + }, + "prevControl": { + "x": 4.216256192398722, + "y": 2.6231245087748944 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 160.0 + }, + { + "waypointRelativePos": 1.249394673123488, + "rotationDegrees": -90.9478722909092 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8141342756183786, + "maxWaypointRelativePos": 1.1274867374005306, + "constraints": { + "maxVelocity": 0.25, + "maxAcceleration": 0.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.133247679045094, + "maxWaypointRelativePos": 2.8504263913824066, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6001740716180366, + "maxWaypointRelativePos": 1.7901193633952235, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.1038869257950408, + "maxWaypointRelativePos": 3.776678445229686, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.125088339222631, + "maxWaypointRelativePos": 1.6621908127208602, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "requestShooting20S", + "waypointRelativePos": 1.2325088339222643, + "endWaypointRelativePos": 1.5943462897526506, + "command": { + "type": "named", + "data": { + "name": "requestShooting20S" + } + } + }, + { + "name": "startFlywheel", + "waypointRelativePos": 2.318021201413423, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "startFlywheel" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.72522429905929 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Depot Path.path b/src/main/deploy/pathplanner/paths/Center Depot Path.path new file mode 100644 index 0000000..a547bf5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot Path.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.660794487847223, + "y": 6.020440321180556 + }, + "prevControl": null, + "nextControl": { + "x": 2.702405813510649, + "y": 5.9506997240295485 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.4294309810169956, + "y": 5.919893391927083 + }, + "prevControl": { + "x": 1.477937911184211, + "y": 5.930899465460525 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.2, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 0.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Shooting", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": 2.0, + "command": { + "type": "named", + "data": { + "name": "requestShooting20S" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/lib/CountingDelay.java b/src/main/java/frc/lib/CountingDelay.java new file mode 100644 index 0000000..e4c31bd --- /dev/null +++ b/src/main/java/frc/lib/CountingDelay.java @@ -0,0 +1,23 @@ +package frc.lib; + +import edu.wpi.first.wpilibj.Timer; + +public class CountingDelay { + private boolean lock; + private double startTimeStamp; + + public CountingDelay() { + reset(); + } + + public boolean delay(double time){ + if (!lock) { + startTimeStamp = Timer.getFPGATimestamp(); + lock = true; + } + return Timer.getFPGATimestamp() - startTimeStamp >= time; + } + public void reset(){ + lock = false; + } +} diff --git a/src/main/java/frc/lib/Elastic.java b/src/main/java/frc/lib/Elastic.java deleted file mode 100644 index 8d0cc95..0000000 --- a/src/main/java/frc/lib/Elastic.java +++ /dev/null @@ -1,390 +0,0 @@ -// 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 index 37158be..55dc7f5 100644 --- a/src/main/java/frc/lib/FieldConstants.java +++ b/src/main/java/frc/lib/FieldConstants.java @@ -14,7 +14,6 @@ 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; /** @@ -305,6 +304,7 @@ public static class Outpost { new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); } + @SuppressWarnings("unused") public enum FieldType { ANDYMARK("andymark"), WELDED("welded"); @@ -316,6 +316,7 @@ public enum FieldType { } } + @SuppressWarnings("unused") public enum AprilTagLayoutType { OFFICIAL("2026-official"), NONE("2026-none"); diff --git a/src/main/java/frc/lib/led/LEDController.java b/src/main/java/frc/lib/led/LEDController.java index 514d585..952bcc7 100644 --- a/src/main/java/frc/lib/led/LEDController.java +++ b/src/main/java/frc/lib/led/LEDController.java @@ -25,4 +25,4 @@ public void switchPreset(String name) { public void registerPreset(LEDPreset preset) { presets.put(preset.name(), preset); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/lib/led/LEDPreset.java b/src/main/java/frc/lib/led/LEDPreset.java index 2b49229..9057c58 100644 --- a/src/main/java/frc/lib/led/LEDPreset.java +++ b/src/main/java/frc/lib/led/LEDPreset.java @@ -3,4 +3,4 @@ public record LEDPreset( String name, double value -) {} +) {} \ 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 a9c2711..4684aef 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,8 +13,6 @@ package frc.robot; -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -24,6 +22,7 @@ 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; @@ -31,6 +30,8 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import com.pathplanner.lib.commands.PathPlannerAuto; + /** * 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 @@ -82,7 +83,7 @@ public void robotInit() { 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 WPILOGWriter(LOG_DIRECTORY)); Logger.addDataReceiver(new NT4Publisher()); break; @@ -105,6 +106,7 @@ public void robotInit() { // Start AdvantageKit logger Logger.start(); + SmartDashboard.putBoolean("Auto/PathFlipped", false); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. @@ -184,9 +186,13 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); + boolean flipped = SmartDashboard.getBoolean("Auto/PathFlipped", false); - // schedule the autonomous command (example) - if (autonomousCommand != null) { + if (flipped) { + PathPlannerAuto auto = new PathPlannerAuto(autonomousCommand.getName()); + PathPlannerAuto flippedAuto = new PathPlannerAuto(auto.getName(), true); + CommandScheduler.getInstance().schedule(flippedAuto); + } else if (autonomousCommand != null) { CommandScheduler.getInstance().schedule(autonomousCommand); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c92ac2..2db0b8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,8 +7,8 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -36,6 +36,7 @@ import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.vision.Vision; +@SuppressWarnings("unused") 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 @@ -54,9 +55,6 @@ public class RobotContainer { .withHeadingPID(6, 0, 0.2); private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - // private final Telemetry logger = new Telemetry(MaxSpeed); private final CommandXboxController driverController = new SpikeController(0, 0.05); private final CommandXboxController operatorController = new SpikeController(1, 0.05); @@ -73,16 +71,15 @@ public class RobotContainer { private static LEDController ledController; public RobotContainer() { - ledController = new LEDController(1); drive = TunerConstants.createDrivetrain(); + ledController = new LEDController(1); this.turret = new Turret(); this.vision = new Vision(drive); - this.intake = new Intake(drive); + this.intake = new Intake(); this.shooter = new Shooter(); - this.targeting = new Targeting(drive); + this.targeting = new Targeting(); this.trigger = new Trigger(shooter, turret); this.findexer = new Findexer(trigger); - NamedCommands.registerCommand("enableIntake", new SetIntakeState(intake, true)); NamedCommands.registerCommand("disableIntake", new SetIntakeState(intake, false)); @@ -194,25 +191,6 @@ else if (driverController.x().getAsBoolean()) { // Left drive.applyRequest(() -> idle).ignoringDisable(true)); driverController.leftTrigger().whileTrue(drive.applyRequest(() -> brake)); - // driverController.rightTrigger().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(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 - - // operatorController.y().onTrue(vision.runOnce(() -> { - // var estimatedPose = vision.getEstimatedPositionFromCameras(); - // if (estimatedPose != null) { - // Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose); - // drive.resetPose(estimatedPose); - // } - // })); } private void setupIntakeBindings() { diff --git a/src/main/java/frc/robot/commands/DeployIntake.java b/src/main/java/frc/robot/commands/DeployIntake.java index d17df9f..1c4e723 100644 --- a/src/main/java/frc/robot/commands/DeployIntake.java +++ b/src/main/java/frc/robot/commands/DeployIntake.java @@ -20,14 +20,14 @@ public void initialize() { @Override public void execute() { - if (timer.hasElapsed(1.0)) { + if (timer.hasElapsed(0.75)) { intake.setDeployServo(0.0); } } @Override public boolean isFinished() { - if (timer.hasElapsed(2.0)) { + if (timer.hasElapsed(1.5)) { return true; } return false; diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java index 591bf50..aeb9b02 100644 --- a/src/main/java/frc/robot/commands/EmptyHopper.java +++ b/src/main/java/frc/robot/commands/EmptyHopper.java @@ -1,10 +1,7 @@ package frc.robot.commands; -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.targeting.Targeting; import frc.robot.subsystems.targeting.Targeting.Target; @@ -22,11 +19,6 @@ public EmptyHopper(Shooter shooter, Targeting targeting, double forTime, boolean this.scoringTime = forTime; - // if (targetHub) { - // this.targeting.setTargetingHub(); - // } else { - // this.targeting.setTargetingShuttleRight(); // TODO: Allow for left selection as well - // } this.targeting.setTarget(Target.HUB); addRequirements(shooter, targeting); @@ -56,7 +48,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - // TODO Auto-generated method stub this.shooter.setDriverSpinUpFlywheel(false); this.shooter.setActuateHoodAndLaunch(false); } diff --git a/src/main/java/frc/robot/commands/SetFlywheelState.java b/src/main/java/frc/robot/commands/SetFlywheelState.java index a1cef2b..b166b3e 100644 --- a/src/main/java/frc/robot/commands/SetFlywheelState.java +++ b/src/main/java/frc/robot/commands/SetFlywheelState.java @@ -1,7 +1,6 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; public class SetFlywheelState extends Command { diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index b4d8ecd..a814f3b 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -63,8 +63,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); private double robotOmegaDegPerSec = 0.0; private Field2d field = new Field2d(); @@ -85,49 +83,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ) ); - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second^2, but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - /* The SysId routine to test */ private final SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; @@ -279,8 +234,8 @@ public void periodic() { private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); - /* Run simulation at a faster rate so PID gains behave more reasonably */ - /* use the measured time delta, get battery voltage from WPILib */ + try (/* Run simulation at a faster rate so PID gains behave more reasonably */ + /* use the measured time delta, get battery voltage from WPILib */ Notifier m_simNotifier = new Notifier(() -> { final double currentTime = Utils.getCurrentTimeSeconds(); double deltaTime = currentTime - m_lastSimTime; @@ -288,8 +243,9 @@ private void startSimThread() { /* use the measured time delta, get battery voltage from WPILib */ updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); + })) { + m_simNotifier.startPeriodic(kSimLoopPeriod); + } } /** diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java index 7f4f485..7197e4d 100644 --- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java +++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.findexer; -import edu.wpi.first.wpilibj.Timer; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.trigger.Trigger; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index ca40a3a..f603365 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,33 +1,17 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj.DriverStation; import frc.lib.subsystem.SpikeSystem; -import frc.robot.subsystems.drive.CommandSwerveDrivetrain; public class Intake extends SpikeSystem { - private static final double DEPLOY_SPEED = 1.0; // Speed in Rotations Per Second to deploy the intake - private static final double RETRACT_SPEED = -1.0; // Speed in Rotations Per Second to retract the intake - private static final double DEPLOY_CURRENT_THRESHOLD = 10.0; // Amp limit for the deploy motor. Watches - // for a resistance to the motor to see when it's - // deployed - private static final double BASE_SPEED_INTAKE = 1; // Rotation Per Second - private static final double SPEED_PER_MPS = 0.05; // Speed added to the base speed per m/s of drive velocity - private static final double MAX_SPEED = 5.0; // speed cap, max speed of the intake in RPS - - public enum IntakeState { - DEPLOYED, RETRACTED, DEPLOYING, RETRACTING - } private IntakeIO intakeIO; - private final CommandSwerveDrivetrain drivetrain; private boolean running = false; // True if the intake is running, False otherwise private boolean forward = true; // Intake constructor - public Intake(CommandSwerveDrivetrain drivetrain) { + public Intake() { super("Intake", new IntakeIOInputsAutoLogged()); - this.drivetrain = drivetrain; } // Intake periodic function @@ -44,17 +28,6 @@ public void onPeriodic() { // Run the DEPLOYED state periodic actions private void doDeployedState() { - // // Get the absolute velocity of the entire robot - // double robotAbsoluteVelocity = Math.abs(drivetrain.getState().Speeds.vxMetersPerSecond); - - // // Adjust the intake speed, increase the intake as the robot moves faster - // // Cap at MAX_SPEED - // double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED); - - // if (forward == false) { - // speed *= -1; - // } - // Update the intakeIO on speed if (forward == false) { intakeIO.setIntakeSpeed(-70); @@ -79,10 +52,6 @@ public void switchDirection() { forward = !forward; } - public void toggle() { - running = !running; - } - // Disables the intake public void disable() { running = false; @@ -91,25 +60,6 @@ public void disable() { intakeIO.setIntakeSpeed(0.0); } - // Deploys the over the bumper intake - public void deploy() { - if (intakeIO.getIntakeState() != IntakeState.DEPLOYED) { // Only try to deploy if we aren't already deployed - intakeIO.setIntakeState(IntakeState.DEPLOYING); // Mark as deploying - } - } - - // Retracts the intake back to its stored position - public void retract() { - if (intakeIO.getIntakeState() != IntakeState.RETRACTED) { // Only try to retract if we aren't already retracted - intakeIO.setIntakeState(IntakeState.RETRACTING); // Mark as retracting - } - } - - // Returns true if the intake is fully deployed, false otherwise - public boolean isDeployed() { - return intakeIO.getIntakeState() == IntakeState.DEPLOYED; - } - @Override protected Runnable setupDataRefresher() { this.intakeIO = new IntakeIOTalonFX(); @@ -117,13 +67,8 @@ protected Runnable setupDataRefresher() { } // Toggles the intake between deployed and retracted states - public void toggleIntake() { + public void toggle() { this.running = !this.running; - // if (intakeIO.getIntakeState() == IntakeState.DEPLOYED || intakeIO.getIntakeState() == IntakeState.DEPLOYING) { - // retract(); - // } else { - // deploy(); - // } } public void setDeployServo(double position) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index fd82ad0..113ec5a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -3,7 +3,6 @@ import frc.lib.subsystem.BaseIO; import frc.lib.subsystem.BaseInputClass; import frc.lib.subsystem.IORefresher; -import frc.robot.subsystems.intake.Intake.IntakeState; import org.littletonrobotics.junction.AutoLog; @@ -14,11 +13,8 @@ class IntakeIOInputs extends BaseInputClass { public double intakeCurrentAmps = 0.0; // Intake Current in Amps public double deployVelocityRPS = 0.0; // Intake deploy motor Rotations Per Second public double deployCurrentAmps = 0.0; // Intake deploy motor Current in Amps - public IntakeState intakeState = IntakeState.RETRACTED; // current intake state } void setIntakeSpeed(double speed); - IntakeState getIntakeState(); - void setIntakeState(IntakeState newState); void setDeployServo(double position); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index c370dad..47114d0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.intake; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -8,16 +9,12 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; -import frc.lib.subsystem.IORefresher; import frc.robot.CanID; -import frc.robot.subsystems.intake.Intake.IntakeState; -public class IntakeIOTalonFX implements IntakeIO, IORefresher { +public class IntakeIOTalonFX implements IntakeIO { // TalonFX Motors private final TalonFX intakeMotor; private final Servo deployServo; @@ -28,12 +25,9 @@ public class IntakeIOTalonFX implements IntakeIO, IORefresher { private final StatusSignal intakeVelocity; private final StatusSignal intakeCurrent; - // Inputs for logging - private IntakeIOInputs intakeIO; - // IntakeIOTalonFX constructor public IntakeIOTalonFX() { - intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID(), "Canivore_Drivetrain"); // Setup the intake motor with the CAN ID + intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID(), new CANBus("Canivore_Drivetrain")); // Setup the intake motor with the CAN ID deployServo = new Servo(CanID.INTAKE_DEPLOY_SERVO.getID()); // Configure motors @@ -56,7 +50,6 @@ public void refreshData() { public void updateInputs(IntakeIOInputs inputs) { inputs.intakeVelocityRPS = intakeVelocity.getValueAsDouble(); inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble(); - intakeIO = inputs; } // Set the speed of the intake motor @@ -73,18 +66,6 @@ public void setIntakeSpeed(double speed) { intakeMotor.setControl(this.velocityControl); } - // Return the state of the Intake - @Override - public IntakeState getIntakeState() { - return intakeIO.intakeState; - } - - // Set the state of the Intake - @Override - public void setIntakeState(IntakeState newState) { - intakeIO.intakeState = newState; - } - public static TalonFXConfiguration getIntakeMotorConfig() { var intakeConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ad23e0e..a83931f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,15 +3,15 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.targeting.ShotData; import frc.robot.subsystems.targeting.Targeting; public class Shooter extends SpikeSystem { - private static final double SHOOTER_READY_THRESHOLD_RPS = 3.0; // RPS threshold to consider the shooter ready + private static final double SHOOTER_READY_THRESHOLD_RPS = 6.0; // RPS threshold to consider the shooter ready private boolean readFromData = true; @@ -32,11 +32,8 @@ public Shooter() { */ @Override public void onPeriodic() { - double distToTarget = getDistanceToTarget(); // distance in meters readFromData = SmartDashboard.getBoolean("ReadFromData", true); - // double targetRPM = ShotData.distanceToRPM.get(distToTarget); - // double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget); double targetRPM = 0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 0e58939..1966fe4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -22,7 +22,6 @@ public class ShooterIOTalonFX implements ShooterIO { private static final double HOOD_ZERO_CURRENT = 1.75; // amps at which we consider the hood to have hit a limit - private static final double FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS = 0.35; // ignore tiny target changes private final TalonFX flywheelMotor; private final TalonFXS hoodMotor; @@ -41,7 +40,6 @@ public class ShooterIOTalonFX implements ShooterIO { private double hoodAngleSetPoint = 0.0; private double flywheelRPSSetPoint = 0.0; - private double lastAppliedFlywheelRPSSetPoint = Double.NaN; private double hoodTargetEncoder = 0.0; private boolean isZeroing = true; @@ -190,9 +188,8 @@ public void setFlywheelVelocity(double rps) { this.flywheelControl.withVelocity(rps); flywheelMotor.setControl(this.flywheelControl); } - - this.lastAppliedFlywheelRPSSetPoint = rps; } + /** * Set the target hood angle. * @param angle target angle diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java index 8331331..db9796d 100644 --- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java +++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java @@ -37,6 +37,9 @@ public class ShotData { distanceToRPM.put(4.9, 2700.0); distanceToHoodAngle.put(4.9, 29.0); + distanceToRPM.put(5.88, 3000.0); + distanceToHoodAngle.put(5.88, 35.0); + distanceToRPM.put(17.069, 5000.0); distanceToHoodAngle.put(17.069, 45.0); } diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java index 9ad74d7..5fecede 100644 --- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java +++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java @@ -13,16 +13,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.FieldConstants; import frc.robot.RobotContainer; -import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.turret.Turret; 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 static Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - private final CommandSwerveDrivetrain drive; + private static final double ROTATION_TOF_MULTIPLIER = 0.5; private static final double FIELD_WIDTH = 8.07; // meters private static final double FIELD_LENGTH = 16.54; // meters @@ -46,8 +44,7 @@ public static enum Target { private boolean overrideRedAlliance = false; private boolean overrideBlueAlliance = false; - public Targeting(CommandSwerveDrivetrain drive) { - this.drive = drive; + public Targeting() { Logger.recordOutput("HubTarget", FieldConstants.Hub.oppTopCenterPoint); Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d())); @@ -99,7 +96,7 @@ public static Translation2d differenceBetweenRobotAndTarget() { .plus(predictedRobotPos); Translation2d toGoalComp = goalPose.minus(predictedTurretPivot); - + Logger.recordOutput("Targeting/StaticDistance", staticDistance); Logger.recordOutput("Targeting/PredictedRobotPos", new Pose2d(predictedRobotPos, predictedHeading)); Logger.recordOutput("Targeting/PredictedTurretPivot", new Pose2d(predictedTurretPivot, predictedHeading)); @@ -130,6 +127,12 @@ public void periodic() { * Set the target of the targeting subsystem. This will change the target position */ public void setTarget(Target target) { + if (target == Target.HUB) { + RobotContainer.getLEDController().switchPreset("hub"); + } else if (target == Target.SHUTTLE_LEFT || target == Target.SHUTTLE_RIGHT) { + RobotContainer.getLEDController().switchPreset("shuttle"); + } + currentTarget = target; } @@ -137,20 +140,10 @@ public void setTarget(Target target) { * Set the target location to center of the hub */ private void setPoseTargetingHub() { - RobotContainer.getLEDController().switchPreset("hub"); overrideBlueAlliance = SmartDashboard.getBoolean("OverrideBlueAlliance", overrideBlueAlliance); overrideRedAlliance = SmartDashboard.getBoolean("OverrideRedAlliance", overrideRedAlliance); - // if (!DriverStation.getAlliance().isPresent() && isRedAlliance) { - // if (isRedAlliance) { - // targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); - // } else { - // targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); - // } - // return; - // } - if (overrideRedAlliance) { targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d(); return; @@ -176,8 +169,6 @@ private void setPoseTargetingHub() { * Set the target location to 0, 0 */ private void setPoseTargetingShuttleRight() { - RobotContainer.getLEDController().switchPreset("shuttle"); - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset); } else { @@ -186,8 +177,6 @@ private void setPoseTargetingShuttleRight() { } private void setPoseTargetingShuttleLeft() { - RobotContainer.getLEDController().switchPreset("shuttle"); - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) { targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, 0 + shuttlingYOffset); } else { diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java index 7baeea7..afa28ab 100644 --- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java +++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.trigger; -import edu.wpi.first.wpilibj.DriverStation; import frc.lib.subsystem.SpikeSystem; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; @@ -33,24 +32,12 @@ public void onPeriodic() { // 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 triggerIO.setSpeed(TRIGGER_SPEED); - // } else if (needsFeeding()) { - // // bring the ball to the indexer and stop once we see a ball - // triggerIO.setSpeed(TRIGGER_SPEED); } else { // stop the indexer if the mechanisms aren't ready and we have a ball queued triggerIO.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 super.io.proximitySensor; - return false; - } - public void setReverseTrigger(boolean reverse) { this.reverseTrigger = reverse; } @@ -66,17 +53,9 @@ public boolean getReverseTrigger() { private boolean mechanismReadyForBalls() { if (shooter.isActuatingHoodAndLaunching()) { // check if turret is at target angle - boolean turretReady = turret.isAtTargetAngle(); - - - // if (DriverStation.isAutonomous() && turretReady) { - // turretReady = shooter.isAtTargetRPS(); - // } + boolean turretReady = turret.isAtTargetAngle() && shooter.isAtTargetRPS(); - if (!turretReady) { - return false; - } - return true; + return turretReady; } return false; } diff --git a/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java b/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java index 5ce7990..06c8470 100644 --- a/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java @@ -3,10 +3,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.DigitalInput; -import frc.lib.subsystem.IORefresher; import frc.robot.CanID; -public class TriggerIOTalonFX implements IORefresher, TriggerIO { +public class TriggerIOTalonFX implements TriggerIO { private final TalonFX motor; // Motor object private final BaseStatusSignal motorRps; // Rotations per second private final DigitalInput proximitySensor; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 8306c15..96b31d0 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -4,17 +4,15 @@ import frc.lib.subsystem.SpikeSystem; import frc.robot.RobotContainer; import frc.robot.subsystems.targeting.Targeting; -import frc.robot.subsystems.targeting.ShotCompensation; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; public class Turret extends SpikeSystem { - public static final double TURRET_AIMING_TOLERANCE_DEGREES = 5.0; // degrees within which we consider the turret to be aimed at the target (+-) + public static final double TURRET_AIMING_TOLERANCE_DEGREES = 40.0; // degrees within which we consider the turret to be aimed at the target (+-) // HARDWARE CONSTANTS // gearing @@ -31,7 +29,6 @@ public class Turret extends SpikeSystem { public static final double ENCODER_COMBINED_PERIOD_TURRET_REV = ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH); - public static final double TURRET_CENTER_OFFSET_DEG = -88.1; // subtracted from robot relative heading public static final double TURRET_ROBOT_OFFSET_DEG = 51.8; // subtracted from robot relative heading to get turret relative heading @@ -48,13 +45,6 @@ public Turret() { @Override public void onPeriodic() { Logger.recordOutput("Turret/TurretCenterOffset", new Pose2d(TURRET_OFFSET_FROM_CENTER, RobotContainer.getDrive().getRotation())); - // turretIO.setTurretAngleFieldRelativeDegrees(0); - - // if (shotData != null) { - // double newTargetAngleDeg = shotData.turretAngleDeg(); - - // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg); - // } if (this.overrideAutomaticAiming) { this.turretIO.setTurretAngleRobotRelativeDegrees(0); @@ -80,7 +70,6 @@ protected Runnable setupDataRefresher() { } public void toggleAimingOverride() { - RobotContainer.getLEDController().switchPreset("fixed"); this.overrideAutomaticAiming = !this.overrideAutomaticAiming; } @@ -91,8 +80,8 @@ public void toggleAimingOverride() { @AutoLogOutput(key="Turret/IsAtTargetAngle") public boolean isAtTargetAngle() { - return Math.abs(io.turretAngularVelocityDegreesPerSecond) < 200.0; - // return Math.abs(io.targetTurretDegrees - io.turretAngleDegreesTurretRelative) < TURRET_AIMING_TOLERANCE_DEGREES; + // return Math.abs(io.turretAngularVelocityDegreesPerSecond) < 200.0; + return Math.abs(io.targetTurretMotorRotations - io.turretMotorPositionRotations) < TURRET_AIMING_TOLERANCE_DEGREES / 180.0; } public void changeTrim(double deltaDegrees) { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index d9f267f..076a4d3 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -22,6 +22,7 @@ public static class TurretIOInputs extends BaseInputClass { public double rawTurretMechanismRotations = 0.0; // raw rotations of the entire turret mechanism public double turretTrimDegrees = 0.0; // minor adjustment to the turret angle based on operator controller input, in degrees public double turretAngularVelocityDegreesPerSecond = 0.0; // current angular velocity of the turret, in degrees per second + public double targetTurretDegreesTurretRelative = 0; } /** diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 522072a..4471ff5 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,14 +1,11 @@ package frc.robot.subsystems.turret; import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -16,17 +13,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.measure.Angle; -import frc.lib.LowPassFilter; import frc.robot.CanID; import frc.robot.subsystems.drive.CommandSwerveDrivetrain; public class TurretIOTalonFX implements TurretIO { - // KS KV CONSTANTS - private static final double kS = 0.35; // volts needed to overcome static friction - private static final double kV = 0.20; // volts per (rotation per second) to maintain motion - // SUBSYSTEMS private final CommandSwerveDrivetrain drive; @@ -48,7 +39,6 @@ public class TurretIOTalonFX implements TurretIO { private double lastTurretAngleDegrees = 0.0; // last calculated angle of the turret in degrees, used for calculating angular velocity // VALUES - private double targetTurretDegreesFieldRelative; // target angle of the turret in degrees, relative to the field private double processedTargetTurretDegreesFieldRelative; // processed target angle of the turret in degrees, relative to the field private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading @@ -56,7 +46,6 @@ public class TurretIOTalonFX implements TurretIO { // COMMANDS private final PositionTorqueCurrentFOC mmRequest = new PositionTorqueCurrentFOC(0.0); - private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(kS, kV); // ks, kv private double turretTrimDegrees = 0.0; @@ -103,7 +92,6 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) { @Override public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) { - this.targetTurretDegreesFieldRelative = fieldRelativeAngleDegrees; double currentRobotHeading = this.drive.getPose().getRotation().getDegrees(); // absolute robot-relative target, in motor rotations @@ -128,7 +116,7 @@ private void setTurretAngleTurretRelativeDegrees(double angleDegrees) { mmRequest.Position = targetMotorRotations; this.turretMotor.setControl( - mmRequest + mmRequest // .withFeedForward(drive.getState().Speeds.omegaRadiansPerSecond) ); } @@ -143,19 +131,6 @@ public void recalculateTurretMotorZeroPosition() { turretMotor.setPosition(this.calculatedMotorOffsetRevs); } - /** - * Calculates the feedforward voltage to apply to the turret motor to counteract the rotation of the robot, based on the current angular velocity of the robot. - * @return the feedforward value to apply to the turret rotation - */ - private double calculateFeedforward() { - // get the current angular velocity of the robot in radians per second - double gyroOmegaRadPerSecond = drive.getState().Speeds.omegaRadiansPerSecond; - - double mechanismRotationsPerSecond = gyroOmegaRadPerSecond / Math.PI; - return feedforward.calculate(-mechanismRotationsPerSecond); - } - - @Override public void refreshData() { StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal); @@ -206,13 +181,13 @@ private double getAngularVelocityDegreesPerSecond() { private Pair getTurretMotionConfigs() { Slot0Configs configs = new Slot0Configs(); - configs.kP = 200; - configs.kI = 180; - configs.kD = 15; + configs.kP = 300; + configs.kI = 10; + configs.kD = 25; - configs.kS = 10; //kS; - configs.kV = 0.8; //kV; - configs.kA = 20; + configs.kS = 0; //kS; + configs.kV = 0; //kV; + configs.kA = 0; MotionMagicConfigs mmConfigs = new MotionMagicConfigs(); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 403fcae..e362896 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -5,7 +5,6 @@ import frc.robot.subsystems.drive.CommandSwerveDrivetrain; import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; -import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import edu.wpi.first.math.geometry.Pose2d; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 2d6f9f2..8e4c347 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import frc.lib.subsystem.BaseIO; import frc.lib.subsystem.BaseInputClass; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java index d95cc97..62f61bf 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java @@ -2,18 +2,15 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import frc.lib.subsystem.IORefresher; -import frc.robot.subsystems.vision.photon.Camera; import frc.robot.subsystems.vision.photon.CameraManager; import org.photonvision.EstimatedRobotPose; import java.util.ArrayList; -import java.util.Collections; import java.util.List; import java.util.Objects; import java.util.function.Supplier; -public class VisionIOPhotonCamera implements VisionIO, IORefresher { +public class VisionIOPhotonCamera implements VisionIO { private final List estimatedRobotPoses; private final Supplier odometryPoseSupplier; diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java index 9c55703..bdc1993 100644 --- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java +++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java @@ -59,7 +59,7 @@ public static List getCameras() { new Camera( "left", new Transform3d( - Inches.of(-2.5), + Inches.of(-2.25), Inches.of(-13.75), Inches.of(8.5625), new Rotation3d(