Move drive constants in driveconstants

Move driver and operator controllers into subclass
Simbotics · Jan 4, 2024 · e391c3c · e391c3c
1 parent 132ae53
commit e391c3c
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 72 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -15,83 +15,70 @@
 import frc.robot.subsystems.led.LEDSubsystem;
 
 public class RobotContainer {
-  private double MaxSpeed = 6; // 6 meters per second desired top speed
-  private double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity
+    private class Controller {
+        public static final CommandController driver = new CommandController(0);
+        public static final CommandController operator = new CommandController(1);
+    }
 
-  /* Setting up bindings for necessary control of the swerve drive platform */
-  private final CommandController driver = new CommandController(0); // Driver Controller
-  private final CommandController operator = new CommandController(1); // Operator Controller
-  final DriveSubsystem drivetrain = DriveConstants.DriveTrain; // My drivetrain
-  private final LEDSubsystem ledSubsystem;
-  private final SwerveRequest.FieldCentric drive =
-      new SwerveRequest.FieldCentric()
-          .withDeadband(MaxSpeed * 0.1)
-          .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
-          .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric
-  // driving in open loop
+    private final LEDSubsystem ledSubsystem;
 
-  private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
-  private final SwerveRequest.RobotCentric forwardStraight =
-      new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
-  private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+    // Set up the base for the drive and drivetrain
+    final DriveSubsystem drivetrain = DriveConstants.DriveTrain;
+    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(DriveConstants.kMaxAngularRate * 0.1).withRotationalDeadband(DriveConstants.kMaxAngularRate * 0.1)
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
 
-  /* Path follower */
-  private Command runAuto = drivetrain.getAutoPath("Tests");
+    private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+    private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric()
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+    private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
-  private final Telemetry logger = new Telemetry(MaxSpeed);
+    /* Path follower */
+    private Command runAuto = drivetrain.getAutoPath("Tests");
 
-  private void configureBindings() {
-    ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
+    private final Telemetry logger = new Telemetry(DriveConstants.kMaxSpeed);
 
-    drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
-        drivetrain
-            .applyRequest(
-                () ->
-                    drive
-                        .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with
-                        // negative Y (forward)
-                        .withVelocityY(
-                            -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left)
-                        .withRotationalRate(
-                            -driver.getRightX()
-                                * MaxAngularRate) // Drive counterclockwise with negative X
-                // (left)
-                )
-            .ignoringDisable(true));
+    private void configureBindings() {
+        ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
 
-    driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake));
-    driver
-        .yellowButton()
-        .whileTrue(
-            drivetrain.applyRequest(
-                () ->
-                    point.withModuleDirection(
-                        new Rotation2d(-driver.getLeftY(), -driver.getLeftX()))));
+        drivetrain.setDefaultCommand(
+                drivetrain
+                        .applyRequest(
+                                () -> drive
+                                        .withVelocityX(-Controller.driver.getLeftY() * DriveConstants.kMaxSpeed) // Drive forward with
+                                        // negative Y (forward)
+                                        .withVelocityY(
+                                                -Controller.driver.getLeftX() * DriveConstants.kMaxSpeed) // Drive left with negative X (left)
+                                        .withRotationalRate(
+                                                -Controller.driver.getRightX()
+                                                        * DriveConstants.kMaxAngularRate) // Drive counterclockwise with negative X
+                                                                          // (left)
+                        )
+                        .ignoringDisable(true));
 
-    // if (Utils.isSimulation()) {
-    // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(),
-    // Rotation2d.fromDegrees(90)));
-    // }
-    drivetrain.registerTelemetry(logger::telemeterize);
-    driver.POVUp()
-        .whileTrue(
-            drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
-    driver.POVDown()
-        .whileTrue(
-            drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
-  }
+        Controller.driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake));
+        Controller.driver
+                .yellowButton()
+                .whileTrue(
+                        drivetrain.applyRequest(
+                                () -> point.withModuleDirection(
+                                        new Rotation2d(-Controller.driver.getLeftY(), -Controller.driver.getLeftX()))));
 
-  public RobotContainer() {
-    configureBindings();
-    ledSubsystem = new LEDSubsystem();
-  }
+        drivetrain.registerTelemetry(logger::telemeterize);
+        Controller.driver.POVUp()
+                .whileTrue(
+                        drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
+        Controller.driver.POVDown()
+                .whileTrue(
+                        drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
+    }
 
-  public Command getAutonomousCommand() {
-    /* First put the drivetrain into auto run mode, then run the auto */
-    return runAuto;
-  }
+    public RobotContainer() {
+        configureBindings();
+        ledSubsystem = new LEDSubsystem();
+    }
 
-  public boolean seedPoseButtonDown() {
-    return driver.leftBumper().getAsBoolean();
-  }
+    public Command getAutonomousCommand() {
+        return runAuto;
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -9,9 +9,11 @@
 
 public class DriveConstants {
   /** Only touch these if you know what you're doing */
-  private static final SlotGains steerGains = new SlotGains(100, 0, 0.05, 0, 0);
+  private static final SlotGains steerGains = new SlotGains(100.0, 0, 0.05, 0, 0);
+  private static final SlotGains driveGains = new SlotGains(3.0, 0, 0, 0, 0);
 
-  private static final SlotGains driveGains = new SlotGains(3, 0, 0, 0, 0);
+  public static final double kMaxSpeed = 6.0; // 6 meters per second desired top speed
+  public static final double kMaxAngularRate = Math.PI; // Half a rotation per second max angular velocity
 
   private static final double kCoupleRatio = 3.5;
 
@@ -23,6 +25,7 @@ public class DriveConstants {
   private static final int kPigeonId = 1;
 
   public static final double kSpeedAt12VoltsMps = 5.0;
+  public static final double kSimLoopPeriod = 0.005; // 5 ms
 
   private static final boolean kSteerMotorReversed = true;
   public static final boolean kInvertLeftSide = false;

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -23,7 +23,6 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
 public class DriveSubsystem extends SwerveDrivetrain implements Subsystem {
-  private static final double kSimLoopPeriod = 0.005; // 5 ms
   private Notifier m_simNotifier = null;
   private double m_lastSimTime;
 
@@ -104,7 +103,7 @@ private void startSimThread() {
               /* use the measured time delta, get battery voltage from WPILib */
               updateSimState(deltaTime, RobotController.getBatteryVoltage());
             });
-    m_simNotifier.startPeriodic(kSimLoopPeriod);
+    m_simNotifier.startPeriodic(DriveConstants.kSimLoopPeriod);
   }
 
   public void initialize() {