Refactor Drive periodic logic and improve formatting

TNilCode · TNilCode · commit 3f9593e55c31 · 2025-11-17T02:27:19.000-06:00
Refactored Drive.java to split periodic logic into smaller functions for sensor updates, controller updates, and desired speed computation. Improved code formatting and consistency across vision and drive subsystems. Minor cleanups and formatting changes in GoalPoseChooser, Vision, VisionConstants, and CameraIOPV for readability.
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
@@ -29,4 +29,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -17,7 +17,6 @@
 import frc.robot.systems.vision.Vision;
 import frc.robot.systems.vision.VisionConstants;
 import frc.robot.systems.vision.VisionConstants.Orientation;
-
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
@@ -39,8 +38,12 @@ public RobotContainer() {
                         },
                         new GyroIOPigeon2(),
                         new Vision(new CameraIO[] {
-                            new CameraIOPV(VisionConstants.kRightCamName, VisionConstants.kRightCamTransform, Orientation.BACK),
-                            new CameraIOPV(VisionConstants.kLeftCamName, VisionConstants.kLeftCamTransform, Orientation.BACK)
+                            new CameraIOPV(
+                                    VisionConstants.kRightCamName,
+                                    VisionConstants.kRightCamTransform,
+                                    Orientation.BACK),
+                            new CameraIOPV(
+                                    VisionConstants.kLeftCamName, VisionConstants.kLeftCamTransform, Orientation.BACK)
                         }));
                 break;
 
@@ -54,8 +57,12 @@ public RobotContainer() {
                         },
                         new GyroIO() {},
                         new Vision(new CameraIO[] {
-                            new CameraIOPV(VisionConstants.kRightCamName, VisionConstants.kRightCamTransform, Orientation.BACK),
-                            new CameraIOPV(VisionConstants.kLeftCamName, VisionConstants.kLeftCamTransform, Orientation.BACK)
+                            new CameraIOPV(
+                                    VisionConstants.kRightCamName,
+                                    VisionConstants.kRightCamTransform,
+                                    Orientation.BACK),
+                            new CameraIOPV(
+                                    VisionConstants.kLeftCamName, VisionConstants.kLeftCamTransform, Orientation.BACK)
                         }));
                 break;
 
diff --git a/src/main/java/frc/robot/systems/drive/Drive.java b/src/main/java/frc/robot/systems/drive/Drive.java
@@ -230,22 +230,7 @@ public Command customFollowPathComamnd(PathPlannerPath path, PPHolonomicDriveCon
                 this);
     }
 
