Merge remote-tracking branch 'origin/GearFox-SonarQubeCleanup' into G…

…earFox-SwerveRefactor

# Conflicts:
#	src/main/java/frc/robot/Constants.java
#	src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java
#	src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java
#	src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java
#	src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java

.github/CODEOWNERS

@@ -0,0 +1,2 @@
+## code changes will send PR to following users
+* @TitaniumTitans/Programming Team

src/main/java/frc/robot/Constants.java

@@ -24,9 +24,10 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  private Constants() {}
-
-  public static final Mode currentMode = Mode.SIM;
+  private Constants() {
+    throw new IllegalStateException("Constants class should not be constructed");
+  }
+  public static final Mode currentMode = Mode.REAL;
 
   public enum Mode {
     /** Running on a real robot. */
@@ -40,13 +41,18 @@ public enum Mode {
   }
 
   public static class DriveConstants {
-    private DriveConstants() {}
+    private DriveConstants() {
+      throw new IllegalStateException("Constants class should not be constructed");
+    }
 
     // module constants
     public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0);
 
+    // kV, kS, kA in order
     protected static final double[] DRIVE_FF_GAINS = new double[]{0.13, 0.1, 0.0};
+    // kP, kI, kD in order
     protected static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0};
+    // kP, kI, kD in order
     protected static final double[] TURN_FB_GAINS = new double[]{7.0, 0.0, 0.0};
 
     public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants(

src/main/java/frc/robot/Robot.java

@@ -44,15 +44,16 @@ public void robotInit() {
     Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
     Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
     Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    String gitDirty = "GitDirty";
     switch (BuildConstants.DIRTY) {
       case 0:
-        Logger.recordMetadata("GitDirty", "All changes committed");
+        Logger.recordMetadata(gitDirty, "All changes committed");
         break;
       case 1:
-        Logger.recordMetadata("GitDirty", "Uncomitted changes");
+        Logger.recordMetadata(gitDirty, "Uncomitted changes");
         break;
       default:
-        Logger.recordMetadata("GitDirty", "Unknown");
+        Logger.recordMetadata(gitDirty, "Unknown");
         break;
     }
 
@@ -102,11 +103,15 @@ public void robotPeriodic() {
 
   /** This function is called once when the robot is disabled. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 
   /** This function is called periodically when disabled. */
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
@@ -121,7 +126,9 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 
   /** This function is called once when teleop is enabled. */
   @Override
@@ -137,7 +144,9 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 
   /** This function is called once when test mode is enabled. */
   @Override
@@ -148,13 +157,19 @@ public void testInit() {
 
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 
   /** This function is called once when the robot is first started up. */
   @Override
-  public void simulationInit() {}
+  public void simulationInit() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    // Function is empty simply to overwrite the default, which throws an error
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.subsystems.drive.DriveSubsystem;
@@ -55,35 +56,41 @@ public class RobotContainer {
   public RobotContainer() {
     switch (Constants.currentMode) {
       case REAL -> {
-        // Real robot, instantiate hardware IO implementations
-        m_driveSubsystem = new DriveSubsystem(
-            new GyroIOPigeon1(12, true),
-            new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
-            new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
-            new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
-            new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
-        m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
-      }
-      case SIM -> {
-        // Sim robot, instantiate physics sim IO implementations
+          // Real robot, instantiate hardware IO implementations
+          m_driveSubsystem = new DriveSubsystem(
+            new GyroIOPigeon1(12),
+              new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
+              new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
+              new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
+              new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
+          m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
+        }
+    case SIM -> {
+      // Sim robot, instantiate physics sim IO implementations
         m_driveSubsystem =
-        new DriveSubsystem(
-          new GyroIO() {},
-          new ModuleIOSim(Constants.DriveConstants.FL_MOD_CONSTANTS),
-          new ModuleIOSim(Constants.DriveConstants.FR_MOD_CONSTANTS),
-          new ModuleIOSim(Constants.DriveConstants.BL_MOD_CONSTANTS),
-          new ModuleIOSim(Constants.DriveConstants.BR_MOD_CONSTANTS));
+          new DriveSubsystem(
+            new GyroIO() {
+            },
+            new ModuleIOSim(DriveConstants.FL_MOD_CONSTANTS),
+            new ModuleIOSim(DriveConstants.FR_MOD_CONSTANTS),
+            new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS),
+            new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS));
         m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
       }
-      default -> {
-        // Replayed robot, disable IO implementations
+    default -> {
+      // Replayed robot, disable IO implementations
         m_driveSubsystem =
-        new DriveSubsystem(
-          new GyroIO() {},
-          new ModuleIO() {},
-          new ModuleIO() {},
-          new ModuleIO() {},
-          new ModuleIO() {});
+                new DriveSubsystem(
+                        new GyroIO() {
+                        },
+                        new ModuleIO() {
+                        },
+                        new ModuleIO() {
+                        },
+                        new ModuleIO() {
+                        },
+                        new ModuleIO() {
+                        });
         m_shooter = new ShooterSubsystem(new ShooterIO() {});
       }
     }

src/main/java/frc/robot/commands/DriveCommands.java

@@ -63,7 +63,7 @@ public static Command joystickDrive(
                   linearVelocity.getX() * driveSubsystem.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * driveSubsystem.getMaxLinearSpeedMetersPerSec(),
                   omega * driveSubsystem.getMaxAngularSpeedRadPerSec(),
-                  new Rotation2d())); //drive.getRotation()));
+                  driveSubsystem.getRotation()));
         },
             driveSubsystem);
   }

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);
     }
   }
 }

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,13 +110,13 @@ 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
     int deltaCount =
-        gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE;
+        gyroInputs.isConnected() ? gyroInputs.getOdometryYawPositions().length : Integer.MAX_VALUE;
     for (int i = 0; i < 4; i++) {
       deltaCount = Math.min(deltaCount, modules[i].getPositionDeltas().length);
     }
@@ -133,10 +132,10 @@ public void periodic() {
       // sample in x, y, and theta based only on the modules, without
       // the gyro. The gyro is always disconnected in simulation.
       var twist = kinematics.toTwist2d(wheelDeltas);
-      if (gyroInputs.connected) {
+      if (gyroInputs.isConnected()) {
         // If the gyro is connected, replace the theta component of the twist
         // with the change in angle since the last sample.
-        Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex];
+        Rotation2d gyroRotation = gyroInputs.getOdometryYawPositions()[deltaIndex];
         twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians());
         lastGyroRotation = gyroRotation;
       }
@@ -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];

src/main/java/frc/robot/subsystems/drive/GyroIO.java

@@ -18,12 +18,44 @@
 
 public interface GyroIO {
   @AutoLog
-  public static class GyroIOInputs {
-    public boolean connected = false;
-    public Rotation2d yawPosition = new Rotation2d();
-    public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
-    public double yawVelocityRadPerSec = 0.0;
+  class GyroIOInputs {
+    protected boolean connected = false;
+    protected Rotation2d yawPosition = new Rotation2d();
+    protected Rotation2d[] odometryYawPositions = new Rotation2d[] {};
+    protected double yawVelocityRadPerSec = 0.0;
+
+    public boolean isConnected() {
+      return connected;
+    }
+
+    public void setConnected(boolean connected) {
+      this.connected = connected;
+    }
+
+    public Rotation2d getYawPosition() {
+      return yawPosition;
+    }
+
+    public void setYawPosition(Rotation2d yawPosition) {
+      this.yawPosition = yawPosition;
+    }
+
+    public Rotation2d[] getOdometryYawPositions() {
+      return odometryYawPositions;
+    }
+
+    public void setOdometryYawPositions(Rotation2d[] odometryYawPositions) {
+      this.odometryYawPositions = odometryYawPositions;
+    }
+
+    public double getYawVelocityRadPerSec() {
+      return yawVelocityRadPerSec;
+    }
+
+    public void setYawVelocityRadPerSec(double yawVelocityRadPerSec) {
+      this.yawVelocityRadPerSec = yawVelocityRadPerSec;
+    }
   }
 
-  public default void updateInputs(GyroIOInputs inputs) {}
+  default void updateInputs(GyroIOInputs inputs) {}
 }