From 5ed48daca593d9e09d9873d48093148033ed74e0 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 17 Mar 2024 12:57:05 -0400 Subject: [PATCH] sonarqube all arm files --- src/main/java/frc/robot/Constants.java | 9 +- src/main/java/frc/robot/Robot.java | 17 +-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/arm/ArmIO.java | 120 ++++++++++++++++-- .../frc/robot/subsystems/arm/ArmIOKraken.java | 11 +- .../robot/subsystems/arm/ArmIOPrototype.java | 8 +- .../frc/robot/subsystems/arm/ArmIOSim.java | 3 +- .../robot/subsystems/arm/ArmSubsystem.java | 10 +- .../robot/subsystems/arm/ArmVisualizer.java | 2 - 9 files changed, 121 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 892d22ac..4b2f7a7f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 05bef1f0..d42c02a7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fd73f138..8bb88821 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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() {}, diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 9944abc6..0be772d9 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/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) {} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index e465eddb..6b0be85d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/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 { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java index 8b6ede62..43ce0a22 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java +++ b/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)); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 6cc61548..dec31564 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/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); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 3a4bb33e..9e8ca5c8 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -2,9 +2,6 @@ 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; @@ -12,10 +9,8 @@ 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 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); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java index 906baf6c..dedcec25 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java +++ b/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;