sonarqube all arm files

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

@@ -12,23 +12,18 @@
 // GNU General Public License for more details.
 
 package frc.robot;
-import com.fasterxml.jackson.core.SerializableString;
-import edu.wpi.first.math.geometry.*;
+
 import com.gos.lib.properties.GosDoubleProperty;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.units.Unit;
 import frc.robot.subsystems.arm.ArmPose;
-import frc.robot.subsystems.arm.ArmSubsystem;
 import frc.robot.subsystems.drive.module.ModuleConstants;
 
 import java.util.List;

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

@@ -15,16 +15,9 @@
 
 import com.ctre.phoenix6.SignalLogger;
 import com.gos.lib.properties.PropertyManager;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.util.datalog.StringLogEntry;
-import edu.wpi.first.wpilibj.DataLogManager;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import lib.factories.TalonFXFactory;
-import lib.logger.DataLogUtil;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -54,11 +47,7 @@ public void robotInit() {
 
     String logPath = "/media/sda1/aoide";
 
-//    DataLogManager.start(logPath);
-//    DriverStation.startDataLog(DataLogManager.getLog());
-
     SignalLogger.setPath(logPath);
-//    SignalLogger.start();
 
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
@@ -85,13 +74,11 @@ public void robotInit() {
     }
 
     switch (Constants.currentMode) {
-      case REAL -> {
+      case REAL, default -> {
         Logger.addDataReceiver(new WPILOGWriter(logPath)); // Log to a USB stick ("/U/logs")
         Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
       }
-      case SIM -> {
-        Logger.addDataReceiver(new NT4Publisher());
-      }
+      case SIM -> Logger.addDataReceiver(new NT4Publisher());
       case REPLAY -> {
         setUseTiming(false); // Run as fast as possible
         String replayLog = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)

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

@@ -96,7 +96,7 @@ public RobotContainer() {
         m_climber = new ClimberSubsystem(new ClimberIOKraken() {});
       }
       case SIM -> {
-//       Sim robot, instantiate physics sim IO implementations
+      // Sim robot, instantiate physics sim IO implementations
         m_driveSubsystem =
             new DriveSubsystem(
                 new GyroIO() {},

src/main/java/frc/robot/subsystems/arm/ArmIO.java

@@ -6,23 +6,119 @@
 public interface ArmIO {
   @AutoLog
   class ArmIOInputs {
-    public double armPositionDegs = 0.0;
-    public double wristPositionDegs = 0.0;
+    protected double armPositionDegs = 0.0;
+    protected double wristPositionDegs = 0.0;
 
-    public double armVelocityDegsPerSecond = 0.0;
-    public double wristVelocityDegsPerSecond = 0.0;
+    protected double armVelocityDegsPerSecond = 0.0;
+    protected double wristVelocityDegsPerSecond = 0.0;
 
-    public double armAppliedOutput = 0.0;
-    public double wristAppliedOutput = 0.0;
+    protected double armAppliedOutput = 0.0;
+    protected double wristAppliedOutput = 0.0;
 
-    public double armClosedLoopOutput = 0.0;
-    public double wristClosedLoopOutput = 0.0;
+    protected double armClosedLoopOutput = 0.0;
+    protected double wristClosedLoopOutput = 0.0;
 
-    public double armDesiredSetpoint = 0.0;
-    public double wristDesiredSetpoint = 0.0;
+    protected double armDesiredSetpoint = 0.0;
+    protected double wristDesiredSetpoint = 0.0;
 
-    public double armCurrentDraw = 0.0;
-    public double wristCurrentDraw = 0.0;
+    protected double armCurrentDraw = 0.0;
+    protected double wristCurrentDraw = 0.0;
+
+    public double getArmPositionDegs() {
+      return armPositionDegs;
+    }
+
+    public void setArmPositionDegs(double armPositionDegs) {
+      this.armPositionDegs = armPositionDegs;
+    }
+
+    public double getWristPositionDegs() {
+      return wristPositionDegs;
+    }
+
+    public void setWristPositionDegs(double wristPositionDegs) {
+      this.wristPositionDegs = wristPositionDegs;
+    }
+
+    public double getArmVelocityDegsPerSecond() {
+      return armVelocityDegsPerSecond;
+    }
+
+    public void setArmVelocityDegsPerSecond(double armVelocityDegsPerSecond) {
+      this.armVelocityDegsPerSecond = armVelocityDegsPerSecond;
+    }
+
+    public double getWristVelocityDegsPerSecond() {
+      return wristVelocityDegsPerSecond;
+    }
+
+    public void setWristVelocityDegsPerSecond(double wristVelocityDegsPerSecond) {
+      this.wristVelocityDegsPerSecond = wristVelocityDegsPerSecond;
+    }
+
+    public double getArmAppliedOutput() {
+      return armAppliedOutput;
+    }
+
+    public void setArmAppliedOutput(double armAppliedOutput) {
+      this.armAppliedOutput = armAppliedOutput;
+    }
+
+    public double getWristAppliedOutput() {
+      return wristAppliedOutput;
+    }
+
+    public void setWristAppliedOutput(double wristAppliedOutput) {
+      this.wristAppliedOutput = wristAppliedOutput;
+    }
+
+    public double getArmClosedLoopOutput() {
+      return armClosedLoopOutput;
+    }
+
+    public void setArmClosedLoopOutput(double armClosedLoopOutput) {
+      this.armClosedLoopOutput = armClosedLoopOutput;
+    }
+
+    public double getWristClosedLoopOutput() {
+      return wristClosedLoopOutput;
+    }
+
+    public void setWristClosedLoopOutput(double wristClosedLoopOutput) {
+      this.wristClosedLoopOutput = wristClosedLoopOutput;
+    }
+
+    public double getArmDesiredSetpoint() {
+      return armDesiredSetpoint;
+    }
+
+    public void setArmDesiredSetpoint(double armDesiredSetpoint) {
+      this.armDesiredSetpoint = armDesiredSetpoint;
+    }
+
+    public double getWristDesiredSetpoint() {
+      return wristDesiredSetpoint;
+    }
+
+    public void setWristDesiredSetpoint(double wristDesiredSetpoint) {
+      this.wristDesiredSetpoint = wristDesiredSetpoint;
+    }
+
+    public double getArmCurrentDraw() {
+      return armCurrentDraw;
+    }
+
+    public void setArmCurrentDraw(double armCurrentDraw) {
+      this.armCurrentDraw = armCurrentDraw;
+    }
+
+    public double getWristCurrentDraw() {
+      return wristCurrentDraw;
+    }
+
+    public void setWristCurrentDraw(double wristCurrentDraw) {
+      this.wristCurrentDraw = wristCurrentDraw;
+    }
   }
 
   default void updateInputs(ArmIOInputsAutoLogged inputs) {}

src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java

@@ -3,24 +3,21 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.*;
+import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.*;
 import com.gos.lib.properties.GosDoubleProperty;
 import com.gos.lib.properties.HeavyDoubleProperty;
-
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
+import frc.robot.Constants.ArmConstants;
 import lib.factories.TalonFXFactory;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
-
-import frc.robot.Constants.ArmConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class ArmIOKraken implements ArmIO {

src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java

@@ -2,7 +2,6 @@
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.GravityTypeValue;
 import edu.wpi.first.math.util.Units;
@@ -17,9 +16,6 @@ public class ArmIOPrototype implements ArmIO {
   private final PidPropertyPublic m_shoulderPID;
   private final PidPropertyPublic m_wristPID;
 
-  private final PositionVoltage m_wristReqPID;
-  private final PositionVoltage m_shoulderReqPID;
-
   private final MotionMagicVoltage m_wristReqMM;
   private final MotionMagicVoltage m_shoulderReqMM;
 
@@ -73,8 +69,6 @@ public ArmIOPrototype() {
         .addKG(ArmConstants.WRIST_KG, GravityTypeValue.Arm_Cosine)
         .build();
 
-    m_shoulderReqPID = new PositionVoltage(0).withSlot(0);
-    m_wristReqPID = new PositionVoltage(0).withSlot(0);
     m_shoulderReqMM = new MotionMagicVoltage(0).withSlot(0);
     m_wristReqMM = new MotionMagicVoltage(0).withSlot(0);
   }
@@ -120,6 +114,6 @@ public void setWristVoltage(double voltage){
 
   @Override
   public void setWristAngle(double degrees, double velocity) {
-      m_wrist.setControl(m_wristReqMM.withPosition(degrees / 360.0));
+    m_wrist.setControl(m_wristReqMM.withPosition(degrees / 360.0));
   }
 }

src/main/java/frc/robot/subsystems/arm/ArmIOSim.java

@@ -61,7 +61,8 @@ public void setWristVoltage(double voltage) {
   @Override
   public void setWristAngle(double degrees, double velocity) {
     double setpointRots = Units.degreesToRotations(degrees);
-    m_wristAppliedOutput = m_wristController.calculate(Units.radiansToRotations(m_wristSim.getAngleRads()), setpointRots);
+    m_wristAppliedOutput =
+        m_wristController.calculate(Units.radiansToRotations(m_wristSim.getAngleRads()), setpointRots);
     m_wristAppliedOutput = MathUtil.clamp(m_wristAppliedOutput, -12.0, 12.0);
     m_wristSim.setInputVoltage(m_wristAppliedOutput);
   }

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -2,20 +2,15 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.ArmSetpoints;
-import org.littletonrobotics.junction.Logger;
 import lib.utils.AimbotUtils;
-import lib.utils.AllianceFlipUtil;
-import lib.utils.FieldConstants;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.function.Supplier;
 
@@ -70,7 +65,6 @@ public ArmSubsystem(ArmIO io, Supplier<Pose2d> supplier) {
 
     m_poseSupplier = supplier;
 
-//    setupLogging();
     m_poseVisualizer = new ArmVisualizer("Current Arm Pose", Color.kFirstBlue);
     m_setpointVisualizer = new ArmVisualizer("Current Arm Setpoint", Color.kFirstRed);
   }
@@ -142,8 +136,6 @@ public void handleState() {
         m_armVelocityMult = 1.0;
         m_wristVelocityMult = 1.0;
 
-        Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d());
-        Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d();
         double groundDistance = Units.metersToInches(AimbotUtils.getDistanceFromSpeaker(m_poseSupplier.get()));
 
         m_desiredWristPoseDegs = AimbotUtils.getWristAngle(groundDistance);

src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java

@@ -1,6 +1,5 @@
 package frc.robot.subsystems.arm;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -11,7 +10,6 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.Constants.ArmConstants;
-import lib.logger.DataLogUtil;
 import lib.utils.AimbotUtils;
 import org.littletonrobotics.junction.Logger;