spotless

Author: mwitcpalek

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

@@ -1,5 +1,5 @@
 package frc.robot.constants;
 
 public class RobotStateConstants {
-    public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
+  public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
 }

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

@@ -1,5 +1,5 @@
 package frc.robot.constants;
 
 public class TagServoingConstants {
-    public static final double kAngleCloseEnough = 3.0;
+  public static final double kAngleCloseEnough = 3.0;
 }

src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java

@@ -1,19 +1,16 @@
 package frc.robot.subsystems.pathHandler;
 
 import choreo.Choreo;
-
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.Timer;
-import frc.robot.Robot;
 import frc.robot.constants.RobotStateConstants;
 import frc.robot.constants.TagServoingConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
-import net.jafama.FastMath;
-
 import java.util.List;
 import java.util.Optional;
 import java.util.Set;
+import net.jafama.FastMath;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
@@ -133,13 +130,16 @@ private void drivePath() {
 
   private void drivePathServo() {
     if (isHandling && runningPath && currPath != null && isServoing) {
-      driveSubsystem.calculateControllerServo(currPath.sampleAt(timer.get(), mirrorTrajectory).get(), 0.0); // TODO: use tag servoing here when ready
+      driveSubsystem.calculateControllerServo(
+          currPath.sampleAt(timer.get(), mirrorTrajectory).get(),
+          0.0); // TODO: use tag servoing here when ready
       if (timer.hasElapsed(currPath.getTotalTime())) {
         driveSubsystem.setAutoDebugMsg("End " + currPathString);
         runningPath = false;
         timer.stop();
         timer.reset();
-        driveSubsystem.calculateControllerServo(currPath.getFinalSample(mirrorTrajectory).get(), 0.0);
+        driveSubsystem.calculateControllerServo(
+            currPath.getFinalSample(mirrorTrajectory).get(), 0.0);
         if (currState == PathStates.DRIVE_FETCH) {
           currState = PathStates.FETCH;
         } else if (currState == PathStates.DRIVE_PLACE_SERVO) {
@@ -186,17 +186,25 @@ public void killPathHandlerAfterPath() {
 
   private boolean shouldTransitionToServoing() {
     boolean isCloseEnough = false;
-    double preNormalizedAngle = FastMath.toRadians(RobotStateConstants.kNodeAngles[FastMath.floorToInt((NodeNames.get(0) - 'a') / 2)]);
-    double goal = mirrorTrajectory ? FastMath.normalizeMinusPiPi(preNormalizedAngle + Math.PI) : preNormalizedAngle;
+    double preNormalizedAngle =
+        FastMath.toRadians(
+            RobotStateConstants.kNodeAngles[FastMath.floorToInt((NodeNames.get(0) - 'a') / 2)]);
+    double goal =
+        mirrorTrajectory
+            ? FastMath.normalizeMinusPiPi(preNormalizedAngle + Math.PI)
+            : preNormalizedAngle;
     double pos = driveSubsystem.getGyroRotation2d().getRadians();
-    if (goal < -Math.PI/2 || goal > Math.PI/2) {
-      isCloseEnough = FastMath.abs(FastMath.normalizeZeroTwoPi(pos) - FastMath.normalizeZeroTwoPi(goal)) < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough);
+    if (goal < -Math.PI / 2 || goal > Math.PI / 2) {
+      isCloseEnough =
+          FastMath.abs(FastMath.normalizeZeroTwoPi(pos) - FastMath.normalizeZeroTwoPi(goal))
+              < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough);
     } else {
-      isCloseEnough = FastMath.abs(pos - goal) < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough);
+      isCloseEnough =
+          FastMath.abs(pos - goal) < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough);
     }
     return currState == PathStates.DRIVE_PLACE
-     && timer.hasElapsed(currPath.getTotalTime() - 1.0)
-     && isCloseEnough;
+        && timer.hasElapsed(currPath.getTotalTime() - 1.0)
+        && isCloseEnough;
   }
 
   public void periodic() {
@@ -209,8 +217,8 @@ public void periodic() {
         break;
       case FETCH:
         // if (robotStateSubsystem.hasPiece) {
-          advanceNodes();
-          currState = PathStates.DRIVE_PLACE;
+        advanceNodes();
+        currState = PathStates.DRIVE_PLACE;
         // }
         break;
       case DRIVE_PLACE:
@@ -226,7 +234,7 @@ public void periodic() {
         break;
       case PLACE:
         // if (RobotStateSubsystem.State != RobotStateSubsystem.hasPiece) {
-          currState = PathStates.DRIVE_FETCH;
+        currState = PathStates.DRIVE_FETCH;
         // }
         break;
       case DONE: