From 5e867712889e45a2d9d17b6ee2847238e09e247d Mon Sep 17 00:00:00 2001 From: C4llSqin <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 13 Jan 2026 18:17:37 -0700 Subject: [PATCH] Make rotate code work --- src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 54 ++++++++++ src/main/deploy/pathplanner/settings.json | 32 ++++++ .../java/frc4388/robot/RobotContainer.java | 102 +++++++++--------- .../robot/commands/alignment/RotTo45.java | 37 +++++++ .../robot/constants/BuildConstants.java | 12 +-- 6 files changed, 181 insertions(+), 57 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/frc4388/robot/commands/alignment/RotTo45.java diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..ac5f521 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..3f475e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..5bf438c --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8569eb3..0f1a263 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,18 +7,14 @@ package frc4388.robot; -// Drive Systems -import edu.wpi.first.wpilibj.DriverStation; - import java.io.File; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.utility.controller.XboxController; +import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -27,43 +23,23 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.utility.controller.XboxController; import frc4388.robot.commands.MoveForTimeCommand; -import frc4388.robot.commands.MoveUntilSuply; -// import frc4388.robot.commands.alignment.DriveToReef; -import frc4388.robot.commands.alignment.DriveUntilLiDAR; -import frc4388.robot.commands.alignment.LidarAlign; -// import frc4388.robot.commands.wait.waitElevatorRefrence; -// import frc4388.robot.commands.wait.waitEndefectorRefrence; -// import frc4388.robot.commands.wait.waitFeedCoral; -import frc4388.robot.commands.wait.waitSupplier; +import frc4388.robot.commands.alignment.RotTo45; import frc4388.robot.constants.Constants; -import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems import frc4388.robot.subsystems.LED; -// import frc4388.robot.subsystems.elevator.Elevator; -// import frc4388.robot.subsystems.elevator.Elevator.CoordinationState; -import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; -// Utilites -import frc4388.utility.DeferredBlock; -import frc4388.utility.compute.TimesNegativeOne; -// import frc4388.utility.compute.ReefPositionHelper.Side; /** * This class is where the bulk of the robot should be declared. Since @@ -91,7 +67,7 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); + // private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); // public List subsystems = new ArrayList<>(); @@ -106,30 +82,54 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + + public RobotContainer() { + + configureButtonBindings(); + + DeferredBlock.addBlock(() -> { // Called on first robot enable + m_robotSwerveDrive.resetGyro(); + }, false); + DeferredBlock.addBlock(() -> { // Called on every robot enable + TimesNegativeOne.update(); + }, true); + + + DriverStation.silenceJoystickConnectionWarning(true); + + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + // m_robotSwerveDrive.driveWithInput(new Translation2d(.4, 0), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setToSlow(); + + } + + + /** - * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

- * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureVirtualButtonBindings() { - - // ? /* Driver Buttons */ + private void configureButtonBindings() { - /* Notice: the following buttons have not been replicated - * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback - * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. - * Auto Recording controls : We don't want an Null Ouroboros for an auto. - */ - // ? /* Operator Buttons */ + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - /* Notice: the following buttons have not been replicated - * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. - * We don't need it in an auto. - * Climbing controls : We don't need to climb in auto. - */ - - // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto. + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new RotTo45(m_robotSwerveDrive)); + + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); } @@ -215,7 +215,7 @@ public class RobotContainer { return this.m_operatorXbox; } - public ButtonBox getButtonBox() { - return this.m_buttonBox; - } + // public ButtonBox getButtonBox() { + // return this.m_buttonBox; + // } } diff --git a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java b/src/main/java/frc4388/robot/commands/alignment/RotTo45.java new file mode 100644 index 0000000..197cf8b --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/RotTo45.java @@ -0,0 +1,37 @@ +package frc4388.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +public class RotTo45 extends Command { + + SwerveDrive m_SwerveDrive; + Rotation2d targetAngle; + + + public RotTo45(SwerveDrive swerveDrive) { + m_SwerveDrive = swerveDrive; + + addRequirements(swerveDrive); + } + + @Override + public void initialize() { + targetAngle = new Rotation2d(); + } + + @Override + public void execute() { + m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle); + } + + @Override + public boolean isFinished() { + Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); + // TODO: Tune + return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5; + } + +} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9236f0e..bdbb588 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,14 +5,14 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; + public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 2; - public static final String GIT_SHA = "17c3ff1ec9ef6763ee1c736622be3ef0fcc30d10"; - public static final String GIT_DATE = "2026-01-08 19:42:41 MST"; + public static final int GIT_REVISION = 3; + public static final String GIT_SHA = "8dbb9d5a9099f417617ec2245e275a42b6788116"; + public static final String GIT_DATE = "2026-01-10 16:52:43 MST"; public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2026-01-10 16:21:06 MST"; - public static final long BUILD_UNIX_TIME = 1768087266722L; + public static final String BUILD_DATE = "2026-01-13 17:47:04 MST"; + public static final long BUILD_UNIX_TIME = 1768351624667L; public static final int DIRTY = 1; private BuildConstants(){}