Merge branch 'main' of https://github.com/Team3256/Kirby_Bot_2024

src/main/java/frc/robot/Constants.java

@@ -19,6 +19,11 @@ public final class Constants {
  public static class ControllerConstants {
  public static final int kDriverControllerPort = 0;
  public static final int kOperatorControllerPort = 1;
+
+ public static class DriverConstants {
+ public static final double kStickDeadband = 0.1;
+ public static final double kRotationalDeadband = 0.12;
+ }
  }
 
  // Naming scheme for FeatureFlags:

src/main/java/frc/robot/RobotContainer.java

@@ -9,9 +9,13 @@
 
 import choreo.auto.AutoChooser;
 import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 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.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -41,9 +45,12 @@
 import frc.robot.subsystems.spindex.Spindex;
 import frc.robot.subsystems.spindex.SpindexConstants;
 import frc.robot.subsystems.spindex.SpindexIOTalonFX;
+import frc.robot.subsystems.swerve.AzimuthConstants;
 import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
+import frc.robot.subsystems.swerve.SwerveConstants;
 import frc.robot.subsystems.swerve.SwerveTelemetry;
 import frc.robot.subsystems.swerve.TunerConstants;
+import frc.robot.subsystems.swerve.requests.SwerveFieldCentricFacingAngle;
 import frc.robot.subsystems.turret.*;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIOLimelight;
@@ -129,13 +136,18 @@ public class RobotContainer {
  public RobotContainer() {
  // Configure the trigger bindings
  configureBindings();
+ configureSwerve();
  configureAutoChooser();
- if (Utils.isSimulation()) {
- swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
- }
- swerve.registerTelemetry(swerveTelemetry::telemeterize);
 
  SimMechs.init();
+ // Schedule `exampleMethodCommand` when the Xbox controller's B button is
+ // pressed,
+ // cancelling on release.
+ // This should be at the end of the configureBindings method.
+ // No other bindings should be added after this line.
+ if (Constants.FeatureFlags.kControllerMapEnabled) {
+ controls.dumpControllerMap();
+ }
  }
 
  /**
@@ -155,21 +167,127 @@ private void configureBindings() {
  controls
  .bindDriver("y", "Set pivot shooter position -100")
  .onTrue(pivotShooter.setPosition(-100));
- // Schedule `exampleMethodCommand` when the Xbox controller's B button is
- // pressed,
- // cancelling on release.
- // This should be at the end of the configureBindings method.
- // No other bindings should be added after this line.
- if (Constants.FeatureFlags.kControllerMapEnabled) {
- controls.dumpControllerMap();
- }
  }
 
  private void configureAutoChooser() {
  autoChooser.addAutoRoutine("Box", autoRoutines::boxAuto);
  }
 
- private void configureSwerve() {}
+ private void configureSwerve() {
+ double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps;
+ double MaxAngularRate = 1.5 * Math.PI; // My drivetrain
+
+ double SlowMaxSpeed = MaxSpeed * 0.3;
+ double SlowMaxAngular = MaxAngularRate * 0.3;
+
+ SwerveRequest.FieldCentric drive =
+ new SwerveRequest.FieldCentric()
+ .withDeadband(Constants.ControllerConstants.DriverConstants.kStickDeadband * MaxSpeed)
+ .withRotationalDeadband(
+ Constants.ControllerConstants.DriverConstants.kRotationalDeadband
+ * MaxAngularRate) // Add a 10% deadband
+ .withDriveRequestType(
+ SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric
+
+ SwerveFieldCentricFacingAngle azi =
+ new SwerveFieldCentricFacingAngle()
+ .withDeadband(MaxSpeed * .15) // TODO: update deadband
+ .withRotationalDeadband(MaxAngularRate * .15) // TODO: update deadband
+ .withHeadingController(SwerveConstants.azimuthController)
+ .withDriveRequestType(SwerveModule.DriveRequestType.Velocity);
+
+ swerve.setDefaultCommand(
+ // Drivetrain will execute this command periodically
+ swerve.applyRequest(
+ () ->
+ drive
+ .withVelocityX(controls.driver.getLeftY() * MaxSpeed) // Drive -y is forward
+ .withVelocityY(controls.driver.getLeftX() * MaxSpeed) // Drive -x is left
+ .withRotationalRate(-controls.driver.getRightX() * MaxAngularRate)));
+
+ /*
+ * Right stick absolute angle mode on trigger hold,
+ * robot adjusts heading to the angle right joystick creates
+ */
+ controls
+ .bindDriver("rightTrigger", "No idea")
+ .whileTrue(
+ swerve.applyRequest(
+ () ->
+ drive
+ .withVelocityX(
+ -controls.driver.getLeftY() * MaxSpeed) // Drive -y is forward
+ .withVelocityY(-controls.driver.getLeftX() * MaxSpeed) // Drive -x is left
+ .withRotationalRate(-controls.driver.getRightX() * MaxAngularRate)));
+
+ // Slows translational and rotational speed to 30%
+ controls
+ .bindDriver("leftTrigger", "help me sam")
+ .whileTrue(
+ swerve.applyRequest(
+ () ->
+ drive
+ .withVelocityX(controls.driver.getLeftY() * (MaxSpeed * 0.17))
+ .withVelocityY(controls.driver.getLeftX() * (MaxSpeed * 0.17))
+ .withRotationalRate(-controls.driver.getRightX() * (1.3 * 0.2 * Math.PI))));
+
+ // Reset robot heading on button press
+ controls
+ .bindDriver("y", "Seed field relative")
+ .onTrue(swerve.runOnce(() -> swerve.seedFieldRelative()));
+
+ // Azimuth angle bindings. isRed == true for red alliance presets. isRed != true
+ // for blue.
+ if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) {
+ controls
+ .bindDriver("rightBumper", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSourceRed)));
+ controls
+ .bindDriver("a", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziAmpRed)));
+ controls
+ .bindDriver("povRight", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziFeederRed)));
+ } else {
+ controls
+ .bindDriver("rightBumper", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSourceBlue)));
+ controls
+ .bindDriver("a", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziAmpBlue)));
+ controls
+ .bindDriver("povRight", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziFeederBlue)));
+ }
+
+ // Universal azimuth bindings
+ controls
+ .bindDriver("leftBumper", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSubwooferFront)));
+ controls
+ .bindDriver("povDownLeft", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSubwooferLeft)));
+ controls
+ .bindDriver("povDownRight", "no idea")
+ .whileTrue(
+ swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSubwooferRight)));
+ controls
+ .bindDriver("povDown", "no idea")
+ .whileTrue(swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziCleanUp)));
+
+ if (Utils.isSimulation()) {
+ swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
+ }
+ swerve.registerTelemetry(swerveTelemetry::telemeterize);
+ }
 
  /**
  * Use this to pass the autonomous command to the main {@link Robot} class.

src/main/java/frc/robot/subsystems/swerve/AzimuthConstants.java

@@ -0,0 +1,41 @@
+// Copyright (c) 2024 FRC 3256
+// https://github.com/Team3256
+//
+// Use of this source code is governed by a 
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.subsystems.swerve;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public final class AzimuthConstants {
+ /* Angles (raw) */
+
+ // TODO: TUNE ALL OF THESE ON PRAC DAY
+ // front of the robot is the intake
+
+ public static final Rotation2d aziAmpRed = Rotation2d.fromDegrees(90);
+
+ public static final Rotation2d aziAmpBlue = Rotation2d.fromDegrees(-aziAmpRed.getDegrees());
+
+ public static final Rotation2d aziSubwooferFront = Rotation2d.fromDegrees(0);
+
+ public static final Rotation2d aziSubwooferLeft = Rotation2d.fromDegrees(-30);
+
+ public static final Rotation2d aziSubwooferRight =
+ Rotation2d.fromDegrees(aziSubwooferLeft.getDegrees());
+
+ public static final Rotation2d aziSourceRed = Rotation2d.fromDegrees(60);
+
+ public static final Rotation2d aziSourceBlue = Rotation2d.fromDegrees(-aziSourceRed.getDegrees());
+
+ public static final Rotation2d aziFeederRed = Rotation2d.fromDegrees(45);
+
+ public static final Rotation2d aziFeederBlue = Rotation2d.fromDegrees(-aziFeederRed.getDegrees());
+
+ public static final Rotation2d aziCleanUp = Rotation2d.fromDegrees(180);
+
+ /* Timeout */
+ public static final double aziCommandTimeOut = 1.5;
+}

src/main/java/frc/robot/utils/ControllerMapper.java

@@ -15,8 +15,8 @@
 import org.littletonrobotics.junction.Logger;
 
 public class ControllerMapper {
- private CommandXboxController driver;
- private CommandXboxController operator;
+ public CommandXboxController driver;
+ public CommandXboxController operator;
 
  public Map<String, Map<String, String>> buttonMap = new HashMap<>();