diff --git a/src/main/java/frc/lib/controllers/VirtualXboxController.java b/src/main/java/frc/lib/controllers/VirtualXboxController.java index 1861d5d..bc06785 100644 --- a/src/main/java/frc/lib/controllers/VirtualXboxController.java +++ b/src/main/java/frc/lib/controllers/VirtualXboxController.java @@ -52,6 +52,15 @@ public Command rumbleFor(double duration, RumbleType rumbleType, double value) { .finallyDo(() -> getHID().setRumble(RumbleType.kBothRumble, 0.0)); } + // New + public Command rumble(RumbleType rumbleType, double value) { + return Commands.run(() -> getHID().setRumble(rumbleType, value)); + } + + public Command stopRumble() { + return Commands.run(() -> getHID().setRumble(RumbleType.kBothRumble, 0.0)); + } + @Override public Trigger leftBumper(EventLoop loop) { return super.leftBumper(loop) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d3ae55e..d73976e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -472,6 +472,8 @@ public static final class SwerveConstants { public static final double maxAngularSpeed = Units.degreesToRadians(180); public static final double slowMotionMaxTranslationalSpeed = Units.feetToMeters(3.53); public static final double turboMaxTranslationalSpeed = Units.feetToMeters(14.5); + // New + public static final double bufferMaxTranslationalSpeed = Units.feetToMeters(1.0); public static final double maxTranslationalAcceleration = Units.feetToMeters(18.0); public static final double maxAngularAcceleration = Units.feetToMeters(270.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 701f367..e289554 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,12 +7,14 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; @@ -89,6 +91,18 @@ public class RobotContainer implements Logged { private Alert swervePrematchAlert = new Alert("", AlertType.INFO); private Alert generalPrematchAlert = new Alert("", AlertType.INFO); + // New + Pose2d currentPose = swerve.getPose(); + double xCoordinate = currentPose.getX(); + double yCoordinate = currentPose.getY(); + private final double stopLength = .3; + private final double MAX_X = swerve.MAX_X; + private final double MIN_X = swerve.MIN_X; + private final double MAX_Y = swerve.MAX_Y; + private final double MIN_Y = swerve.MIN_Y; + private double stopTime = 0; + private boolean isStopped = false; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { DataLogManager.log("Initializing Robot Container"); @@ -291,6 +305,11 @@ public RobotContainer() { new Trigger(intake::beamBroken) .and(RobotModeTriggers.teleop()) .onTrue(driverController.rumbleFor(0.25, RumbleType.kRightRumble, 1.0)); + // New + new Trigger(swerve::isNearPracticeOutOfBounds) + .and(RobotModeTriggers.teleop()) + .onTrue(driverController.rumble(RumbleType.kBothRumble, .5)) + .onFalse(driverController.stopRumble()); new Trigger(() -> DriverStation.getMatchTime() > 0.0 && DriverStation.getMatchTime() < 23.0) .and(RobotModeTriggers.teleop()) @@ -349,6 +368,82 @@ private void configureDriveBindings() { driverController.x().whileTrue(swerve.run(swerve::lockModules)); + // New + driverController + .b() + .and(driverController.a()) + .onTrue( + Commands.sequence( + Commands.runOnce( + () -> { + swerve.setZeroPosition(); + SmartDashboard.putString("Status", "Indoor Drive Competition Mode Enabled"); + }), + new TeleopSwerve( + driverController::getLeftY, + driverController::getLeftX, + driverController::getRightX, + driverController::getRightY, + driverController::getLeftBumper, + () -> { + double currentTime = Timer.getFPGATimestamp(); + + if (xCoordinate >= MAX_X - 2) { + return Math.max( + 0.1, SwerveConstants.maxTranslationalSpeed * (MAX_X - xCoordinate) / 2); + } else if (xCoordinate <= MIN_X + 2) { + return Math.max( + 0.1, SwerveConstants.maxTranslationalSpeed * (xCoordinate - MIN_X) / 2); + } else if (yCoordinate >= MAX_Y - 2) { + return Math.max( + 0.1, SwerveConstants.maxTranslationalSpeed * (MAX_Y - yCoordinate) / 2); + } else if (yCoordinate <= MIN_Y + 2) { + return Math.max( + 0.1, SwerveConstants.maxTranslationalSpeed * (yCoordinate - MIN_Y) / 2); + } + + if (xCoordinate >= MAX_X - stopLength + || xCoordinate <= MIN_X + stopLength + || yCoordinate >= MAX_Y - stopLength + || yCoordinate <= MIN_Y + stopLength) { + + if (!isStopped) { + stopTime = currentTime; + isStopped = true; + } + return 0; + } + + if (isStopped) { + if (currentTime - stopTime >= 2.0) { + isStopped = false; + } else { + return 0; + } + } + + return SwerveConstants.maxTranslationalSpeed; + }, + SwerveConstants.maxAngularSpeed, + swerve))); + + // driverController.b().onTrue( + // new TeleopSwerve(driverController::getLeftY, + // driverController::getLeftX, + // driverController::getRightX, + // driverController::getRightY, + // driverController::getLeftBumper, + // () -> { + // if(xCoordinate > 7.5 || xCoordinate < -7.5 || yCoordinate > 5 || yCoordinate <-5) { + // driverController.rumbleFor(5, RumbleType.kBothRumble, 1.0); + // return SwerveConstants.bufferMaxTranslationalSpeed; + // } + // return SwerveConstants.maxTranslationalSpeed; + // }, + // SwerveConstants.maxAngularSpeed + // , swerve) + // ); + // slowMode // .and(turnToSpeaker.negate()) // .whileTrue( @@ -364,6 +459,7 @@ private void configureDriveBindings() { driverController .a() + .and(driverController.b().negate()) .onTrue( Commands.runOnce( () -> diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 46ce4ed..aac1d13 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -87,6 +87,13 @@ import org.photonvision.targeting.PhotonTrackedTarget; public class Swerve extends VirtualSubsystem implements Logged { + // New + private Pose2d zeroPosition = new Pose2d(0, 0, new Rotation2d()); + public final double MIN_X = -7.5; + public final double MIN_Y = -5.0; + public final double MAX_X = 7.5; + public final double MAX_Y = 5.0; + public static record PoseEstimate(Pose3d estimatedPose, double timestamp, Vector standardDevs) implements Comparable { @Override @@ -618,6 +625,24 @@ public SwerveModuleState[] getDesiredStates() { }; } + // New + Pose2d currentPose = getPose(); + double xCoordinate = currentPose.getX(); + double yCoordinate = currentPose.getY(); + + public boolean isNearPracticeOutOfBounds() { + if (xCoordinate >= MAX_X - 1) { + return true; + } else if (xCoordinate <= MIN_X + 1) { + return true; + } else if (yCoordinate >= MAX_Y - 1) { + return true; + } else if (yCoordinate <= MIN_Y + 1) { + return true; + } + return false; + } + private boolean odometryUpdateValid(Twist2d delta) { Pose2d poseExponential = new Pose2d().exp(delta); @@ -1002,6 +1027,11 @@ public void periodic() { ((double) (odometryUpdateCount - odometryRejectCount) / odometryUpdateCount) * 100.0); } + // New + public void setZeroPosition() { + poseEstimator.resetPosition(getRawHeading(), getModulePositions(), zeroPosition); + } + @Override public void simulationPeriodic() { frontLeftModule.simulationPeriodic();