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 diff --git a/simgui-ds.json b/simgui-ds.json index 1b43491..464a7db 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,6 +90,14 @@ } ], "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, + { + "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 07f73b0..7d577f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,14 +11,13 @@ 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; + public static final int kPoseControllerPort = 2; // 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/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 6f14f66..7f2bff9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,98 +3,28 @@ 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.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; -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.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.controls.OperatorControls; +import frc.robot.controls.PoseControls; 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. */ @@ -102,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(); @@ -119,63 +52,10 @@ 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); + OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null); + PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); } private void buildNamedAutoCommands() { @@ -195,10 +75,6 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } - public SwerveDrive getSwerveDrive() { return drivebase.getSwerveDrive(); } @@ -206,45 +82,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 new file mode 100644 index 0000000..2b181b9 --- /dev/null +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -0,0 +1,131 @@ +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; + +public class DriverControls { + + private static Pose2d getTargetPose() { + return PoseControls.getTargetPose(); + } + + public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { + CommandXboxController controller = new CommandXboxController(port); + + 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); + + controller.y().whileTrue(Commands.run( + () -> { + driveInputStream + .aim(getTargetPose()) + .aimWhile(true); + }).finallyDo(() -> driveInputStream.aimWhile(false))); + + drivetrain.setDefaultCommand( + 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)), + // new ProfiledPIDController(5, 0, 0, + // new Constraints( + // Units.degreesToRadians(360), + // Units.degreesToRadians(180)))); + + // controller.rightBumper().whileTrue(Commands.runEnd( + // () -> driveInputStream.driveToPoseEnabled(true), + // () -> driveInputStream.driveToPoseEnabled(false))); + + // NOTE: PathPlanner way of doing obstacle-aware drive to pose + controller.rightBumper() + .whileTrue(Commands.defer( + () -> drivetrain.driveToPose(drivetrain.getTargetPose()), + java.util.Set.of(drivetrain))); + + if (DriverStation.isTest()) { + // drivetrain.setDefaultCommand(driveFieldOrientedAngularVelocity); + // Overrides drive command above! + // Might be useful for robot-oriented controls in testing + + 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.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"); + } +} 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..ef4c3bd --- /dev/null +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -0,0 +1,12 @@ +package frc.robot.controls; + +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +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(port); + } +} 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..01e044b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -37,34 +37,31 @@ 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; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; -import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; @@ -148,6 +145,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)); @@ -156,6 +154,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(); }); @@ -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 @@ -211,6 +196,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? + } }); } @@ -318,6 +307,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. @@ -694,6 +697,10 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an Constants.MAX_SPEED); } + public Pose2d getTargetPose() { + return swerveDrive.field.getObject("targetPose").getPose(); + } + /** * Gets the current field-relative velocity (x, y and omega) of the robot *