-    /*
-     * If you broke it into functions: updateData -> setChassisSpeeds(data, state) -> runSwerve(speeds)
-     * Periodic Logic:
-     * Data Collection(Modules, gyro, Vision, odometry)
-     * Update Controllers use
-     * Setting Chassis Speeds based on driveState
-     * Teleop sets based of joystick, Autons sets based on pathplanner(ppDesiredSpeeds), etc. etc.
-     * runSwerve() function sets the desired state to modules
-     * State is set by setDriveState() and related commands which also resets the controllers
-     */
-    @Override
-    public void periodic() {
-        ///////////////////// DATA COLLECTION AND UPDATE CONTROLLERS AND ODOMETRY \\\\\\\\\\\\\\\\\\
-        /* Modules */
-        for (Module module : modules) module.periodic();
-
+    private void updateSensorsAndOdometry() {
         /* GYRO */
         gyro.updateInputs(gyroInputs);
         Logger.processInputs("Drive/Gyro", gyroInputs);
@@ -265,31 +250,24 @@ public void periodic() {
             if (observation.hasObserved())
                 poseEstimator.addVisionMeasurement(observation.pose(), observation.timeStamp(), observation.stdDevs());
 
-            Logger.recordOutput(
-                    observation.camName() + "/stdDevX", observation.stdDevs().get(0));
-            Logger.recordOutput(
-                    observation.camName() + "/stdDevY", observation.stdDevs().get(1));
-            Logger.recordOutput(
-                    observation.camName() + "/stdDevTheta",
-                    observation.stdDevs().get(2));
-            // Logger.recordOutput(observation.camName()+"/TransformFromOdometry",
-            // odometry.getPoseMeters().minus(observation.pose()));
+            Logger.recordOutput(observation.camName() + "/stdDevX", observation.stdDevs().get(0));
+            Logger.recordOutput(observation.camName() + "/stdDevY", observation.stdDevs().get(1));
+            Logger.recordOutput(observation.camName() + "/stdDevTheta", observation.stdDevs().get(2));
         }
 
         poseEstimator.update(robotRotation, getModulePositions());
         odometry.update(robotRotation, getModulePositions());
 
         field.setRobotPose(getPoseEstimate());
+    }
 
-        // Logger.recordOutput("Drive/Odometry/FieldCurrentChassisSpeeds",
-        // ChassisSpeeds.fromRobotRelativeSpeeds(getRobotChassisSpeeds(), robotRotation));
-
-        /* Updating Controllers */
+    private void updateDriveControllers() {
         headingController.updateHeadingController();
         autoAlignController.updateAlignmentControllers();
         GoalPoseChooser.updateSideStuff();
+    }
 
-        ///////////////////// SETTING DESIRED SPEEDS FROM DRIVE STATE \\\\\\\\\\\\\\\\\\
+    private void computeDesiredSpeeds() {
         ChassisSpeeds teleopSpeeds =
                 teleopController.computeChassiSpeeds(getPoseEstimate().getRotation(), getRobotChassisSpeeds(), false);
         switch (driveState) {
@@ -312,8 +290,7 @@ public void periodic() {
                         headingController.getSnapOutput(getPoseEstimate().getRotation()));
                 break;
             case INTAKE_HEADING_ALIGN:
-                goalRotation = AllianceFlipUtil.apply(
-                        GoalPoseChooser.getIntakePose(getPoseEstimate()).getRotation());
+                goalRotation = AllianceFlipUtil.apply(GoalPoseChooser.getIntakePose(getPoseEstimate()).getRotation());
                 desiredSpeeds = new ChassisSpeeds(
                         teleopSpeeds.vxMetersPerSecond,
                         teleopSpeeds.vyMetersPerSecond,
@@ -327,8 +304,6 @@ public void periodic() {
                         headingController.getSnapOutput(getPoseEstimate().getRotation()));
                 break;
             case DRIVE_TO_CORAL:
-                desiredSpeeds = autoAlignController.calculate(goalPose, getPoseEstimate());
-                break;
             case DRIVE_TO_INTAKE:
                 desiredSpeeds = autoAlignController.calculate(goalPose, getPoseEstimate());
                 break;
@@ -343,9 +318,10 @@ public void periodic() {
             case DRIVE_TO_ALGAE:
                 // desiredSpeeds = autoAlignController.calculate(goalPose, getPoseEstimate());
                 ChassisSpeeds algaeAlignSpeeds = autoAlignController.calculate(goalPose, getPoseEstimate());
-                double forwardJoy = (goalPose.getX() > AllianceFlipUtil.apply(frc.robot.game.FieldConstants.kReefCenter.getX()))
-                        ? -teleopSpeeds.vxMetersPerSecond
-                        : teleopSpeeds.vxMetersPerSecond;
+                double forwardJoy =
+                        (goalPose.getX() > AllianceFlipUtil.apply(frc.robot.game.FieldConstants.kReefCenter.getX()))
+                                ? -teleopSpeeds.vxMetersPerSecond
+                                : teleopSpeeds.vxMetersPerSecond;
                 if (AllianceFlipUtil.shouldFlip()) forwardJoy *= -1;
                 desiredSpeeds = new ChassisSpeeds(
                         /* Flips speed to preserve field relative. Not best solution, but probably good enough? */
@@ -389,8 +365,16 @@ public void periodic() {
             default:
                 /* Defaults to Teleop control if no other cases are run*/
         }
+    }
 
-        ///////////////////////// SETS DESIRED CHASSIS SPEED TO MODULES \\\\\\\\\\\\\\\\\\\\\\\\
+    @Override
+    public void periodic() {
+        for (Module module : modules) module.periodic();
+
+        updateSensorsAndOdometry();
+        updateDriveControllers();
+        computeDesiredSpeeds();
+        
         if (desiredSpeeds != null) runSwerve(desiredSpeeds);
     }
 
@@ -409,8 +393,6 @@ public void setDriveState(DriveState state) {
         driveState = state;
         switch (driveState) {
             case PROCESSOR_HEADING_ALIGN:
-                headingController.reset(getPoseEstimate().getRotation(), gyroInputs.yawVelocityPS);
-                break;
             case REEF_HEADING_ALIGN:
                 headingController.reset(getPoseEstimate().getRotation(), gyroInputs.yawVelocityPS);
                 break;
diff --git a/src/main/java/frc/robot/systems/drive/controllers/GoalPoseChooser.java b/src/main/java/frc/robot/systems/drive/controllers/GoalPoseChooser.java
@@ -151,8 +151,10 @@ public static Pose2d getTargetPose(int pTagID) {
     }
 
     public static Pose2d getTargetPose(int pTagID, double pXOffsetM, double pYOffsetM) {
-        Pose2d tagPose =
-                FieldConstants.kFieldLayout.getTagPose(pTagID).map(Pose3d::toPose2d).orElse(null);
+        Pose2d tagPose = FieldConstants.kFieldLayout
+                .getTagPose(pTagID)
+                .map(Pose3d::toPose2d)
+                .orElse(null);
 
         Translation2d tagTranslation = tagPose.getTranslation()
                 .plus(new Translation2d(
diff --git a/src/main/java/frc/robot/systems/vision/CameraIOPV.java b/src/main/java/frc/robot/systems/vision/CameraIOPV.java
@@ -1,3 +1,5 @@
+// REBELLION 10014
+
 package frc.robot.systems.vision;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -12,7 +14,6 @@
 import frc.robot.game.FieldConstants;
 import frc.robot.systems.vision.VisionConstants.CameraSimConfigs;
 import frc.robot.systems.vision.VisionConstants.Orientation;
-
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
@@ -40,32 +41,28 @@ public CameraIOPV(String pName, Transform3d pCameraTransform, Orientation pOrien
         this.mPhotonCam = new PhotonCamera(mCamName);
         this.mCameraTransform = pCameraTransform;
         this.mOrientation = pOrientation;
-        
+
         PhotonCamera.setVersionCheckEnabled(false); // Avoid version spam
 
         mPoseEstimator = new PhotonPoseEstimator(
-            FieldConstants.kFieldLayout,
-            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-            pCameraTransform
-        );
+                FieldConstants.kFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, pCameraTransform);
 
         mPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.CLOSEST_TO_LAST_POSE);
 
         if (Constants.currentMode == Mode.SIM) {
             setupSimulation();
         }
     }
-    
+
     private void setupSimulation() {
         mVisionSim = new VisionSystemSim("main");
         mVisionSim.addAprilTags(FieldConstants.kFieldLayout);
 
         SimCameraProperties cameraProps = new SimCameraProperties();
         cameraProps.setCalibration(
-            (int) CameraSimConfigs.resWidth.value, 
-            (int) CameraSimConfigs.resHeight.value, 
-            new Rotation2d(CameraSimConfigs.fovDeg.value)
-        );
+                (int) CameraSimConfigs.resWidth.value,
+                (int) CameraSimConfigs.resHeight.value,
+                new Rotation2d(CameraSimConfigs.fovDeg.value));
         cameraProps.setCalibError(CameraSimConfigs.avgErrorPx.value, CameraSimConfigs.errorStdDevPx.value);
         cameraProps.setFPS(CameraSimConfigs.fps.value);
         cameraProps.setAvgLatencyMs(CameraSimConfigs.avgLatencyMs.value);
@@ -75,7 +72,6 @@ private void setupSimulation() {
         mVisionSim.addCamera(mCameraSim, mCameraTransform);
     }
 
-
     @Override
     public void updateInputs(CameraIOInputs pInputs, Pose2d pLastRobotPose, Pose2d pSimOdomPose) {
         pInputs.iCamName = mCamName;
@@ -106,7 +102,8 @@ public void updateInputs(CameraIOInputs pInputs, Pose2d pLastRobotPose, Pose2d p
             pInputs.iIsConnected = mPhotonCam.isConnected();
 
             if (latestValidResult == null || !latestValidResult.hasTargets()) {
-                DriverStation.reportWarning("No valid pose found in unread PhotonVision results for " + mCamName, false);
+                DriverStation.reportWarning(
+                        "No valid pose found in unread PhotonVision results for " + mCamName, false);
                 pInputs.iHasTarget = false;
                 return;
             }
@@ -125,10 +122,10 @@ public void updateInputs(CameraIOInputs pInputs, Pose2d pLastRobotPose, Pose2d p
             pInputs.iLatestTimestamp = latestValidResult.getTimestampSeconds();
 
             latestEstimatedRobotPose.ifPresent(est -> {
-                if (mOrientation == Orientation.BACK) 
-                    pInputs.iLatestEstimatedRobotPose = est.estimatedPose.transformBy(new Transform3d(new Translation3d(), new Rotation3d(0.0, 0.0, Math.PI)));
-                else 
-                    pInputs.iLatestEstimatedRobotPose = est.estimatedPose;
+                if (mOrientation == Orientation.BACK)
+                    pInputs.iLatestEstimatedRobotPose = est.estimatedPose.transformBy(
+                            new Transform3d(new Translation3d(), new Rotation3d(0.0, 0.0, Math.PI)));
+                else pInputs.iLatestEstimatedRobotPose = est.estimatedPose;
 
                 int count = est.targetsUsed.size();
                 Transform3d[] tagTransforms = new Transform3d[count];
@@ -137,13 +134,13 @@ public void updateInputs(CameraIOInputs pInputs, Pose2d pLastRobotPose, Pose2d p
                 for (int i = 0; i < count; i++) {
                     tagTransforms[i] = est.targetsUsed.get(i).getBestCameraToTarget();
                     ambiguities[i] = est.targetsUsed.get(i).getPoseAmbiguity();
-                }            
+                }
 
                 pInputs.iNumberOfTargets = count;
                 pInputs.iLatestTagTransforms = tagTransforms;
                 pInputs.iLatestTagAmbiguities = ambiguities;
             });
-            
+
         } catch (Exception e) {
             e.printStackTrace();
             resetInputs(pInputs);
diff --git a/src/main/java/frc/robot/systems/vision/Vision.java b/src/main/java/frc/robot/systems/vision/Vision.java
diff --git a/src/main/java/frc/robot/systems/vision/VisionConstants.java b/src/main/java/frc/robot/systems/vision/VisionConstants.java
