diff --git a/CAD/2025/config.json b/CAD/2025/config.json new file mode 100644 index 0000000..354272b --- /dev/null +++ b/CAD/2025/config.json @@ -0,0 +1,20 @@ +{ + "name": "2025 RA25", + "rotations": [ + { + "axis": "x", + "degrees": 90.0 + }, + { + "axis": "y", + "degrees": 0.0 + }, + { + "axis": "z", + "degrees": 270.0 + } + ], + "position": [0.0, 0.0, 0.0], + "cameras": [], + "components": [] +} diff --git a/CAD/2025/model.glb b/CAD/2025/model.glb new file mode 100644 index 0000000..118a643 Binary files /dev/null and b/CAD/2025/model.glb differ diff --git a/advantagescope-layout.json b/advantagescope-layout.json new file mode 100644 index 0000000..5f23065 --- /dev/null +++ b/advantagescope-layout.json @@ -0,0 +1,170 @@ +{ + "hubs": [ + { + "x": 3007, + "y": 157, + "width": 1607, + "height": 808, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/SmartDashboard/Field/OdometryPose", + "/SmartDashboard/Field/Robot", + "/SmartDashboard/Field/XModules", + "/SmartDashboard/swerve/advantagescope", + "/SmartDashboard/MapleSim", + "/AdvantageKit/RealOutputs/Sim/RobotPose", + "/SmartDashboard/swerve" + ] + }, + "tabs": { + "selected": 3, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "loy0vs5fjh42ly5j6azpwuon54lsm5nf", + "renderer": "", + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Line Graph", + "controller": { + "leftSources": [], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "v8eqw2uqjo9n76steymeyw5g88i3biae", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 2, + "title": "2D Field", + "controller": { + "sources": [], + "field": "FRC:2026 Field", + "orientation": 0, + "size": "large" + }, + "controllerUUID": "dox8w70if5g8tlnoza04ddj3f0dbyo0y", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "3D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs/Sim/RobotPose", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "2026 KitBot" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Fuel", + "logType": "Pose3d[]", + "visible": true, + "options": { + "variant": "Fuel" + } + } + ], + "game": "FRC:2026 Field" + }, + "controllerUUID": "hqfgw1v7wxguag7mnwu6tr4abtw76zrj", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 3.1633543281769336, + 6.109330082975031, + -6.816309206052037 + ], + "cameraTarget": [ + 0, + 0.5, + 0 + ] + }, + "controlsHeight": 200 + }, + { + "type": 9, + "title": "Swerve", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "NT:/SmartDashboard/swerve/advantagescope/currentStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/SmartDashboard/swerve/advantagescope/desiredStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/SmartDashboard/swerve/advantagescope/desiredChassisSpeeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#0000ff" + } + }, + { + "type": "rotation", + "logKey": "NT:/SmartDashboard/swerve/advantagescope/robotRotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "dlzbn4u6wah7cpafohlmyohrj3a5rozt", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "26.0.0" +} diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index f744922..beab0f0 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -6,17 +6,17 @@ "absoluteEncoderOffset": 44.205, "drive": { "type": "sparkmax_neo", - "id": 8, + "id": 7, "canbus": null }, "angle": { "type": "sparkmax_neo", - "id": 7, + "id": 11, "canbus": null }, "encoder": { "type": "thrifty", - "id": 2, + "id": 3, "canbus": null }, "inverted": { diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index e1340f6..7e6e354 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -6,17 +6,17 @@ "absoluteEncoderOffset": 295.091, "drive": { "type": "sparkmax_neo", - "id": 12, + "id": 8, "canbus": null }, "angle": { "type": "sparkmax_neo", - "id": 11, + "id": 12, "canbus": null }, "encoder": { "type": "thrifty", - "id": 3, + "id": 4, "canbus": null }, "inverted": { diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 0de5d66..9d0ccec 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -6,17 +6,17 @@ "absoluteEncoderOffset": 6.450, "drive": { "type": "sparkmax_neo", - "id": 6, + "id": 5, "canbus": null }, "angle": { "type": "sparkmax_neo", - "id": 5, + "id": 9, "canbus": null }, "encoder": { "type": "thrifty", - "id": 0, + "id": 1, "canbus": null }, "inverted": { diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 7bd34ac..5d30a60 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -6,17 +6,17 @@ "absoluteEncoderOffset": 119.779, "drive": { "type": "sparkmax_neo", - "id": 10, + "id": 6, "canbus": null }, "angle": { "type": "sparkmax_neo", - "id": 9, + "id": 10, "canbus": null }, "encoder": { "type": "thrifty", - "id": 1, + "id": 2, "canbus": null }, "inverted": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4130dbe..8e7bebf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,12 +1,32 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; + +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; + +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; public class Constants { public static class SwerveDriveConstants { - public static final SwerveDriveKinematics k_kinematics = new SwerveDriveKinematics(); //get this in when we know bot dims + private final static Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + private final static Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + private final static Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + private final static Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + public static final SwerveDriveKinematics k_kinematics = new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + ); //get this in when we know bot dims + public static final double k_maxSpeed = Units.feetToMeters(14.5); + + public static final double k_maxDriverSpeed = 1.0; // Meters per second + public static final double k_maxDriverBoostSpeed = 4.5; + + public static final double k_boostScaler = k_maxDriverBoostSpeed / k_maxDriverSpeed; } public static class ControllerConstants{ @@ -35,4 +55,16 @@ public static class FieldConstants { public final static double k_width = Units.feetToMeters(26.0) + Units.inchesToMeters(5); public final static double k_length = Units.feetToMeters(57.0) + Units.inchesToMeters(6.0 + (7.0 / 8.0)); } + + public static class SimulationConstants{ + public final static boolean k_isInSimulation = true; + public final static DriveTrainSimulationConfig k_config = DriveTrainSimulationConfig.Default() + .withGyro(COTS.ofPigeon2()) + .withSwerveModule(COTS.ofMark4( + DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60 + DCMotor.getFalcon500(1), // Steer motor is a Falcon 500 + COTS.WHEELS.COLSONS.cof, // Use the COF for Colson Wheels + 3)) + .withBumperSize(Inches.of(28.5), Inches.of(33.5)); + } } diff --git a/src/main/java/frc/robot/Elastic.java b/src/main/java/frc/robot/Elastic.java new file mode 100644 index 0000000..ba31548 --- /dev/null +++ b/src/main/java/frc/robot/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.robot; + +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/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..fc23d71 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,16 +4,30 @@ package frc.robot; -import edu.wpi.first.wpilibj.TimedRobot; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private Command m_autonomousCommand; + private Simulation sim; private final RobotContainer m_robotContainer; public Robot() { + Logger.recordMetadata("ProjectName", "RA26_RobotCode"); + Logger.addDataReceiver(new NT4Publisher()); + + if (isReal()) { + Logger.addDataReceiver(new WPILOGWriter()); + } + + Logger.start(); + m_robotContainer = new RobotContainer(); } @@ -23,13 +37,16 @@ public void robotPeriodic() { } @Override - public void disabledInit() {} + public void disabledInit() { + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } @Override - public void disabledExit() {} + public void disabledExit() { + } @Override public void autonomousInit() { @@ -41,10 +58,12 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override - public void autonomousExit() {} + public void autonomousExit() { + } @Override public void teleopInit() { @@ -54,10 +73,12 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override - public void teleopExit() {} + public void teleopExit() { + } @Override public void testInit() { @@ -65,8 +86,22 @@ public void testInit() { } @Override - public void testPeriodic() {} + public void testPeriodic() { + } + + @Override + public void testExit() { + } + + @Override + public void simulationInit() { + sim = new Simulation(m_robotContainer.getSwerveSystem()); + + sim.init(); + } @Override - public void testExit() {} + public void simulationPeriodic() { + sim.periodic(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8138600..947ba73 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,29 +7,25 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.controls.DriverController; +import frc.robot.controls.DriverControls; import frc.robot.subsystems.SwerveSystem; public class RobotContainer { - private final SwerveSystem m_swerve = new SwerveSystem();; + private final SwerveSystem m_swerve = new SwerveSystem(); public RobotContainer() { configureBindings(); - - m_swerve.setDefaultCommand( - m_swerve.driveCommand( - DriverController.getController().getLeftY() * -1, - DriverController.getController().getLeftX() * -1, - DriverController.getController().getRightX() * -1 - ) - ); } private void configureBindings() { - DriverController.configure(Constants.ControllerConstants.kDriverControllerPort, m_swerve); + DriverControls.configure(Constants.ControllerConstants.kDriverControllerPort, m_swerve); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + + public SwerveSystem getSwerveSystem(){ + return m_swerve; + } } diff --git a/src/main/java/frc/robot/Simulation.java b/src/main/java/frc/robot/Simulation.java new file mode 100644 index 0000000..237e439 --- /dev/null +++ b/src/main/java/frc/robot/Simulation.java @@ -0,0 +1,27 @@ +package frc.robot; + +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import org.littletonrobotics.junction.Logger; + +import frc.robot.subsystems.SwerveSystem; + +public class Simulation { + private SimulatedArena m_arena; + private SwerveSystem m_swerve; + + public Simulation(SwerveSystem swerveDrive) { + m_swerve = swerveDrive; + m_arena = SimulatedArena.getInstance(); + m_arena.resetFieldForAuto(); + } + + public void init() { + } + + public void periodic() { + m_arena.simulationPeriodic(); + Logger.recordOutput("FieldSimulation/Fuel", + m_arena.getGamePiecesArrayByType("Fuel")); + Logger.recordOutput("Sim/RobotPose", m_swerve.getSwerveDrive().getPose()); + } +} diff --git a/src/main/java/frc/robot/auto/TaskSequencer.java b/src/main/java/frc/robot/auto/TaskSequencer.java new file mode 100644 index 0000000..74635ab --- /dev/null +++ b/src/main/java/frc/robot/auto/TaskSequencer.java @@ -0,0 +1,5 @@ +package frc.robot.auto; + +public class TaskSequencer { + +} diff --git a/src/main/java/frc/robot/auto/modes/Modes.java b/src/main/java/frc/robot/auto/modes/Modes.java new file mode 100644 index 0000000..fc0c50b --- /dev/null +++ b/src/main/java/frc/robot/auto/modes/Modes.java @@ -0,0 +1,40 @@ +package frc.robot.auto.modes; +import frc.robot.auto.tasks.DriveTask; +import frc.robot.auto.tasks.Tasks; + +import java.util.ArrayList; + +//left, right, center, score + +public abstract class Modes { + private ArrayList m_tasks; + + public Modes() { + m_tasks = new ArrayList<>(); + } + + public Tasks getNextTask() { + // Pop the first task off the list and return it + try { + return m_tasks.remove(0); + } catch (IndexOutOfBoundsException ex) { + return null; + } + } + + public void queueTask(Tasks task) { + m_tasks.add(task); + } + + public void queueTasks(ArrayList tasks) { + for (Tasks task : tasks) { + m_tasks.add(task); + } + } + + public void queueEnd() { + queueTask(new DriveTask(0, 0)); + } + + public abstract void queueTasks(); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/auto/tasks/DoNothingTask.java b/src/main/java/frc/robot/auto/tasks/DoNothingTask.java new file mode 100644 index 0000000..3071800 --- /dev/null +++ b/src/main/java/frc/robot/auto/tasks/DoNothingTask.java @@ -0,0 +1,35 @@ +package frc.robot.auto.tasks; + +import frc.robot.Telemetry; + +public class DoNothingTask extends Tasks { + @Override + public void prepare(){ + Telemetry.logString("DoNothingTask", "Starting do nothing auto..."); + m_prepared = true; + } + + @Override + public void updateSim(){ + logIsRunning(true); + + Telemetry.logString("DoNothingTask", "Do nothing auto complete"); + } + + @Override + public void update(){ + logIsRunning(true); + + Telemetry.logString("DoNothingTask", "Do nothing auto complete"); + } + + @Override + public void done(){ + logIsRunning(false); + } + + @Override + public boolean isFinished(){ + return true; + } +} diff --git a/src/main/java/frc/robot/auto/tasks/DriveTask.java b/src/main/java/frc/robot/auto/tasks/DriveTask.java new file mode 100644 index 0000000..36aca6b --- /dev/null +++ b/src/main/java/frc/robot/auto/tasks/DriveTask.java @@ -0,0 +1,84 @@ +package frc.robot.auto.tasks; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.subsystems.SwerveSystem; +import limelight.networktables.PoseEstimate; +import frc.robot.subsystems.LimelightSystem; +import frc.robot.Telemetry; + +//Drive Forward Based on Field +public class DriveTask extends Tasks{ + private SwerveSystem m_swerve; + private LimelightSystem m_limelight; + private double m_targetDistance; + private double m_speed; + private Pose2d m_startPose; + private Pose2d m_curPose; + private Pose2d m_checkerPose; + private double m_speedX = 0.0; + private double m_speedY = 0.0; + + public DriveTask(double distance, double speed){ + m_swerve = new SwerveSystem(); + m_limelight = new LimelightSystem(m_swerve.getSwerveDrive()); + m_targetDistance = distance; + m_speed = speed; + } + + public DriveTask(double distance, double xSpeed, double ySpeed){ + m_swerve = new SwerveSystem(); + m_limelight = new LimelightSystem(m_swerve.getSwerveDrive()); + m_targetDistance = distance; + m_speedX = xSpeed; + m_speedY = ySpeed; + } + + @Override + public void prepare() { + + m_limelight.getMeasurements().ifPresent((PoseEstimate pose) -> { + m_startPose = pose.pose.toPose2d(); + }); + + m_prepared = true; + } + + @Override + public void update() { + if (m_speedX != 0.0 || m_speedY != 0.0){ + m_swerve.driveSpeedCommand(m_speedX, m_speedY, 0, false); + } + else{ + logIsRunning(m_isFinished); + + m_limelight.getMeasurements().ifPresent((PoseEstimate pose) -> { + m_curPose = pose.pose.toPose2d(); + }); + + // Vx = V * cos(theta) + double xSpeed = m_speed * Math.cos(m_curPose.getRotation().getRadians()); + // Vy = V * sin(theta) + double ySpeed = m_speed * Math.sin(m_curPose.getRotation().getRadians()); + + m_swerve.driveSpeedCommand(xSpeed, ySpeed, 0, false); + } + } + + @Override + public boolean isFinished() { + m_limelight.getMeasurements().ifPresent((PoseEstimate pose) -> { + m_checkerPose = pose.pose.toPose2d(); + }); + + Pose2d relativePose = m_startPose.relativeTo(m_checkerPose); + return Math.hypot(relativePose.getX(), relativePose.getY()) >= m_targetDistance; + } + + @Override + public void done(){ + logIsRunning(false); + + Telemetry.logString("DriveForwardTask", "Auto driving done"); + m_swerve.driveSpeedCommand(0.0, 0.0, 0.0, false); + } +} diff --git a/src/main/java/frc/robot/auto/tasks/Rotate.java b/src/main/java/frc/robot/auto/tasks/Rotate.java new file mode 100644 index 0000000..5c731a5 --- /dev/null +++ b/src/main/java/frc/robot/auto/tasks/Rotate.java @@ -0,0 +1,51 @@ +package frc.robot.auto.tasks; + +import frc.robot.subsystems.SwerveSystem; +import limelight.networktables.PoseEstimate; +import frc.robot.subsystems.LimelightSystem; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; + +public class Rotate extends Tasks { + private SwerveSystem m_swerve; + private LimelightSystem m_limelight; + private double m_rot; + private Pose2d m_prevPose; + private Pose2d m_curPose; + + public Rotate(double rotation){ + m_rot = rotation; + m_swerve = new SwerveSystem(); + m_limelight = new LimelightSystem(m_swerve.getSwerveDrive()); + } + + @Override + public void prepare() { + m_limelight.getMeasurements().ifPresent((PoseEstimate pose) -> { + m_prevPose = pose.pose.toPose2d(); + }); + m_prepared = true; + } + + @Override + public void update() { + logIsRunning(m_isFinished); + m_swerve.driveCommand(0, 0, Units.degreesToRadians(m_rot)); + + } + + @Override + public boolean isFinished() { + m_limelight.getMeasurements().ifPresent((PoseEstimate pose) -> { + m_curPose = pose.pose.toPose2d(); + }); + + return m_prevPose.getRotation().getDegrees() - m_curPose.getRotation().getDegrees() == m_rot; + } + + @Override + public void done(){ + logIsRunning(false); + } + +} diff --git a/src/main/java/frc/robot/auto/tasks/Tasks.java b/src/main/java/frc/robot/auto/tasks/Tasks.java new file mode 100644 index 0000000..60853bb --- /dev/null +++ b/src/main/java/frc/robot/auto/tasks/Tasks.java @@ -0,0 +1,27 @@ +package frc.robot.auto.tasks; +import org.littletonrobotics.junction.Logger; + +public abstract class Tasks { + public boolean m_isFinished = false; + public boolean m_prepared = false; + + public abstract void prepare(); + + public abstract void update(); + + public void updateSim() { + } + + public abstract boolean isFinished(); + + public void done() { + }; + + public void logIsRunning(boolean isRunning) { + Logger.recordOutput("Auto/Tasks/" + this.getClass().getSimpleName(), isRunning); + } + + public boolean isPrepared() { + return m_prepared; + } +} diff --git a/src/main/java/frc/robot/auto/tasks/Wait.java b/src/main/java/frc/robot/auto/tasks/Wait.java new file mode 100644 index 0000000..c060c14 --- /dev/null +++ b/src/main/java/frc/robot/auto/tasks/Wait.java @@ -0,0 +1,34 @@ +package frc.robot.auto.tasks; +import edu.wpi.first.wpilibj.Timer; + +public class Wait extends Tasks { + private Timer m_runningTimer = new Timer(); + private double m_time; + + public Wait(double time) { + m_time = time; + } + + @Override + public void prepare() { + m_runningTimer.reset(); + m_runningTimer.start(); + m_prepared = true; + } + + @Override + public void update() { + logIsRunning(m_isFinished); + } + + @Override + public boolean isFinished() { + return m_runningTimer.get() >= m_time; + } + + @Override + public void done(){ + logIsRunning(false); + } + +} diff --git a/src/main/java/frc/robot/controls/DriverController.java b/src/main/java/frc/robot/controls/DriverControls.java similarity index 72% rename from src/main/java/frc/robot/controls/DriverController.java rename to src/main/java/frc/robot/controls/DriverControls.java index 3ee9fbe..a59812c 100644 --- a/src/main/java/frc/robot/controls/DriverController.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -5,11 +5,9 @@ import swervelib.SwerveInputStream; import frc.robot.Constants; -public class DriverController { - private static CommandXboxController controller; - +public class DriverControls { public static void configure(int port, SwerveSystem drivetrain) { - controller = new CommandXboxController(port); + CommandXboxController controller = new CommandXboxController(port); SwerveInputStream driveInputStream = SwerveInputStream.of(drivetrain.getSwerveDrive(), () -> controller.getLeftY() * -1, @@ -20,10 +18,7 @@ public static void configure(int port, SwerveSystem drivetrain) { // .scaleTranslation(0.25) // TODO: Tune speed scaling .deadband(Constants.ControllerConstants.k_DEADBAND); - drivetrain.setDriveInputStream(driveInputStream); - } - - public static CommandXboxController getController(){ - return controller; + drivetrain.setDefaultCommand( + drivetrain.driveFieldOriented(driveInputStream).withName("Drive")); } } diff --git a/src/main/java/frc/robot/controls/OperatorController.java b/src/main/java/frc/robot/controls/OperatorControls.java similarity index 88% rename from src/main/java/frc/robot/controls/OperatorController.java rename to src/main/java/frc/robot/controls/OperatorControls.java index 1ceb6f0..2cef173 100644 --- a/src/main/java/frc/robot/controls/OperatorController.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -public class OperatorController { +public class OperatorControls { public static void configure() { @SuppressWarnings("unused") CommandXboxController controller = new CommandXboxController(1); diff --git a/src/main/java/frc/robot/subsystems/LimelightSystem.java b/src/main/java/frc/robot/subsystems/LimelightSystem.java index e3f8f23..e6e8677 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSystem.java @@ -3,6 +3,7 @@ import java.util.Optional; import static edu.wpi.first.units.Units.DegreesPerSecond; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import limelight.Limelight; @@ -80,4 +81,8 @@ public boolean exceptions(PoseEstimate foo) { return true; } + + public Optional getMeasurements(){ + return limelight.createPoseEstimator(EstimationMode.MEGATAG2).getPoseEstimate(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSystem.java b/src/main/java/frc/robot/subsystems/SwerveSystem.java index 2b88066..ca2bf0c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSystem.java @@ -1,16 +1,21 @@ package frc.robot.subsystems; import java.io.File; import java.io.IOException; +import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import swervelib.parser.SwerveParser; +import swervelib.simulation.SwerveIMUSimulation; +import swervelib.simulation.ironmaple.simulation.drivesims.GyroSimulation; import swervelib.telemetry.SwerveDriveTelemetry; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import frc.robot.Constants; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -27,6 +32,7 @@ public class SwerveSystem extends SubsystemBase { SwerveDriveKinematics m_kinematics; SwerveDriveOdometry m_odometry; SwerveIMU m_gyro; + SwerveIMUSimulation m_simGyro; SwerveDrive swerveDrive; SwerveModule[] swerveModules; SwerveDriveOdometry odometry; @@ -36,7 +42,7 @@ public class SwerveSystem extends SubsystemBase { public SwerveSystem() { this.m_kinematics = Constants.SwerveDriveConstants.k_kinematics; - this.m_limelight = new LimelightSystem(swerveDrive); + this.m_limelight = Constants.SimulationConstants.k_isInSimulation ? null : new LimelightSystem(swerveDrive); File swerveDir = new File(Filesystem.getDeployDirectory(), "swerve"); @@ -54,10 +60,14 @@ public SwerveSystem() { this.swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); this.m_gyro = this.swerveDrive.getGyro(); + this.m_simGyro = new SwerveIMUSimulation(new GyroSimulation(0, 0)); Command onEnable = Commands.runOnce(() -> { Telemetry.log("running..."); - m_limelight.onEnabled(); + if (Constants.SimulationConstants.k_isInSimulation){} + else{ + m_limelight.onEnabled(); + } }); RobotModeTriggers.teleop().onTrue(onEnable); @@ -66,7 +76,7 @@ public SwerveSystem() { this.odometry = new SwerveDriveOdometry( this.m_kinematics, - this.m_gyro.getRotation3d().toRotation2d(), + Constants.SimulationConstants.k_isInSimulation ? this.m_simGyro.getGyroRotation3d().toRotation2d() : this.m_gyro.getRotation3d().toRotation2d(), new SwerveModulePosition[]{new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition()}, new Pose2d(0, 0, new Rotation2d()) ); @@ -74,7 +84,10 @@ public SwerveSystem() { @Override public void periodic(){ - m_limelight.periodic(); + if (Constants.SimulationConstants.k_isInSimulation){} + else{ + m_limelight.periodic(); + } } @Override @@ -93,11 +106,35 @@ public Command driveCommand(double translationX, double translationY, }); } + public Command driveFieldOriented(Supplier velocity) { + return run(() -> { + swerveDrive.driveFieldOriented(velocity.get()); + }); + } + + public void driveSpeedCommand(double speedX, double speedY, double rot, boolean basedOnField) { + SwerveModuleState[] swerveModuleStates = Constants.SwerveDriveConstants.k_kinematics.toSwerveModuleStates( + basedOnField + ? ChassisSpeeds.fromFieldRelativeSpeeds(speedX, speedY, rot, + Constants.SimulationConstants.k_isInSimulation ? this.m_simGyro.getGyroRotation3d().toRotation2d() : m_gyro.getRotation3d().toRotation2d() + ) : new ChassisSpeeds(speedX, speedY, rot) + ); + + double maxBoostSpeed = Constants.SwerveDriveConstants.k_maxSpeed * Constants.SwerveDriveConstants.k_boostScaler; + + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, maxBoostSpeed); + + SwerveModule[] modules = this.swerveDrive.getModules(); + for (int i = 0; i < modules.length; i++) { + modules[i].setDesiredState(swerveModuleStates[i], false, false); + } + } + public SwerveDrive getSwerveDrive() { return this.swerveDrive; } - public void setDriveInputStream(SwerveInputStream stream) { - this.driveInputStream = stream; + public SwerveDriveOdometry getOdometry(){ + return this.odometry; } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..162ad66 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 0000000..b674372 --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,30 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.3.14", + "frcYear": "2026", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.3.14" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [ + + ], + "cppDependencies": [ + + ] +}