From a1dfb4bd98c15b0ec55394d31c6cff3889bc3487 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 8 Jan 2026 14:50:09 -0500 Subject: [PATCH 01/10] Refactor driving controls --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 122 +----------------- .../frc/robot/controls/DriverControls.java | 102 +++++++++++++++ 3 files changed, 108 insertions(+), 122 deletions(-) create mode 100644 src/main/java/frc/robot/controls/DriverControls.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 07f73b0..eabfcab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,14 +11,12 @@ public final class Constants { public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms spark max velocity lag public static final double MAX_SPEED = Units.feetToMeters(14.5); - public static class OperatorConstants { + public static class ControllerConstants { public static final int kDriverControllerPort = 0; + public static final int kOperatorControllerPort = 1; // Joystick Deadband public static final double DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - public static final double TURN_CONSTANT = 6; } public static class DriveConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f14f66..3bf6d41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,14 +11,9 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.FeetPerSecond; @@ -27,74 +22,20 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.ControllerConstants; +import frc.robot.controls.DriverControls; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveDrive; -import swervelib.SwerveInputStream; public class RobotContainer { - // The robot's subsystems and commands are defined here... - // private final AlgaeSubsystem m_algaeSubsystem = new AlgaeSubsystem(); - // private final CoralSubsystem m_coralSubsystem = new CoralSubsystem(); - private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController driverXbox = new CommandXboxController( - OperatorConstants.kDriverControllerPort); - private final SendableChooser autoChooser; - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled - * by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) - .deadband(OperatorConstants.DEADBAND) - // .scaleTranslation(0.8) // TODO: Tune speed scaling - .allianceRelativeControl(true); - - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative - * input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX, - driverXbox::getRightY) - .headingWhile(true); - - /** - * Clone's the angular velocity input stream and converts it to a robotRelative - * input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) - .allianceRelativeControl(false); - - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis(4)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() - .withControllerHeadingAxis( - () -> driverXbox.getRawAxis(4), - () -> driverXbox.getRawAxis(5)) - .headingWhile(true) - .translationHeadingOffset(true) - .translationHeadingOffset(Rotation2d.fromDegrees(0)); - /** * The container for the robot. Contains subsystems, I/O devices, and commands. */ @@ -119,63 +60,8 @@ public RobotContainer() { } private void configureBindings() { - Command driveFieldOrientedAngularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - // Command driveSetpointGen = - // drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle); - - Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard) - .withName("Drive.FieldOrientedKeyboard"); - // Command driveSetpointGenKeyboard = - // drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboard); - - if (RobotBase.isSimulation()) { - drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); - } else { - // FIELD RELATIVE DRIVE - drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity.withName("Drive.FieldOriented")); - - // ROBOT RELATIVE DRIVE - // Command driveRobotOrientedAngularVelocity = - // drivebase.driveFieldOriented(driveRobotOriented); - // drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity.withName("Drive.RobotOriented")); - } - - if (Robot.isSimulation()) { - Pose2d target = new Pose2d(new Translation2d(1, 4), Rotation2d.fromDegrees(90)); - drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target); - - driveDirectAngleKeyboard.driveToPose(() -> target, - new ProfiledPIDController(5, 0, 0, - new Constraints(5, 2)), - new ProfiledPIDController(5, 0, 0, - new Constraints( - Units.degreesToRadians(360), - Units.degreesToRadians(180)))); - driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); - driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); - driverXbox.button(2).whileTrue(Commands.runEnd( - () -> driveDirectAngleKeyboard.driveToPoseEnabled(true), - () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); - } - - if (DriverStation.isTest()) { - drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity); // Overrides drive command above! - - driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.back().whileTrue(drivebase.centerModulesCommand()); - driverXbox.leftBumper().onTrue(Commands.none()); - driverXbox.rightBumper().onTrue(Commands.none()); - } else { - driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - - driverXbox.start().whileTrue(drivebase.sysIdAngleMotorCommand()); - driverXbox.back().whileTrue(drivebase.sysIdDriveMotorCommand()); - // driverXbox.back().whileTrue(fireAlgae()); - - driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.rightBumper().onTrue(Commands.none()); - } + // Set up controllers + DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null); } private void buildNamedAutoCommands() { diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java new file mode 100644 index 0000000..c6ab5ec --- /dev/null +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -0,0 +1,102 @@ +package frc.robot.controls; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.ControllerConstants; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.SwerveSubsystem; +import swervelib.SwerveInputStream; + +public class DriverControls { + public enum DriveMode { + ROBOT_ORIENTED, + FIELD_ORIENTED, + FIELD_ORIENTED_HEADING, + AIM_AT_POSE + } + + private static DriveMode driveMode = DriveMode.FIELD_ORIENTED_HEADING; + + public void setDriveMode(DriveMode mode) { + driveMode = mode; + } + + public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { + CommandXboxController controller = new CommandXboxController(ControllerConstants.kDriverControllerPort); + + SwerveInputStream driveInputStream = SwerveInputStream.of(drivetrain.getSwerveDrive(), + () -> controller.getLeftY() * -1, + () -> controller.getLeftX() * -1) + // .scaleTranslation(0.8) // TODO: Tune speed scaling + .deadband(ControllerConstants.DEADBAND); + + switch (driveMode) { + case ROBOT_ORIENTED -> driveInputStream + .robotRelative(true) + .allianceRelativeControl(false) + .withControllerRotationAxis(() -> controller.getRightX() * -1); + case FIELD_ORIENTED -> driveInputStream + .robotRelative(false) + .allianceRelativeControl(true) + .withControllerRotationAxis(() -> controller.getRightX() * -1) + .headingWhile(true); + case FIELD_ORIENTED_HEADING -> driveInputStream + .robotRelative(false) + .allianceRelativeControl(true) + .withControllerHeadingAxis( + controller::getRightX, + controller::getRightY) + .headingWhile(true) + .translationHeadingOffset(true) + .translationHeadingOffset(Rotation2d.fromDegrees(0)); + case AIM_AT_POSE -> { + // ??? + } + } + ; + drivetrain.setDefaultCommand( + drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + driveMode.name())); + + // if (Robot.isSimulation()) { + // Pose2d target = new Pose2d(new Translation2d(1, 4), + // Rotation2d.fromDegrees(90)); + // drivetrain.getSwerveDrive().field.getObject("targetPose").setPose(target); + + // driveDirectAngleKeyboard.driveToPose(() -> target, + // new ProfiledPIDController(5, 0, 0, + // new Constraints(5, 2)), + // new ProfiledPIDController(5, 0, 0, + // new Constraints( + // Units.degreesToRadians(360), + // Units.degreesToRadians(180)))); + // controller.start().onTrue(Commands.runOnce(() -> drivetrain.resetOdometry(new + // Pose2d(3, 3, new Rotation2d())))); + // controller.button(1).whileTrue(drivetrain.sysIdDriveMotorCommand()); + // controller.button(2).whileTrue(Commands.runEnd( + // () -> driveDirectAngleKeyboard.driveToPoseEnabled(true), + // () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); + // } + + if (DriverStation.isTest()) { + // drivetrain.setDefaultCommand(driveFieldOrientedAngularVelocity); // Overrides + // drive command above! + + controller.x().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); + controller.start().onTrue((Commands.runOnce(drivetrain::zeroGyro))); + controller.back().whileTrue(drivetrain.centerModulesCommand()); + controller.leftBumper().onTrue(Commands.none()); + controller.rightBumper().onTrue(Commands.none()); + } else { + controller.a().onTrue((Commands.runOnce(drivetrain::zeroGyro))); + + controller.start().whileTrue(drivetrain.sysIdAngleMotorCommand()); + controller.back().whileTrue(drivetrain.sysIdDriveMotorCommand()); + // controller.back().whileTrue(fireAlgae()); + + controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); + controller.rightBumper().onTrue(Commands.none()); + } + } +} From 545063d096031ed32bf10e538a24194e6efae891 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 8 Jan 2026 15:29:45 -0500 Subject: [PATCH 02/10] Ops --- src/main/java/frc/robot/controls/DriverControls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index c6ab5ec..e498bb6 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -55,7 +55,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur // ??? } } - ; + drivetrain.setDefaultCommand( drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + driveMode.name())); From 2db8fa47a8f3ea9424888d781aa6352009984cb6 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 8 Jan 2026 15:30:12 -0500 Subject: [PATCH 03/10] Better use FIELD_ORIENTED --- src/main/java/frc/robot/controls/DriverControls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index e498bb6..edd20c6 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -17,7 +17,7 @@ public enum DriveMode { AIM_AT_POSE } - private static DriveMode driveMode = DriveMode.FIELD_ORIENTED_HEADING; + private static DriveMode driveMode = DriveMode.FIELD_ORIENTED; public void setDriveMode(DriveMode mode) { driveMode = mode; From d5109cb13b3d7df5c826cc4ef9032c0f6308bb27 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Thu, 8 Jan 2026 16:52:51 -0500 Subject: [PATCH 04/10] Reimplement drive-to-pose, and a pose controller --- simgui-ds.json | 4 + src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 4 + .../frc/robot/controls/DriverControls.java | 41 ++--- .../java/frc/robot/controls/PoseControls.java | 160 ++++++++++++++++++ .../frc/robot/subsystems/SwerveSubsystem.java | 20 ++- 7 files changed, 202 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc/robot/controls/PoseControls.java diff --git a/simgui-ds.json b/simgui-ds.json index 1b43491..d22e3cd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,6 +90,10 @@ } ], "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, { "guid": "78696e70757401000000000000000000", "useGamepad": true diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eabfcab..7d577f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,6 +14,7 @@ public final class Constants { public static class ControllerConstants { public static final int kDriverControllerPort = 0; public static final int kOperatorControllerPort = 1; + public static final int kPoseControllerPort = 2; // Joystick Deadband public static final double DEADBAND = 0.1; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 21fa55c..348f7b7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,7 +5,6 @@ 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.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3bf6d41..e2a85f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants.ControllerConstants; import frc.robot.controls.DriverControls; +import frc.robot.controls.PoseControls; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveDrive; @@ -62,6 +63,9 @@ public RobotContainer() { private void configureBindings() { // Set up controllers DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null); + + // Configure pose controls for virtual target pose manipulation + PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); } private void buildNamedAutoCommands() { diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index edd20c6..87f6edc 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -1,6 +1,9 @@ package frc.robot.controls; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -40,8 +43,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur case FIELD_ORIENTED -> driveInputStream .robotRelative(false) .allianceRelativeControl(true) - .withControllerRotationAxis(() -> controller.getRightX() * -1) - .headingWhile(true); + .withControllerRotationAxis(() -> controller.getRightX() * -1); case FIELD_ORIENTED_HEADING -> driveInputStream .robotRelative(false) .allianceRelativeControl(true) @@ -59,35 +61,27 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur drivetrain.setDefaultCommand( drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + driveMode.name())); - // if (Robot.isSimulation()) { - // Pose2d target = new Pose2d(new Translation2d(1, 4), - // Rotation2d.fromDegrees(90)); - // drivetrain.getSwerveDrive().field.getObject("targetPose").setPose(target); + driveInputStream.driveToPose(drivetrain.getTargetPoseSupplier(), + new ProfiledPIDController(5, 0, 0, + new Constraints(5, 2)), + new ProfiledPIDController(5, 0, 0, + new Constraints( + Units.degreesToRadians(360), + Units.degreesToRadians(180)))); - // driveDirectAngleKeyboard.driveToPose(() -> target, - // new ProfiledPIDController(5, 0, 0, - // new Constraints(5, 2)), - // new ProfiledPIDController(5, 0, 0, - // new Constraints( - // Units.degreesToRadians(360), - // Units.degreesToRadians(180)))); - // controller.start().onTrue(Commands.runOnce(() -> drivetrain.resetOdometry(new - // Pose2d(3, 3, new Rotation2d())))); - // controller.button(1).whileTrue(drivetrain.sysIdDriveMotorCommand()); - // controller.button(2).whileTrue(Commands.runEnd( - // () -> driveDirectAngleKeyboard.driveToPoseEnabled(true), - // () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); - // } + controller.rightBumper().whileTrue(Commands.runEnd( + () -> driveInputStream.driveToPoseEnabled(true), + () -> driveInputStream.driveToPoseEnabled(false))); if (DriverStation.isTest()) { - // drivetrain.setDefaultCommand(driveFieldOrientedAngularVelocity); // Overrides - // drive command above! + // drivetrain.setDefaultCommand(driveFieldOrientedAngularVelocity); + // Overrides drive command above! + // Might be useful for robot-oriented controls in testing controller.x().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); controller.start().onTrue((Commands.runOnce(drivetrain::zeroGyro))); controller.back().whileTrue(drivetrain.centerModulesCommand()); controller.leftBumper().onTrue(Commands.none()); - controller.rightBumper().onTrue(Commands.none()); } else { controller.a().onTrue((Commands.runOnce(drivetrain::zeroGyro))); @@ -96,7 +90,6 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur // controller.back().whileTrue(fireAlgae()); controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); - controller.rightBumper().onTrue(Commands.none()); } } } diff --git a/src/main/java/frc/robot/controls/PoseControls.java b/src/main/java/frc/robot/controls/PoseControls.java new file mode 100644 index 0000000..779b8db --- /dev/null +++ b/src/main/java/frc/robot/controls/PoseControls.java @@ -0,0 +1,160 @@ +package frc.robot.controls; + +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 edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; +import frc.robot.subsystems.SwerveSubsystem; + +/** + * Controls for moving a virtual target pose on the field. + * The target pose can be used for autonomous navigation (drive-to-pose). + */ +public class PoseControls { + private static Pose2d targetPose = new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)); + + // Movement speeds for adjusting target pose + private static final double TRANSLATION_SPEED = 0.05; // meters per periodic cycle + private static final double ROTATION_SPEED = 2.0; // degrees per periodic cycle + + /** + * Configure pose controls on the given controller. + * + * @param controller The controller to bind pose adjustment controls to + * @param drivetrain The swerve subsystem for updating the field visualization + */ + public static void configure(int port, SwerveSubsystem drivetrain) { + CommandXboxController controller = new CommandXboxController(port); + + // Update the field visualization with initial pose + updateFieldPose(drivetrain); + + // Continuous joystick control for target pose adjustment + // Left stick: X/Y translation, Right stick X: Rotation + // Using ignoringDisable(true) so it runs even when disabled, and no + // requirements so it doesn't conflict + Commands.run(() -> { + double leftX = controller.getLeftX(); + double leftY = controller.getLeftY(); + double rightX = controller.getRightX(); + + // Apply deadband + if (Math.abs(leftX) < Constants.ControllerConstants.DEADBAND) + leftX = 0; + if (Math.abs(leftY) < Constants.ControllerConstants.DEADBAND) + leftY = 0; + if (Math.abs(rightX) < Constants.ControllerConstants.DEADBAND) + rightX = 0; + + // Alliance-relative controls: + // Forward (negative leftY) should move away from driver station + // Blue alliance: driver station at x=0, so forward = +X + // Red alliance: driver station at x=16.5m, so forward = -X (flip X direction) + // Left (negative leftX) should move toward left side of field from driver POV + // Blue alliance: left = +Y + // Red alliance: left = -Y (flip Y direction) + boolean isRedAlliance = DriverStation.getAlliance() + .map(alliance -> alliance == DriverStation.Alliance.Red) + .orElse(false); + + double allianceMultiplier = isRedAlliance ? -1.0 : 1.0; + + // Adjust target pose based on joystick input (alliance-relative) + if (leftX != 0 || leftY != 0) { + // -leftY = forward motion, leftX = strafe right + // Apply alliance multiplier to make controls relative to driver station + adjustTargetTranslation( + -leftY * TRANSLATION_SPEED * allianceMultiplier, // Forward/back -> X + -leftX * TRANSLATION_SPEED * allianceMultiplier); // Left/right -> Y + } + if (rightX != 0) { + adjustTargetRotation(-rightX * ROTATION_SPEED); + } + + updateFieldPose(drivetrain); + }).ignoringDisable(true).withName("PoseControls.Update").schedule(); + + // Y button: Reset target pose to robot's current position + controller.y().onTrue(Commands.runOnce(() -> { + targetPose = drivetrain.getPose(); + updateFieldPose(drivetrain); + }).ignoringDisable(true)); + } + + /** + * Adjust the target translation by the given delta values. + * + * @param deltaX Change in X position (meters) + * @param deltaY Change in Y position (meters) + */ + private static void adjustTargetTranslation(double deltaX, double deltaY) { + targetPose = new Pose2d( + targetPose.getX() + deltaX, + targetPose.getY() + deltaY, + targetPose.getRotation()); + } + + /** + * Adjust the target rotation by the given delta. + * + * @param deltaDegrees Change in rotation (degrees) + */ + private static void adjustTargetRotation(double deltaDegrees) { + targetPose = new Pose2d( + targetPose.getTranslation(), + targetPose.getRotation().plus(Rotation2d.fromDegrees(deltaDegrees))); + } + + /** + * Update the field visualization with the current target pose. + * + * @param drivetrain The swerve subsystem containing the field + */ + private static void updateFieldPose(SwerveSubsystem drivetrain) { + drivetrain.getSwerveDrive().field.getObject("targetPose").setPose(targetPose); + } + + /** + * Get the current target pose. + * + * @return The current target pose + */ + public static Pose2d getTargetPose() { + return targetPose; + } + + /** + * Get a supplier for the current target pose. + * Useful for commands that need a dynamic pose reference. + * + * @return Supplier that returns the current target pose + */ + public static Supplier getTargetPoseSupplier() { + return () -> targetPose; + } + + /** + * Set the target pose directly. + * + * @param pose The new target pose + */ + public static void setTargetPose(Pose2d pose) { + targetPose = pose; + } + + /** + * Set the target pose and update the field visualization. + * + * @param pose The new target pose + * @param drivetrain The swerve subsystem for updating the field + */ + public static void setTargetPose(Pose2d pose, SwerveSubsystem drivetrain) { + targetPose = pose; + updateFieldPose(drivetrain); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 7c580ab..2c7fab5 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -37,28 +37,26 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; - import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meter; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Inches; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; import limelight.Limelight; import limelight.networktables.AngularVelocity3d; import limelight.networktables.LimelightPoseEstimator; +import limelight.networktables.LimelightPoseEstimator.EstimationMode; import limelight.networktables.LimelightSettings.ImuMode; import limelight.networktables.LimelightSettings.LEDMode; import limelight.networktables.Orientation3d; import limelight.networktables.PoseEstimate; -import limelight.networktables.LimelightPoseEstimator.EstimationMode; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -694,6 +692,20 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an Constants.MAX_SPEED); } + public Pose2d getTargetPose() { + return swerveDrive.field.getObject("targetPose").getPose(); + } + + /** + * Get a supplier for the current target pose. + * Useful for commands that need a dynamic pose reference. + * + * @return Supplier that returns the current target pose + */ + public Supplier getTargetPoseSupplier() { + return () -> getTargetPose(); + } + /** * Gets the current field-relative velocity (x, y and omega) of the robot * From 7643ed27be93521cc3303d34b81e4b61ef03f686 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Thu, 8 Jan 2026 21:10:34 -0500 Subject: [PATCH 05/10] Eh --- .../frc/robot/controls/DriverControls.java | 28 ++++++++++++------- .../frc/robot/subsystems/SwerveSubsystem.java | 20 +++++++++++++ 2 files changed, 38 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 87f6edc..1102245 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.ControllerConstants; @@ -61,17 +62,24 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur drivetrain.setDefaultCommand( drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + driveMode.name())); - driveInputStream.driveToPose(drivetrain.getTargetPoseSupplier(), - new ProfiledPIDController(5, 0, 0, - new Constraints(5, 2)), - new ProfiledPIDController(5, 0, 0, - new Constraints( - Units.degreesToRadians(360), - Units.degreesToRadians(180)))); + // driveInputStream.driveToPose(drivetrain.getTargetPoseSupplier(), + // new ProfiledPIDController(5, 0, 0, + // new Constraints(5, 2)), + // new ProfiledPIDController(5, 0, 0, + // new Constraints( + // Units.degreesToRadians(360), + // Units.degreesToRadians(180)))); - controller.rightBumper().whileTrue(Commands.runEnd( - () -> driveInputStream.driveToPoseEnabled(true), - () -> driveInputStream.driveToPoseEnabled(false))); + // controller.rightBumper().whileTrue(Commands.runEnd( + // () -> driveInputStream.driveToPoseEnabled(true), + // () -> driveInputStream.driveToPoseEnabled(false))); + + // Use defer() to capture the target pose at button press time, not at robot + // init + controller.rightBumper() + .whileTrue(Commands.defer( + () -> drivetrain.driveToPose(drivetrain.getTargetPose()), + java.util.Set.of(drivetrain))); if (DriverStation.isTest()) { // drivetrain.setDefaultCommand(driveFieldOrientedAngularVelocity); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 2c7fab5..91f58d4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -146,6 +146,7 @@ public SwerveSubsystem(File directory) { System.out.println("Setting LL IMU Assist Alpha to 0.001"); limelight.getSettings() + // .withImuMode(ImuMode.InternalImuMT1Assist) .withImuAssistAlpha(0.001) .save(); }).ignoringDisable(true)); @@ -154,6 +155,7 @@ public SwerveSubsystem(File directory) { System.out.println("Setting LL IMU Assist Alpha to 0.01"); limelight.getSettings() + // .withImuMode(ImuMode.InternalImuMT1Assist) .withImuAssistAlpha(0.01) .save(); }); @@ -209,6 +211,10 @@ public void periodic() { poseEstimate.pose.toPose2d(), poseEstimate.timestampSeconds); // TODO: possibly add stddevs here + // TODO: Instead of providing the limelight's pose as is, replace the rotation + // component with the current pose rotation so the process doesn't take it into + // account? + } }); } @@ -316,6 +322,20 @@ public Command driveToPose(Pose2d pose) { ); } + public Command driveToPoseCommand(Supplier poseSupplier) { + // Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + swerveDrive.getMaximumChassisVelocity(), 4.0, + swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720)); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + poseSupplier.get(), + constraints, + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec + ); + } + /** * Drive with {@link SwerveSetpointGenerator} from 254, implemented by * PathPlanner. From 212060288c5b7a4b52b101acfc0276c722c493a6 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 9 Jan 2026 01:07:36 -0500 Subject: [PATCH 06/10] Auto aiming works --- simgui-ds.json | 4 ++ .../frc/robot/controls/DriverControls.java | 55 +++++-------------- .../frc/robot/subsystems/SwerveSubsystem.java | 25 --------- 3 files changed, 19 insertions(+), 65 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index d22e3cd..464a7db 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -94,6 +94,10 @@ "guid": "78696e70757401000000000000000000", "useGamepad": true }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, { "guid": "78696e70757401000000000000000000", "useGamepad": true diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 1102245..f8060e5 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -1,11 +1,7 @@ package frc.robot.controls; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.ControllerConstants; @@ -14,17 +10,9 @@ import swervelib.SwerveInputStream; public class DriverControls { - public enum DriveMode { - ROBOT_ORIENTED, - FIELD_ORIENTED, - FIELD_ORIENTED_HEADING, - AIM_AT_POSE - } - - private static DriveMode driveMode = DriveMode.FIELD_ORIENTED; - public void setDriveMode(DriveMode mode) { - driveMode = mode; + private static Pose2d getTargetPose() { + return PoseControls.getTargetPose(); } public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { @@ -33,35 +21,23 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur SwerveInputStream driveInputStream = SwerveInputStream.of(drivetrain.getSwerveDrive(), () -> controller.getLeftY() * -1, () -> controller.getLeftX() * -1) + .withControllerRotationAxis(() -> controller.getRightX() * -1) + .robotRelative(false) + .allianceRelativeControl(true) // .scaleTranslation(0.8) // TODO: Tune speed scaling .deadband(ControllerConstants.DEADBAND); - switch (driveMode) { - case ROBOT_ORIENTED -> driveInputStream - .robotRelative(true) - .allianceRelativeControl(false) - .withControllerRotationAxis(() -> controller.getRightX() * -1); - case FIELD_ORIENTED -> driveInputStream - .robotRelative(false) - .allianceRelativeControl(true) - .withControllerRotationAxis(() -> controller.getRightX() * -1); - case FIELD_ORIENTED_HEADING -> driveInputStream - .robotRelative(false) - .allianceRelativeControl(true) - .withControllerHeadingAxis( - controller::getRightX, - controller::getRightY) - .headingWhile(true) - .translationHeadingOffset(true) - .translationHeadingOffset(Rotation2d.fromDegrees(0)); - case AIM_AT_POSE -> { - // ??? - } - } + controller.y().whileTrue(Commands.run( + () -> { + driveInputStream + .aim(getTargetPose()) + .aimWhile(true); + }).finallyDo(() -> driveInputStream.aimWhile(false))); drivetrain.setDefaultCommand( - drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + driveMode.name())); + drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + ".test")); + // NOTE: YAGSL way of doing direct drive to pose // driveInputStream.driveToPose(drivetrain.getTargetPoseSupplier(), // new ProfiledPIDController(5, 0, 0, // new Constraints(5, 2)), @@ -74,8 +50,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur // () -> driveInputStream.driveToPoseEnabled(true), // () -> driveInputStream.driveToPoseEnabled(false))); - // Use defer() to capture the target pose at button press time, not at robot - // init + // NOTE: PathPlanner way of doing obstacle-aware drive to pose controller.rightBumper() .whileTrue(Commands.defer( () -> drivetrain.driveToPose(drivetrain.getTargetPose()), diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 91f58d4..01e044b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -62,7 +62,6 @@ import swervelib.SwerveDriveTest; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; -import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; @@ -178,20 +177,6 @@ public SwerveSubsystem(File directory) { setupPathPlanner(); } - /** - * Construct the swerve drive. - * - * @param driveCfg SwerveDriveConfiguration for the swerve. - * @param controllerCfg Swerve Controller. - */ - public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, - controllerCfg, - Constants.MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), - Rotation2d.fromDegrees(0))); - } - @Override public void periodic() { // swerveDrive.updateOdometry(); // TODO: see if this is needed @@ -716,16 +701,6 @@ public Pose2d getTargetPose() { return swerveDrive.field.getObject("targetPose").getPose(); } - /** - * Get a supplier for the current target pose. - * Useful for commands that need a dynamic pose reference. - * - * @return Supplier that returns the current target pose - */ - public Supplier getTargetPoseSupplier() { - return () -> getTargetPose(); - } - /** * Gets the current field-relative velocity (x, y and omega) of the robot * From 29447f7bdfc14e200952f58d7ec10c3755a1b2e3 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 9 Jan 2026 12:28:08 -0500 Subject: [PATCH 07/10] more controls cleanup --- src/main/java/frc/robot/RobotContainer.java | 45 ------------- .../frc/robot/controls/DriverControls.java | 65 +++++++++++++++++-- 2 files changed, 59 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e2a85f0..cfe84c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,10 +85,6 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } - public SwerveDrive getSwerveDrive() { return drivebase.getSwerveDrive(); } @@ -96,45 +92,4 @@ public SwerveDrive getSwerveDrive() { public Pose2d getRobotPose() { return drivebase.getPose(); } - - public SwerveDriveSimulation getSwerveDriveSimulation() { - return drivebase.getSwerveDrive().getMapleSimDrive().orElseThrow(); - } - - public Command fireAlgae() { - return Commands.runOnce(() -> { - System.err.println("FIRE!"); - - SimulatedArena arena = SimulatedArena.getInstance(); - - // Translation2d robotPosition, - // Translation2d shooterPositionOnRobot, - // ChassisSpeeds chassisSpeeds, - // Rotation2d shooterFacing, - // Distance initialHeight, - // LinearVelocity launchingSpeed, - // Angle shooterAngle - - ReefscapeAlgaeOnFly algae = new ReefscapeAlgaeOnFly( - drivebase.getPose().getTranslation(), - new Translation2d(), - drivebase.getSwerveDrive().getRobotVelocity().times(-1), - drivebase.getSwerveDrive().getOdometryHeading(), - Distance.ofBaseUnits(1, Feet), - LinearVelocity.ofBaseUnits(5, FeetPerSecond), - Angle.ofBaseUnits(45, Degrees)); - - // Configure callbacks to visualize the flight trajectory of the projectile - algae.withProjectileTrajectoryDisplayCallBack( - // Callback for when the note will eventually hit the target (if configured) - (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot", - pose3ds.toArray(Pose3d[]::new)), - // Callback for when the note will eventually miss the target, or if no target - // is configured - (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot", - pose3ds.toArray(Pose3d[]::new))); - - arena.addGamePieceProjectile(algae); - }).withName("Fire.Algae"); - } } diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index f8060e5..5123354 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -1,10 +1,25 @@ package frc.robot.controls; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Feet; +import static edu.wpi.first.units.Units.FeetPerSecond; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.ControllerConstants; +import frc.robot.Robot; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveInputStream; @@ -61,18 +76,56 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur // Overrides drive command above! // Might be useful for robot-oriented controls in testing - controller.x().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); - controller.start().onTrue((Commands.runOnce(drivetrain::zeroGyro))); - controller.back().whileTrue(drivetrain.centerModulesCommand()); - controller.leftBumper().onTrue(Commands.none()); - } else { controller.a().onTrue((Commands.runOnce(drivetrain::zeroGyro))); + controller.b().whileTrue(drivetrain.centerModulesCommand()); + controller.x().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); + controller.y().onTrue((Commands.runOnce(drivetrain::zeroGyro))); controller.start().whileTrue(drivetrain.sysIdAngleMotorCommand()); controller.back().whileTrue(drivetrain.sysIdDriveMotorCommand()); - // controller.back().whileTrue(fireAlgae()); + controller.leftBumper().onTrue(Commands.none()); + } else if (Robot.isSimulation()) { + controller.back().whileTrue(fireAlgae(drivetrain)); + } else { controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); } } + + public static Command fireAlgae(SwerveSubsystem drivetrain) { + return Commands.runOnce(() -> { + System.err.println("FIRE!"); + + SimulatedArena arena = SimulatedArena.getInstance(); + + // Translation2d robotPosition, + // Translation2d shooterPositionOnRobot, + // ChassisSpeeds chassisSpeeds, + // Rotation2d shooterFacing, + // Distance initialHeight, + // LinearVelocity launchingSpeed, + // Angle shooterAngle + + ReefscapeAlgaeOnFly algae = new ReefscapeAlgaeOnFly( + drivetrain.getPose().getTranslation(), + new Translation2d(), + drivetrain.getSwerveDrive().getRobotVelocity().times(-1), + drivetrain.getSwerveDrive().getOdometryHeading(), + Distance.ofBaseUnits(1, Feet), + LinearVelocity.ofBaseUnits(5, FeetPerSecond), + Angle.ofBaseUnits(45, Degrees)); + + // Configure callbacks to visualize the flight trajectory of the projectile + algae.withProjectileTrajectoryDisplayCallBack( + // Callback for when the note will eventually hit the target (if configured) + (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot", + pose3ds.toArray(Pose3d[]::new)), + // Callback for when the note will eventually miss the target, or if no target + // is configured + (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot", + pose3ds.toArray(Pose3d[]::new))); + + arena.addGamePieceProjectile(algae); + }).withName("Fire.Algae"); + } } From c93508983fc5eae9f2a6ad043e7593dd9172acd1 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 9 Jan 2026 12:32:49 -0500 Subject: [PATCH 08/10] Add base Operator controls --- src/main/java/frc/robot/RobotContainer.java | 22 +++++-------------- .../frc/robot/controls/OperatorControls.java | 13 +++++++++++ 2 files changed, 19 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/robot/controls/OperatorControls.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cfe84c0..7f2bff9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,23 +3,10 @@ import java.io.File; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; -import org.littletonrobotics.junction.Logger; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Translation2d; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Feet; -import static edu.wpi.first.units.Units.FeetPerSecond; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -28,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants.ControllerConstants; import frc.robot.controls.DriverControls; +import frc.robot.controls.OperatorControls; import frc.robot.controls.PoseControls; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveDrive; @@ -44,7 +32,10 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); buildNamedAutoCommands(); - DriverStation.silenceJoystickConnectionWarning(true); + + if (!Robot.isReal()) { + DriverStation.silenceJoystickConnectionWarning(true); + } // Have the autoChooser pull in all PathPlanner autos as options autoChooser = AutoBuilder.buildAutoChooser(); @@ -63,8 +54,7 @@ public RobotContainer() { private void configureBindings() { // Set up controllers DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null); - - // Configure pose controls for virtual target pose manipulation + OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null); PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java new file mode 100644 index 0000000..82bf62b --- /dev/null +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -0,0 +1,13 @@ +package frc.robot.controls; + +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.ControllerConstants; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.SwerveSubsystem; + +public class OperatorControls { + + public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { + CommandXboxController controller = new CommandXboxController(ControllerConstants.kDriverControllerPort); + } +} From 6a0be92750281cbbabbfe6ee17d85840643524a7 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 9 Jan 2026 13:23:53 -0500 Subject: [PATCH 09/10] Add build action --- .github/workflows/build.yml | 52 +++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 .github/workflows/build.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..f2899c1 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,52 @@ +name: Build + +on: + push: + branches: + - master + - main + pull_request: + branches: + - master + - main + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + java-version: '17' + distribution: 'temurin' + + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + - name: Cache Gradle packages + uses: actions/cache@v4 + with: + path: | + ~/.gradle/caches + ~/.gradle/wrapper + key: ${{ runner.os }}-gradle-${{ hashFiles('**/*.gradle*', '**/gradle-wrapper.properties') }} + restore-keys: | + ${{ runner.os }}-gradle- + + - name: Build with Gradle + run: ./gradlew build + + - name: Run tests + run: ./gradlew test + + - name: Upload build artifacts + uses: actions/upload-artifact@v4 + if: always() + with: + name: build-reports + path: build/reports/ + retention-days: 7 From a535c5a0db7a259e5f954bc13850af75779e5421 Mon Sep 17 00:00:00 2001 From: dracco1993 Date: Fri, 9 Jan 2026 13:28:46 -0500 Subject: [PATCH 10/10] Correct unused method param --- src/main/java/frc/robot/controls/DriverControls.java | 2 +- src/main/java/frc/robot/controls/OperatorControls.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 5123354..2b181b9 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -31,7 +31,7 @@ private static Pose2d getTargetPose() { } public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { - CommandXboxController controller = new CommandXboxController(ControllerConstants.kDriverControllerPort); + CommandXboxController controller = new CommandXboxController(port); SwerveInputStream driveInputStream = SwerveInputStream.of(drivetrain.getSwerveDrive(), () -> controller.getLeftY() * -1, diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index 82bf62b..ef4c3bd 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -1,13 +1,12 @@ package frc.robot.controls; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants.ControllerConstants; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; public class OperatorControls { public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { - CommandXboxController controller = new CommandXboxController(ControllerConstants.kDriverControllerPort); + CommandXboxController controller = new CommandXboxController(port); } }