another mass update of files

TitaniumTitans · Jan 20, 2024 · 416d81d · 416d81d
1 parent a6e9352
commit 416d81d
Show file tree

Hide file tree

Showing 5 changed files with 42 additions and 43 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -57,7 +57,7 @@ public RobotContainer() {
       case REAL -> {
           // Real robot, instantiate hardware IO implementations
           m_driveSubsystem = new DriveSubsystem(
-            new GyroIOPigeon1(12, true),
+            new GyroIOPigeon1(12),
               new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
               new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
               new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),

diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java
@@ -13,6 +13,7 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
@@ -86,7 +87,7 @@ public void add(double velocity, double voltage) {
     }
 
     public void print() {
-      if (velocityData.size() == 0 || voltageData.size() == 0) {
+      if (velocityData.isEmpty() || voltageData.isEmpty()) {
         return;
       }
 
@@ -96,11 +97,11 @@ public void print() {
               voltageData.stream().mapToDouble(Double::doubleValue).toArray(),
               1);
 
-      System.out.println("FF Characterization Results:");
-      System.out.println("\tCount=" + Integer.toString(velocityData.size()) + "");
-      System.out.println(String.format("\tR2=%.5f", regression.R2()));
-      System.out.println(String.format("\tkS=%.5f", regression.beta(0)));
-      System.out.println(String.format("\tkV=%.5f", regression.beta(1)));
+      DriverStation.reportWarning("FF Characterization Results:", false);
+      DriverStation.reportWarning("\tCount=" + Integer.toString(velocityData.size()) + "", false);
+      DriverStation.reportWarning(String.format("\tR2=%.5f", regression.R2()), false);
+      DriverStation.reportWarning(String.format("\tkS=%.5f", regression.beta(0)), false);
+      DriverStation.reportWarning(String.format("\tkV=%.5f", regression.beta(1)), false);
     }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -33,6 +33,8 @@
 import frc.robot.subsystems.drive.module.Module;
 import frc.robot.subsystems.drive.module.ModuleIO;
 import frc.robot.util.LocalADStarAK;
+
+import java.util.List;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -51,7 +53,7 @@ public class DriveSubsystem extends SubsystemBase {
   private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
   private final Module[] modules = new Module[4]; // FL, FR, BL, BR
 
-  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
+  private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
   private Pose2d pose = new Pose2d();
   private Rotation2d lastGyroRotation = new Rotation2d();
 
@@ -81,16 +83,13 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
         this);
     Pathfinding.setPathfinder(new LocalADStarAK());
     PathPlannerLogging.setLogActivePathCallback(
-        (activePath) -> {
-          Logger.recordOutput(
-              "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
-        });
+        (List<Pose2d> activePath) -> Logger.recordOutput(
+            "Odometry/Trajectory", activePath.toArray(new Pose2d[0])));
     PathPlannerLogging.setLogTargetPoseCallback(
-        (targetPose) -> {
-          Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
-        });
+        (Pose2d targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
   }
 
+  @Override
   public void periodic() {
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
@@ -111,8 +110,8 @@ public void periodic() {
     }
     // Log empty setpoint states when disabled
     if (DriverStation.isDisabled()) {
-      Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
-      Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+      Logger.recordOutput("SwerveStates/Setpoints");
+      Logger.recordOutput("SwerveStates/SetpointsOptimized");
     }
 
     // Update odometry
@@ -203,7 +202,7 @@ public double getCharacterizationVelocity() {
     return driveVelocityAverage / 4.0;
   }
 
-  /** Returns the module states (turn angles and drive velocities) for all of the modules. */
+  /** Returns the module states (turn angles and drive velocities) for all the modules. */
   @AutoLogOutput(key = "SwerveStates/Measured")
   private SwerveModuleState[] getModuleStates() {
     SwerveModuleState[] states = new SwerveModuleState[4];

diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -18,12 +18,12 @@
 
 public interface GyroIO {
   @AutoLog
-  public static class GyroIOInputs {
+  class GyroIOInputs {
     public boolean connected = false;
     public Rotation2d yawPosition = new Rotation2d();
     public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
     public double yawVelocityRadPerSec = 0.0;
   }
 
-  public default void updateInputs(GyroIOInputs inputs) {}
+  default void updateInputs(GyroIOInputs inputs) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java
@@ -3,35 +3,34 @@
 import com.ctre.phoenix.sensors.PigeonIMU;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
-import frc.robot.subsystems.drive.module.PhoenixOdometryThread;
 import frc.robot.subsystems.drive.module.SparkMaxOdometryThread;
 
 import java.util.Queue;
 
 public class GyroIOPigeon1 implements GyroIO {
-    private final PigeonIMU m_pigeon;
-    private double prevYaw;
-    private Queue<Double> yawPositionQueue;
+  private final PigeonIMU m_pigeon;
+  private double prevYaw;
+  private Queue<Double> yawPositionQueue;
 
-    public GyroIOPigeon1(int id, boolean pheonixDrive) {
-        m_pigeon = new PigeonIMU(id);
-        m_pigeon.configFactoryDefault();
-        m_pigeon.setYaw(0.0);
-        yawPositionQueue =
-                SparkMaxOdometryThread.getInstance().registerSignal(m_pigeon::getYaw);
-    }
+  public GyroIOPigeon1(int id) {
+    m_pigeon = new PigeonIMU(id);
+    m_pigeon.configFactoryDefault();
+    m_pigeon.setYaw(0.0);
+    yawPositionQueue =
+            SparkMaxOdometryThread.getInstance().registerSignal(m_pigeon::getYaw);
+  }
 
-    @Override
-    public void updateInputs(GyroIOInputs inputs) {
-        inputs.connected = m_pigeon.getState() == PigeonIMU.PigeonState.Ready;
-        inputs.yawPosition = Rotation2d.fromDegrees(m_pigeon.getYaw());
-        inputs.yawVelocityRadPerSec = (m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20);
-        prevYaw = m_pigeon.getYaw();
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = m_pigeon.getState() == PigeonIMU.PigeonState.Ready;
+    inputs.yawPosition = Rotation2d.fromDegrees(m_pigeon.getYaw());
+    inputs.yawVelocityRadPerSec = (m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20);
+    prevYaw = m_pigeon.getYaw();
 
-        inputs.odometryYawPositions =
-                yawPositionQueue.stream()
-                        .map(Rotation2d::fromDegrees)
-                        .toArray(Rotation2d[]::new);
-        yawPositionQueue.clear();
-    }
+    inputs.odometryYawPositions =
+        yawPositionQueue.stream()
+                      .map(Rotation2d::fromDegrees)
+                      .toArray(Rotation2d[]::new);
+    yawPositionQueue.clear();
+  }
 }