Merge 2024 Drivetrain Code

Merge of 2024 Drivetrain code with 2025 Phoenix 6 library changes.
TODO: Verify all swerve constants

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

@@ -4,13 +4,19 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.constants.SWERVE;
+import frc.robot.constants.USB;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.utils.Telemetry;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -20,18 +26,53 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+  private final CommandSwerveDrivetrain m_swerveDrive = TunerConstants.createDrivetrain();
+  private final Telemetry m_telemetry = new Telemetry();
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
+  private final Joystick leftJoystick = new Joystick(USB.leftJoystick);
+  private final Joystick rightJoystick = new Joystick(USB.rightJoystick);
   private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+      new CommandXboxController(USB.xBoxController);
+
+  private double MaxSpeed =
+      TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
+  private double MaxAngularRate =
+      RotationsPerSecond.of(SWERVE.kMaxRotationRadiansPerSecond)
+          .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+
+  /* Setting up bindings for necessary control of the swerve drive platform */
+  private final SwerveRequest.FieldCentric drive =
+      new SwerveRequest.FieldCentric()
+          .withDeadband(MaxSpeed * 0.1)
+          .withRotationalDeadband(MaxAngularRate * 0.1); // Add a 10% deadband
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    m_swerveDrive.registerTelemetry(m_telemetry::telemeterize);
+
     // Configure the trigger bindings
+    initializeSubSystems();
     configureBindings();
   }
 
+  private void initializeSubSystems() {
+    m_swerveDrive.setDefaultCommand(
+        // Drivetrain will execute this command periodically
+        m_swerveDrive.applyRequest(
+            () ->
+                drive
+                    .withVelocityX(
+                        leftJoystick.getRawAxis(1)
+                            * MaxSpeed) // Drive forward with negative Y (forward)
+                    .withVelocityY(
+                        leftJoystick.getRawAxis(0) * MaxSpeed) // Drive left with negative X (left)
+                    .withRotationalRate(
+                        rightJoystick.getRawAxis(0)
+                            * MaxAngularRate) // Drive counterclockwise with negative X (left)
+            ));
+  }
+
   /**
    * Use this method to define your trigger->command mappings. Triggers can be created via the
    * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
@@ -42,13 +83,13 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
-
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+    //    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
+    //    new Trigger(m_exampleSubsystem::exampleCondition)
+    //        .onTrue(new ExampleCommand(m_exampleSubsystem));
+    //
+    //    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
+    //    // cancelling on release.
+    //    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
   }
 
   /**
@@ -58,6 +99,6 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
+    return new WaitCommand(0);
   }
 }

src/main/java/frc/robot/constants/CAN.java

@@ -0,0 +1,22 @@
+package frc.robot.constants;
+
+public class CAN {
+  public static final String rioCanbus = "rio";
+  public static String driveBaseCanbus = "drivebase";
+
+  public static final int pigeon = 9;
+
+  public static final int frontLeftCanCoder = 10;
+  public static final int frontRightCanCoder = 11;
+  public static final int backLeftCanCoder = 12;
+  public static final int backRightCanCoder = 13;
+
+  public static final int frontLeftDriveMotor = 20;
+  public static final int frontLeftTurnMotor = 21;
+  public static final int frontRightDriveMotor = 22;
+  public static final int frontRightTurnMotor = 23;
+  public static final int backLeftDriveMotor = 24;
+  public static final int backLeftTurnMotor = 25;
+  public static final int backRightDriveMotor = 26;
+  public static final int backRightTurnMotor = 27;
+}

src/main/java/frc/robot/constants/SWERVE.java

@@ -0,0 +1,76 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
+public class SWERVE {
+  public static double kTrackWidth = Units.inchesToMeters(21);
+  public static final double kWheelBase = Units.inchesToMeters(19);
+  public static final double kDriveBaseRadius =
+      Math.sqrt(Math.pow(kTrackWidth / 2.0, 2) + Math.pow(kWheelBase / 2.0, 2));
+
+  // TODO: Reimplement/add to team library
+  //    public static final Map<MODULE_POSITION, Translation2d> kModuleTranslations =
+  //            Map.of(
+  //                    MODULE_POSITION.FRONT_LEFT,
+  //                    new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0),
+  //                    MODULE_POSITION.FRONT_RIGHT,
+  //                    new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0),
+  //                    MODULE_POSITION.BACK_LEFT,
+  //                    new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0),
+  //                    MODULE_POSITION.BACK_RIGHT,
+  //                    new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0));
+  //
+  //    public static final SwerveDriveKinematics kSwerveKinematics =
+  //            new SwerveDriveKinematics(
+  //                    ModuleMap.orderedValues(kModuleTranslations, new Translation2d[0]));
+  public static final Translation2d kFrontLeftPosition =
+      new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0);
+  public static final Translation2d kFrontRightPosition =
+      new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0);
+  public static final Translation2d kBackLeftPosition =
+      new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0);
+  public static final Translation2d kBackRightPosition =
+      new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0);
+
+  public static final double kMaxSpeedMetersPerSecond = Units.feetToMeters(18);
+  public static final double kMaxRotationRadiansPerSecond = Math.PI * 2.0;
+
+  // TODO: Verify values
+  public static final double kDriveMotorGearRatio = 6.12;
+  public static final double kTurnMotorGearRatio = 150.0 / 7.0;
+  public static final double kCoupleRatio = 3.5714285714285716; // TODO: Verify
+  public static final double kWheelRadiusInches = 1.95;
+  public static final double kWheelDiameterMeters = 2.0 * Units.inchesToMeters(kWheelRadiusInches);
+
+  public static final boolean[] kTurnInversions = {true, false, false, false};
+  public static final boolean[] kDriveInversions = {false, false, false, true};
+
+  // In rotations
+  public static double kFrontLeftEncoderOffset = 0.219970703125;
+  public static double kFrontRightEncoderOffset = 0.265380859375;
+  public static double kBackLeftEncoderOffset = -0.046875;
+  public static double kBackRightEncoderOffset = 0.328125;
+
+  public static double kTurnKP = 100;
+  public static double kTurnKI = 0;
+  public static double kTurnKD = 0.5;
+  public static double kTurnKS = 0.1;
+  public static double kTurnKV = 1.91;
+  public static double kTurnKA = 0;
+
+  public static double kDriveKP = 0.1;
+  public static double kDriveKI = 0;
+  public static double kDriveKD = 0;
+  public static double kDriveKS = 0;
+  public static double kDriveKV = 0.124;
+  public static double kDriveKA = 0;
+
+  public static final double kTurnSatorCurrentLimit = 60;
+
+  public static final double kSlipCurrent = 120.0;
+  public static final double kDriveInertia = 0.01;
+  public static final double kTurnInertia = 0.01;
+  public static final double kDriveFrictionVoltage = 0.2;
+  public static final double kTurnFrictionVoltage = 0.2;
+}

src/main/java/frc/robot/constants/USB.java

@@ -0,0 +1,9 @@
+package frc.robot.constants;
+
+public final class USB {
+  public static final int leftJoystick = 0;
+  public static final int rightJoystick = 1;
+  public static final int xBoxController = 2;
+
+  public static final int testController = 3;
+}