cleanup drive code

MiamiBeachBots · Jan 13, 2024 · 5f3bbcd · 5f3bbcd
1 parent 73b468e
commit 5f3bbcd
Show file tree

Hide file tree

Showing 2 changed files with 65 additions and 48 deletions.
diff --git a/src/main/java/frc/robot/DriveConstants.java b/src/main/java/frc/robot/DriveConstants.java
@@ -32,6 +32,9 @@ public final class DriveConstants {
   // values for your robot.
   // Feed Forward Constants
   public static final double kDriveFeedForward = 0.0;
+  // Max speed Constants
+  public static final double kMaxOutputDrive = 1.0;
+  public static final double kMinOutputDrive = -1.0;
   // Feed Back / PID Constants
   public static final double kPDriveVel = 3.6293;
   public static final double kIDriveVel = 0.0;

diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -117,7 +117,8 @@ public DriveSubsystem() {
     // init drive function
     m_ddrive = new DifferentialDrive(m_backLeft, m_backRight);
 
-    // init Encoders
+    // init Encoders, we use all 4 encoders even though only 2 are used in feedback to decrease
+    // error
     m_encoderBackLeft = m_backLeft.getEncoder();
     m_encoderFrontLeft = m_frontLeft.getEncoder();
     m_encoderBackRight = m_backRight.getEncoder();
@@ -145,6 +146,50 @@ public DriveSubsystem() {
     resetGyro();
 
     // setup PID controllers
+    configureMotorPIDControllers();
+
+    // Configure RIO PID Controllers
+    // config turn pid controller.
+    m_turnController.enableContinuousInput(-180.0f, 180.0f);
+    m_turnController.setTolerance(TurnToleranceDeg, TurnRateToleranceDegPerS);
+    // this is the target pitch/ tilt error.
+    m_balanceController.setGoal(0);
+    m_balanceController.setTolerance(BalanceToleranceDeg); // max error in degrees
+
+    // configure Odemetry
+    m_driveOdometry =
+        new DifferentialDrivePoseEstimator(
+            DriveConstants.kDriveKinematics,
+            getRotation2d(),
+            getPositionLeft(),
+            getPositionRight(),
+            new Pose2d());
+
+    // Setup Base AutoBuilder (Autonomous)
+    AutoBuilder.configureRamsete(
+        this::getPose, // Pose2d supplier
+        this::resetPose, // Pose2d consumer, used to reset odometry at the beginning of auto
+        this::getSpeeds, // A method for getting the chassis' current speed and direction
+        this::setSpeeds, // A consumer that takes the desired chassis speed and direction
+        DriveConstants.autoReplanningConfig,
+        () -> {
+          // Boolean supplier that controls when the path will be mirrored for the red alliance
+          // This will flip the path being followed to the red side of the field.
+          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+          var alliance = DriverStation.getAlliance();
+          if (alliance.isPresent()) {
+            return alliance.get() == DriverStation.Alliance.Red;
+          }
+          return false;
+        },
+        this // Reference to this subsystem to set requirements
+        );
+
+    SmartDashboard.putData("Field", field); // add field to dashboard
+  }
+
+  private void configureMotorPIDControllers() {
     // setup velocity PID controllers (used by auto)
     m_backLeftPIDController.setP(
         DriveConstants.kPDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
@@ -166,10 +211,14 @@ public DriveSubsystem() {
         DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainVelocityPIDSlot);
     m_backRightPIDController.setFF(
         DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainVelocityPIDSlot);
-    // m_backLeftPIDController.setOutputRange(kMinOutput, kMaxOutputVel,
-    // DriveConstants.kDrivetrainVelocityPIDSlot);
-    // m_backRightPIDController.setOutputRange(kMinOutput, kMaxOutputVel,
-    // DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backLeftPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backRightPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainVelocityPIDSlot);
 
     // setup position PID controllers (used when we manually path find)
     m_backLeftPIDController.setP(
@@ -192,49 +241,14 @@ public DriveSubsystem() {
         DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainPositionPIDSlot);
     m_backRightPIDController.setFF(
         DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainPositionPIDSlot);
-    // m_backLeftPIDController.setOutputRange(kMinOutput, kMaxOutputPos,
-    // DriveConstants.kDrivetrainPositionPIDSlot);
-    // m_backRightPIDController.setOutputRange(kMinOutput, kMaxOutputPos,
-    // DriveConstants.kDrivetrainPositionPIDSlot);
-
-    // configure Odemetry
-    m_driveOdometry =
-        new DifferentialDrivePoseEstimator(
-            DriveConstants.kDriveKinematics,
-            getRotation2d(),
-            getPositionLeft(),
-            getPositionRight(),
-            new Pose2d());
-
-    // config turn pid controller.
-    m_turnController.enableContinuousInput(-180.0f, 180.0f);
-    m_turnController.setTolerance(TurnToleranceDeg, TurnRateToleranceDegPerS);
-    // this is the target pitch/ tilt error.
-    m_balanceController.setGoal(0);
-    m_balanceController.setTolerance(BalanceToleranceDeg); // max error in degrees
-
-    // Setup Base AutoBuilder (Autonomous)
-    AutoBuilder.configureRamsete(
-        this::getPose, // Pose2d supplier
-        this::resetPose, // Pose2d consumer, used to reset odometry at the beginning of auto
-        this::getSpeeds, // A method for getting the chassis' current speed and direction
-        this::setSpeeds, // A consumer that takes the desired chassis speed and direction
-        DriveConstants.autoReplanningConfig,
-        () -> {
-          // Boolean supplier that controls when the path will be mirrored for the red alliance
-          // This will flip the path being followed to the red side of the field.
-          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-
-          var alliance = DriverStation.getAlliance();
-          if (alliance.isPresent()) {
-            return alliance.get() == DriverStation.Alliance.Red;
-          }
-          return false;
-        },
-        this // Reference to this subsystem to set requirements
-        );
-
-    SmartDashboard.putData("Field", field); // add field to dashboard
+    m_backLeftPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainPositionPIDSlot);
   }
 
   // default tank drive function