format

team581 · Sep 18, 2024 · ccb7040 · ccb7040
1 parent 6753bac
commit ccb7040
Show file tree

Hide file tree

Showing 5 changed files with 21 additions and 26 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -132,11 +132,16 @@ public void testPeriodic() {}
   public void testExit() {}
 
   private void configureBindings() {
-    swerve.setDefaultCommand(swerve.run(() -> {
-      if (DriverStation.isTeleop()) {
-        swerve.driveTeleop(hardware.driverController.getLeftX(),hardware.driverController.getLeftY() , hardware.driverController.getRightX());
-      }
-    }));
+    swerve.setDefaultCommand(
+        swerve.run(
+            () -> {
+              if (DriverStation.isTeleop()) {
+                swerve.driveTeleop(
+                    hardware.driverController.getLeftX(),
+                    hardware.driverController.getLeftY(),
+                    hardware.driverController.getRightX());
+              }
+            }));
 
     hardware
         .driverController

diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java
@@ -11,9 +11,8 @@
 import frc.robot.util.state_machines.StateMachine;
 import frc.robot.vision.VisionResult;
 import frc.robot.vision.VisionSubsystem;
-import java.util.Optional;
-import java.util.List;
 import java.util.ArrayList;
+import java.util.List;
 
 public class LocalizationSubsystem extends StateMachine<LocalizationState> {
   private final ImuSubsystem imu;
@@ -43,8 +42,7 @@ public Pose2d getPose() {
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
-for (int resultnum : latestResult.size())
-       results = latestResult.get(resultnum);
+    for (var results : latestResult) {
       Pose2d visionPose = results.pose();
 
       double visionTimestamp = results.timestamp();
@@ -60,9 +58,9 @@ public void robotPeriodic() {
                 RobotConfig.get().vision().xyStdDev(),
                 RobotConfig.get().vision().thetaStdDev()));
         lastAddedVisionTimestamp = visionTimestamp;
+      }
 
+      poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
     }
-
-    poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
   }
 }
diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.config.RobotConfig;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.util.ControllerHelpers;
@@ -120,14 +119,11 @@ public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {
   public void driveTeleop(double x, double y, double theta) {
     double leftY =
         -1.0
-            * ControllerHelpers.getExponent(
-                ControllerHelpers.getDeadbanded(x, leftYDeadband), 1.5);
+            * ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(x, leftYDeadband), 1.5);
     double leftX =
-        ControllerHelpers.getExponent(
-            ControllerHelpers.getDeadbanded(y, leftXDeadband), 1.5);
+        ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(y, leftXDeadband), 1.5);
     double rightX =
-        ControllerHelpers.getExponent(
-            ControllerHelpers.getDeadbanded(theta, rightXDeadband), 2);
+        ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(theta, rightXDeadband), 2);
 
     if (RobotConfig.get().swerve().invertRotation()) {
       rightX *= -1.0;

diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java
@@ -1,13 +1,11 @@
 package frc.robot.vision;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import java.util.Optional;
 
-public class Limelight{
+public class Limelight {
   private Optional<VisionResult> rawVisionResult;
   private Optional<VisionResult> processedVisionResult;
-public String limeLightName = "";
-
+  public String limeLightName = "";
 
   public Optional<VisionResult> getRawVisionResult() {
     var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limeLightName);

diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java
@@ -3,16 +3,15 @@
 import frc.robot.imu.ImuSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
-import java.util.List;
 import java.util.ArrayList;
+import java.util.List;
 
 public class VisionSubsystem extends StateMachine<VisionState> {
   private final ImuSubsystem imu;
   private final Limelight leftLimelight;
   private final Limelight rightLimelight;
   private final List<VisionResult> processedVisionResult = new ArrayList<>();
 
-
   public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight rightLimelight) {
     super(SubsystemPriority.VISION, VisionState.DEFAULT_STATE);
     this.imu = imu;
@@ -29,10 +28,9 @@ protected void collectInputs() {
     if (leftResult.isPresent()) {
       processedVisionResult.add(leftResult.get());
     }
-    if(rightResult.isPresent()) {
+    if (rightResult.isPresent()) {
       processedVisionResult.add(rightResult.get());
     }
-
   }
 
   public List<VisionResult> getVisionResult() {