From d639b3076d10f39eb5bc18c42c3dc5e707e991ff Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 21 Mar 2026 15:49:44 -0600 Subject: [PATCH] Ramp --- .../pathplanner/autos/ShootPreloadBump.auto | 43 +++++++++++ .../deploy/pathplanner/paths/Bump test.path | 75 +++++++++++++++++++ .../pathplanner/paths/BumpToCenter.path | 54 +++++++++++++ .../java/frc4388/robot/RobotContainer.java | 30 +++++++- .../robot/constants/BuildConstants.java | 10 +-- .../robot/constants/FieldConstants.java | 21 ++++-- .../robot/subsystems/swerve/SwerveDrive.java | 14 ++++ 7 files changed, 234 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/ShootPreloadBump.auto create mode 100644 src/main/deploy/pathplanner/paths/Bump test.path create mode 100644 src/main/deploy/pathplanner/paths/BumpToCenter.path diff --git a/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto new file mode 100644 index 0000000..43867c9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump test" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "BumpToCenter" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump test.path b/src/main/deploy/pathplanner/paths/Bump test.path new file mode 100644 index 0000000..dfb810c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump test.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4908076923076936, + "y": 6.633345195729537 + }, + "prevControl": null, + "nextControl": { + "x": 2.3977564102564113, + "y": 6.0868195547038955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4908076923076936, + "y": 5.6745769230769225 + }, + "prevControl": { + "x": 2.6330609248104158, + "y": 5.743196664476699 + }, + "nextControl": { + "x": 3.7815128205128214, + "y": 5.651320512820514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.967615384615385, + "y": 5.709461538461539 + }, + "prevControl": { + "x": 4.746653846153847, + "y": 5.430384615384616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5211640211640187, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.24536426676825 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.59775645089275 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BumpToCenter.path b/src/main/deploy/pathplanner/paths/BumpToCenter.path new file mode 100644 index 0000000..7051d4f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BumpToCenter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.935255041518388, + "y": 5.6650177935943065 + }, + "prevControl": null, + "nextControl": { + "x": 6.073930090574693, + "y": 5.873030367178768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.559893238434164, + "y": 7.547876631079477 + }, + "prevControl": { + "x": 7.1940806642941855, + "y": 7.182064056939502 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.98776039963968 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.19348942398209 + }, + "useDefaultConstraints": true +} \ 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 46eb3b0..2559999 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.Swerve.StayInPosition; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.subsystems.intake.Intake; @@ -155,7 +156,7 @@ public class RobotContainer { new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), new WaitCommand(5), IntakeRetracted, - new WaitCommand(5), + new WaitCommand(10), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) ); @@ -185,7 +186,29 @@ public class RobotContainer { // NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Intake Extended", IntakeExtended); NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); + + NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed)); + NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter)); + NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)); + NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter)); + NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter)); + NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> { + if (TimesNegativeOne.isRed) { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED); + } else { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE); + } + })); + + NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> { + if (!TimesNegativeOne.isRed) { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED); + } else { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE); + } + })); + DriverStation.silenceJoystickConnectionWarning(true); @@ -372,8 +395,11 @@ public class RobotContainer { private void configureSINGLEBindings() { //Driver controls + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED))); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b6a032f..b2c6e49 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 181; - public static final String GIT_SHA = "3c1b94a2d86a624a9320493a0240ee552491beca"; - public static final String GIT_DATE = "2026-03-21 13:04:40 MDT"; + public static final int GIT_REVISION = 182; + public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5"; + public static final String GIT_DATE = "2026-03-21 13:38:34 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-21 13:30:10 MDT"; - public static final long BUILD_UNIX_TIME = 1774121410165L; + public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT"; + public static final long BUILD_UNIX_TIME = 1774129559544L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java index f5a7245..49c75c0 100644 --- a/src/main/java/frc4388/robot/constants/FieldConstants.java +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -1,18 +1,27 @@ package frc4388.robot.constants; -import java.util.Arrays; +import static edu.wpi.first.units.Units.Meters; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; public final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + public static final double BUMP_OFFSET = 0.4; + public static final Transform2d BUMP_OFFSET_RED = new Transform2d( + Meters.of(BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d( + Meters.of(-BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + // Test april tag field layout // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( // Arrays.asList(new AprilTag[] { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 29a3686..5523eec 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -24,6 +24,7 @@ import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; @@ -433,6 +434,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + public void offsetOdoPosition(Transform2d offset) { + // Manually performing an addittion on the pose + // WHY doesn't WPILIB have the ability to not transform poses + Pose2d new_pose = new Pose2d( + new Translation2d( + state.currentPose.getX() + offset.getX(), + state.currentPose.getY() + offset.getY() + ), + state.currentPose.getRotation() + ); + this.io.resetPose(new_pose); + } + public void defenseXPosition(){ io.setControl(new SwerveRequest.SwerveDriveBrake()); }