From a2e84e5684c193246713a36186a9d8098ba15fb6 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 13:28:30 -0800 Subject: [PATCH 01/16] Create CODEOWNERS for automatic PR review --- .github/CODEOWNERS | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 00000000..b0795ee4 --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,2 @@ +## code changes will send PR to following users +* @TitaniumTitans/Programming Team From f922aa325ebd5a1e211df8877a91eab22336a6d6 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:07:06 -0800 Subject: [PATCH 02/16] Fixed Constants.java SonarQube --- src/main/java/frc/robot/Constants.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d81ef09..3971679d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,9 +24,12 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + private Constants() { + throw new IllegalStateException("Constants class should not be constructed"); + } public static final Mode currentMode = Mode.REAL; - public static enum Mode { + public enum Mode { /** Running on a real robot. */ REAL, @@ -38,13 +41,19 @@ public static enum Mode { } public static class 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); - public static final double[] DRIVE_FF_GAINS = new double[]{0.13, 0.1, 0.0}; - public static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0}; - public static final double[] TURN_FB_GAINS = new double[]{7.0, 0.0, 0.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( 0, From 4d799ee87ddeb845e8121d0ed50b70c32c17b1b2 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:07:11 -0800 Subject: [PATCH 03/16] Update Robot.java --- src/main/java/frc/robot/Robot.java | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ea47d351..132643a0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -102,11 +102,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 +125,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 +143,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 +156,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 + } } From 9e692f1706a3707289bd25f876db09d96450864f Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:07:14 -0800 Subject: [PATCH 04/16] Update PhoenixOdometryThread.java --- .../robot/subsystems/drive/module/PhoenixOdometryThread.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index 70f337e1..2328dfb1 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -88,7 +88,9 @@ public void run() { // of Pro licensing. No reasoning for this behavior // is provided by the documentation. Thread.sleep((long) (1000.0 / Module.ODOMETRY_FREQUENCY)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + if (signals.length > 0) { + BaseStatusSignal.refreshAll(signals); + } } } catch (InterruptedException e) { e.printStackTrace(); From 2480d842ab73a2d950f3998a9f4c3f05e68f38e6 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:09:47 -0800 Subject: [PATCH 05/16] Update RobotContainer.java --- src/main/java/frc/robot/RobotContainer.java | 87 ++++++++++----------- 1 file changed, 42 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 617eac14..9b0ee378 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; @@ -34,7 +33,6 @@ import frc.robot.subsystems.shooter.ShooterIOPrototype; import frc.robot.subsystems.shooter.ShooterSubsystem; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -52,52 +50,49 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ 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 - m_driveSubsystem = + 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 + m_driveSubsystem = + new DriveSubsystem( + new GyroIO() { + }, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); + } + default -> { + // Replayed robot, disable IO implementations + m_driveSubsystem = new DriveSubsystem( - new GyroIO() { - }, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - m_shooter = new ShooterSubsystem(new ShooterIOPrototype()); - } - default -> { - // Replayed robot, disable IO implementations - m_driveSubsystem = - new DriveSubsystem( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }); - m_shooter = new ShooterSubsystem(new ShooterIO() { - }); - } + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); + m_shooter = new ShooterSubsystem(new ShooterIO() {}); } + } // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -106,7 +101,9 @@ public RobotContainer() { autoChooser.addOption( "Drive FF Characterization", new FeedForwardCharacterization( - m_driveSubsystem, m_driveSubsystem::runCharacterizationVolts, m_driveSubsystem::getCharacterizationVelocity)); + m_driveSubsystem, + m_driveSubsystem::runCharacterizationVolts, + m_driveSubsystem::getCharacterizationVelocity)); // Configure the button bindings configureButtonBindings(); @@ -146,6 +143,6 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand();//autoChooser.get(); + return autoChooser.get(); } } From a6e9352650916700f4f99db663c95255844bb03c Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:10:30 -0800 Subject: [PATCH 06/16] Update DriveCommands.java --- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 086cd217..3dd81731 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/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); } From 416d81d348b07d2ba765e5846542009bd9d49371 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:27:08 -0800 Subject: [PATCH 07/16] another mass update of files --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../commands/FeedForwardCharacterization.java | 13 +++--- .../subsystems/drive/DriveSubsystem.java | 21 +++++---- .../frc/robot/subsystems/drive/GyroIO.java | 4 +- .../robot/subsystems/drive/GyroIOPigeon1.java | 45 +++++++++---------- 5 files changed, 42 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9b0ee378..82d9ea5a 100644 --- 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 index d7ac7a7a..8081bd68 100644 --- 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 index e369e57f..59f063e1 100644 --- 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 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 index 38ccec1f..bcf122ed 100644 --- 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 index 5d127d11..dee1c14c 100644 --- 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 yawPositionQueue; + private final PigeonIMU m_pigeon; + private double prevYaw; + private Queue 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(); + } } From 79c20aa9504f9404d32de1ba170fa58624a83773 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:31:42 -0800 Subject: [PATCH 08/16] sonarqube still mad about this --- src/main/java/frc/robot/Robot.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 132643a0..4250d1ea 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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; } From 009d44abbfb06e661fdc749b8624a2177b1a8d59 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:39:05 -0800 Subject: [PATCH 09/16] minor updates for module.java and gyroiopigeon2 --- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../robot/subsystems/drive/module/Module.java | 34 +++++++++---------- 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 7470c01d..b2cebb45 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -57,7 +57,7 @@ public void updateInputs(GyroIOInputs inputs) { inputs.odometryYawPositions = yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) + .map(Rotation2d::fromDegrees) .toArray(Rotation2d[]::new); yawPositionQueue.clear(); } diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 3f4a466a..7dd76355 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -18,10 +18,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Module { @@ -51,22 +49,22 @@ public Module(ModuleIO io) { // Switch constants based on mode (the physics simulator is treated as a // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL, REPLAY -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - m_driveFeedback = new PIDController(0.05, 0.0, 0.0); - m_turnFeedback = new PIDController(7.0, 0.0, 0.0); - } - case SIM -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - m_driveFeedback = new PIDController(0.1, 0.0, 0.0); - m_turnFeedback = new PIDController(10.0, 0.0, 0.0); - } - default -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - m_driveFeedback = new PIDController(0.0, 0.0, 0.0); - m_turnFeedback = new PIDController(0.0, 0.0, 0.0); - } + switch (Constants.currentMode) { + case REAL, REPLAY -> { + m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + m_driveFeedback = new PIDController(0.05, 0.0, 0.0); + m_turnFeedback = new PIDController(7.0, 0.0, 0.0); + } + case SIM -> { + m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + m_driveFeedback = new PIDController(0.1, 0.0, 0.0); + m_turnFeedback = new PIDController(10.0, 0.0, 0.0); + } + default -> { + m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + m_driveFeedback = new PIDController(0.0, 0.0, 0.0); + m_turnFeedback = new PIDController(0.0, 0.0, 0.0); + } } m_turnFeedback.enableContinuousInput(-Math.PI, Math.PI); From 4cf7196603fc14167c9e3e94a87127c57a15fc3a Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:52:07 -0800 Subject: [PATCH 10/16] optimized imports --- src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index 7010f9f6..dde93d57 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -13,7 +13,6 @@ package frc.robot.subsystems.drive.module; -import com.gos.lib.properties.pid.PidProperty; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; From 2d79899344aaf718443faba32d2fb07f6ac70e1b Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:52:50 -0800 Subject: [PATCH 11/16] Update ModuleIO.java --- src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index dde93d57..b2bb029e 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -50,5 +50,7 @@ public default void setDriveBrakeMode(boolean enable) {} public default void setTurnBrakeMode(boolean enable) {} // Used to pass moduleConstants - public default ModuleConstants getModuleConstants() {throw new RuntimeException("getModuleConstants() not implemented");} + public default ModuleConstants getModuleConstants() { + throw new RuntimeException("getModuleConstants() not implemented"); + } } From bc8bc259d403f3cee714e7acc5118d9254a5d861 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:53:30 -0800 Subject: [PATCH 12/16] Update ModuleIO.java --- .../robot/subsystems/drive/module/ModuleIO.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index b2bb029e..c4a80124 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -18,7 +18,7 @@ public interface ModuleIO { @AutoLog - public static class ModuleIOInputs { + class ModuleIOInputs { public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; @@ -35,22 +35,22 @@ public static class ModuleIOInputs { } /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} + default void updateInputs(ModuleIOInputs inputs) {} /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} + default void setDriveVoltage(double volts) {} /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} + default void setTurnVoltage(double volts) {} /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} + default void setDriveBrakeMode(boolean enable) {} /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + default void setTurnBrakeMode(boolean enable) {} // Used to pass moduleConstants - public default ModuleConstants getModuleConstants() { - throw new RuntimeException("getModuleConstants() not implemented"); + default ModuleConstants getModuleConstants() { + throw new UnsupportedOperationException("getModuleConstants() not implemented"); } } From 1d4d4d9540cd4cb7c25b354c5c9802c0baa73437 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 15:17:06 -0800 Subject: [PATCH 13/16] this should be all other than inputs classes --- .../commands/FeedForwardCharacterization.java | 2 +- .../drive/module/ModuleIOSparkMax.java | 6 +- .../drive/module/ModuleIOTalonFX.java | 1 + .../robot/subsystems/shooter/ShooterIO.java | 16 ++-- .../shooter/ShooterIOPrototype.java | 86 +++++++++--------- .../subsystems/shooter/ShooterSubsystem.java | 60 +++++++------ .../java/frc/robot/util/LocalADStarAK.java | 6 +- .../frc/robot/util/PolynomialRegression.java | 88 ++++++++++++++----- 8 files changed, 156 insertions(+), 109 deletions(-) diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index 8081bd68..e50d09c6 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -99,7 +99,7 @@ public void print() { 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("\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/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java index 07c482c2..8a9ed623 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java @@ -50,7 +50,7 @@ public class ModuleIOSparkMax implements ModuleIO { private final Queue drivePositionQueue; private final Queue turnPositionQueue; - private final boolean isTurnMotorInverted = true; + private static final boolean TURN_MOTOR_INVERTED = true; private final Rotation2d absoluteEncoderOffset; public ModuleIOSparkMax(int index) { @@ -80,7 +80,7 @@ public ModuleIOSparkMax(int index) { absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; default: - throw new RuntimeException("Invalid module index"); + throw new UnsupportedOperationException("Invalid module index"); } driveSparkMax.restoreFactoryDefaults(); @@ -92,7 +92,7 @@ public ModuleIOSparkMax(int index) { driveEncoder = driveSparkMax.getEncoder(); turnRelativeEncoder = turnSparkMax.getEncoder(); - turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkMax.setInverted(TURN_MOTOR_INVERTED); driveSparkMax.setSmartCurrentLimit(40); turnSparkMax.setSmartCurrentLimit(30); driveSparkMax.enableVoltageCompensation(12.0); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index ed183e4e..3fb04620 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -43,6 +43,7 @@ * absolute encoders using AdvantageScope. These values are logged under * "/Drive/ModuleX/TurnAbsolutePositionRad" */ + public class ModuleIOTalonFX implements ModuleIO { private final TalonFX m_driveTalon; private final TalonFX m_turnTalon; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 366c94a5..38d32e0c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -3,14 +3,12 @@ import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { - - @AutoLog - class ShooterIOInputs { - } - default void setMotorVoltageTL(double voltage) {} - default void setMotorVoltageTR(double voltage) {} - default void setMotorVoltageBL(double voltage) {} - default void setMotorVoltageBR(double voltage) {} - default void setKickerVoltage(double voltage) {} + @AutoLog + class ShooterIOInputs {} + default void setMotorVoltageTL(double voltage) {} + default void setMotorVoltageTR(double voltage) {} + default void setMotorVoltageBL(double voltage) {} + default void setMotorVoltageBR(double voltage) {} + default void setKickerVoltage(double voltage) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index b796facd..1603cdb9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -5,47 +5,47 @@ import com.revrobotics.CANSparkMax; public class ShooterIOPrototype implements ShooterIO { - private final CANSparkFlex m_topLeftMotor; - private final CANSparkFlex m_topRightMotor; - private final CANSparkMax m_bottomLeftMotor; - private final CANSparkMax m_bottomRightMotor; - private final CANSparkMax m_kickekMotor; - public ShooterIOPrototype() { - m_topLeftMotor = new CANSparkFlex(13, CANSparkLowLevel.MotorType.kBrushless); - m_topRightMotor = new CANSparkFlex(14, CANSparkLowLevel.MotorType.kBrushless); - m_bottomLeftMotor = new CANSparkMax(15, CANSparkLowLevel.MotorType.kBrushless); - m_bottomRightMotor = new CANSparkMax(16, CANSparkLowLevel.MotorType.kBrushless); - m_kickekMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); - - m_topLeftMotor.setInverted(false); - m_bottomLeftMotor.setInverted(true); - - m_topLeftMotor.burnFlash(); - m_topRightMotor.burnFlash(); - } - - @Override - public void setMotorVoltageTL(double voltage) { - m_topLeftMotor.setVoltage(voltage); - } - - @Override - public void setMotorVoltageTR(double voltage) { - m_topRightMotor.setVoltage(voltage); - } - - @Override - public void setMotorVoltageBL(double voltage) { - m_bottomLeftMotor.setVoltage(voltage); - } - - @Override - public void setMotorVoltageBR(double voltage) { - m_bottomRightMotor.setVoltage(voltage); - } - - @Override - public void setKickerVoltage(double voltage) { - m_kickekMotor.setVoltage(voltage); - } + private final CANSparkFlex m_topLeftMotor; + private final CANSparkFlex m_topRightMotor; + private final CANSparkMax m_bottomLeftMotor; + private final CANSparkMax m_bottomRightMotor; + private final CANSparkMax m_kickekMotor; + public ShooterIOPrototype() { + m_topLeftMotor = new CANSparkFlex(13, CANSparkLowLevel.MotorType.kBrushless); + m_topRightMotor = new CANSparkFlex(14, CANSparkLowLevel.MotorType.kBrushless); + m_bottomLeftMotor = new CANSparkMax(15, CANSparkLowLevel.MotorType.kBrushless); + m_bottomRightMotor = new CANSparkMax(16, CANSparkLowLevel.MotorType.kBrushless); + m_kickekMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); + + m_topLeftMotor.setInverted(false); + m_bottomLeftMotor.setInverted(true); + + m_topLeftMotor.burnFlash(); + m_topRightMotor.burnFlash(); + } + + @Override + public void setMotorVoltageTL(double voltage) { + m_topLeftMotor.setVoltage(voltage); + } + + @Override + public void setMotorVoltageTR(double voltage) { + m_topRightMotor.setVoltage(voltage); + } + + @Override + public void setMotorVoltageBL(double voltage) { + m_bottomLeftMotor.setVoltage(voltage); + } + + @Override + public void setMotorVoltageBR(double voltage) { + m_bottomRightMotor.setVoltage(voltage); + } + + @Override + public void setKickerVoltage(double voltage) { + m_kickekMotor.setVoltage(voltage); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index d32b3733..ad39de26 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -6,34 +6,36 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - private final ShooterIO m_io; - public ShooterSubsystem(ShooterIO io) { - m_io = io; - SmartDashboard.putNumber("Top wheel ratio", 0.8); - SmartDashboard.putNumber("Left wheel", 0.6); - SmartDashboard.putNumber("Right wheel", 0.6); - } - - public void setShooterPowerLeft(double power) { - m_io.setMotorVoltageTL((power * 12.0) * SmartDashboard.getNumber("Top wheel ratio", 0.85)); - m_io.setMotorVoltageBL(power * 12.0); - } - - public void setShooterPowerRight(double power) { - m_io.setMotorVoltageTR((power * 12.0) * SmartDashboard.getNumber("Top wheel ratio", 0.85)); - m_io.setMotorVoltageBR(power * 12.0); - } - - public void setKickerPower(double power) { - m_io.setKickerVoltage(power * 12.0); - } - - public Command setShooterPower(double left, double right) { - return run(() -> { - setShooterPowerLeft(left == 0.0 ? 0.0 : SmartDashboard.getNumber("Left wheel", 0.6)); - setShooterPowerRight(right == 0.0 ? 0.0 : SmartDashboard.getNumber("Right wheel", 0.6)); - setKickerPower(left == 0.0 ? 0.0 : 1.0); - }); - } + private final ShooterIO m_io; + + private static final String TOP_WHEEL_RATIO = "Top wheel ratio"; + public ShooterSubsystem(ShooterIO io) { + m_io = io; + SmartDashboard.putNumber(TOP_WHEEL_RATIO, 0.8); + SmartDashboard.putNumber("Left wheel", 0.6); + SmartDashboard.putNumber("Right wheel", 0.6); + } + + public void setShooterPowerLeft(double power) { + m_io.setMotorVoltageTL((power * 12.0) * SmartDashboard.getNumber(TOP_WHEEL_RATIO, 0.85)); + m_io.setMotorVoltageBL(power * 12.0); + } + + public void setShooterPowerRight(double power) { + m_io.setMotorVoltageTR((power * 12.0) * SmartDashboard.getNumber(TOP_WHEEL_RATIO, 0.85)); + m_io.setMotorVoltageBR(power * 12.0); + } + + public void setKickerPower(double power) { + m_io.setKickerVoltage(power * 12.0); + } + + public Command setShooterPower(double left, double right) { + return run(() -> { + setShooterPowerLeft(left == 0.0 ? 0.0 : SmartDashboard.getNumber("Left wheel", 0.6)); + setShooterPowerRight(right == 0.0 ? 0.0 : SmartDashboard.getNumber("Right wheel", 0.6)); + setKickerPower(left == 0.0 ? 0.0 : 1.0); + }); + } } diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 37222f23..58520e26 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -100,9 +100,9 @@ public void setDynamicObstacles( } private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); + protected LocalADStar adStar = new LocalADStar(); + protected boolean isNewPathAvailable = false; + protected List currentPathPoints = Collections.emptyList(); @Override public void toLog(LogTable table) { diff --git a/src/main/java/frc/robot/util/PolynomialRegression.java b/src/main/java/frc/robot/util/PolynomialRegression.java index 28c57967..2bc44b4c 100644 --- a/src/main/java/frc/robot/util/PolynomialRegression.java +++ b/src/main/java/frc/robot/util/PolynomialRegression.java @@ -2,6 +2,7 @@ import Jama.Matrix; import Jama.QRDecomposition; +import edu.wpi.first.wpilibj.DriverStation; // NOTE: This file is available at // http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html @@ -25,8 +26,8 @@ public class PolynomialRegression implements Comparable { private final String variableName; // name of the predictor variable private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error + private final Matrix beta; // the polynomial regression coefficients + private final double sse; // sum of squares due to error private double sst; // total sum of squares /** @@ -74,7 +75,9 @@ public PolynomialRegression(double[] x, double[] y, int degree, String variableN // find least squares solution qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; + if (qr.isFullRank()) { + break; + } // decrease degree and try again this.degree--; @@ -88,7 +91,9 @@ public PolynomialRegression(double[] x, double[] y, int degree, String variableN // mean of y[] values double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; + for (int i = 0; i < n; i++) { + sum += y[i]; + } double mean = sum / n; // total variation to be accounted for @@ -110,7 +115,9 @@ public PolynomialRegression(double[] x, double[] y, int degree, String variableN */ public double beta(int j) { // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + if (Math.abs(beta.get(j, 0)) < 1E-4) { + return 0.0; + } return beta.get(j, 0); } @@ -129,8 +136,10 @@ public int degree() { * @return the coefficient of determination R2, which is a real number between * 0 and 1 */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function + public double r2() { + if (sst == 0.0) { + return 1.0; // constant function + } return 1.0 - sse / sst; } @@ -143,7 +152,9 @@ public double R2() { public double predict(double x) { // horner's method double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + for (int j = degree; j >= 0; j--) { + y = beta(j) + (x * y); + } return y; } @@ -158,38 +169,73 @@ public String toString() { int j = degree; // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + while (j >= 0 && Math.abs(beta(j)) < 1E-5) { + j--; + } // create remaining terms while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + if (j == 0) { + s.append(String.format("%.10f ", beta(j))); + } + else if (j == 1) { + s.append(String.format("%.10f %s + ", beta(j), variableName)); + } else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); j--; } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + s.append(" (R^2 = ").append(String.format("%.3f", r2())).append(")"); - // replace "+ -2n" with "- 2n" + // replace "+ -2n" with "- 2n" return s.toString().replace("+ -", "- "); } /** Compare lexicographically. */ public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; + double lEPSILON = 1E-5; int maxDegree = Math.max(this.degree(), that.degree()); for (int j = maxDegree; j >= 0; j--) { double term1 = 0.0; double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; + if (this.degree() >= j) { + term1 = this.beta(j); + } + if (that.degree() >= j) { + term2 = that.beta(j); + } + if (Math.abs(term1) < lEPSILON) { + term1 = 0.0; + } + if (Math.abs(term2) < lEPSILON) { + term2 = 0.0; + } + if (term1 < term2) { + return -1; + } + else if (term1 > term2) { + return +1; + } } return 0; } + @Override + public boolean equals(Object o) { + if (o == null) { + return false; + } + if (o.getClass() == this.getClass()) { + return this.compareTo((PolynomialRegression) o) == 0; + } else { + return false; + } + } + + @Override + public int hashCode() { + return super.hashCode(); + } + /** * Unit tests the {@code PolynomialRegression} data type. * @@ -200,6 +246,6 @@ public static void main(String[] args) { double[] y = {100, 350, 1500, 6700, 20160, 40000}; PolynomialRegression regression = new PolynomialRegression(x, y, 3); - System.out.println(regression); + DriverStation.reportWarning(String.valueOf(regression), false); } } From 0f862f8c525cefba6eeaae9a627ff2becf123f13 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 21 Jan 2024 09:12:02 -0800 Subject: [PATCH 14/16] fixed build issue with sim modules --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 82d9ea5a..664e879a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; @@ -70,10 +71,10 @@ public RobotContainer() { new DriveSubsystem( new GyroIO() { }, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); + 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 -> { From b5ad9b72572a2ad3ef750d11a133d829ac5da857 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 21 Jan 2024 09:24:11 -0800 Subject: [PATCH 15/16] Fixed encapsulation errors on IOInputs classes --- .../subsystems/drive/DriveSubsystem.java | 6 +- .../frc/robot/subsystems/drive/GyroIO.java | 40 +++++- .../robot/subsystems/drive/GyroIOPigeon1.java | 13 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 13 +- .../robot/subsystems/drive/module/Module.java | 20 +-- .../subsystems/drive/module/ModuleIO.java | 114 ++++++++++++++++-- .../subsystems/drive/module/ModuleIOSim.java | 23 ++-- .../drive/module/ModuleIOSparkMax.java | 47 +++----- .../drive/module/ModuleIOTalonFX.java | 43 +++---- 9 files changed, 211 insertions(+), 108 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 59f063e1..118065c9 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -116,7 +116,7 @@ public void periodic() { // 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); } @@ -132,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; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index bcf122ed..c60673c1 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -19,10 +19,42 @@ public interface GyroIO { @AutoLog class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; + 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; + } } 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 index dee1c14c..183004ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java @@ -22,15 +22,14 @@ public GyroIOPigeon1(int id) { @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); + inputs.setConnected(m_pigeon.getState() == PigeonIMU.PigeonState.Ready); + inputs.setYawPosition(Rotation2d.fromDegrees(m_pigeon.getYaw())); + inputs.setYawVelocityRadPerSec((m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20)); prevYaw = m_pigeon.getYaw(); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map(Rotation2d::fromDegrees) - .toArray(Rotation2d[]::new); + inputs.setOdometryYawPositions(yawPositionQueue.stream() + .map(Rotation2d::fromDegrees) + .toArray(Rotation2d[]::new)); yawPositionQueue.clear(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index b2cebb45..e74d0b90 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -51,14 +51,13 @@ public GyroIOPigeon2(boolean phoenixDrive) { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.setConnected(BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK)); + inputs.setYawPosition(Rotation2d.fromDegrees(yaw.getValueAsDouble())); + inputs.setYawVelocityRadPerSec(Units.degreesToRadians(yawVelocity.getValueAsDouble())); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map(Rotation2d::fromDegrees) - .toArray(Rotation2d[]::new); + inputs.setOdometryYawPositions(yawPositionQueue.stream() + .map(Rotation2d::fromDegrees) + .toArray(Rotation2d[]::new)); yawPositionQueue.clear(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 4bcf94f5..bc03ec63 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -84,8 +84,8 @@ public void periodic() { // On first cycle, reset relative turn encoder // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (m_turnRelativeOffset == null && m_inputs.turnAbsolutePosition.getRadians() != 0.0) { - m_turnRelativeOffset = m_inputs.turnAbsolutePosition.minus(m_inputs.turnPosition); + if (m_turnRelativeOffset == null && m_inputs.getTurnAbsolutePosition().getRadians() != 0.0) { + m_turnRelativeOffset = m_inputs.getTurnAbsolutePosition().minus(m_inputs.getTurnPosition()); } // Run closed loop turn control @@ -107,18 +107,18 @@ public void periodic() { double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; m_io.setDriveVoltage( m_driveFeedforward.calculate(velocityRadPerSec) - + m_driveFeedback.calculate(m_inputs.driveVelocityRadPerSec, velocityRadPerSec)); + + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec)); } } // Calculate position deltas for odometry int deltaCount = - Math.min(m_inputs.odometryDrivePositionsRad.length, m_inputs.odometryTurnPositions.length); + Math.min(m_inputs.getOdometryDrivePositionsRad().length, m_inputs.getOdometryTurnPositions().length); m_positionDeltas = new SwerveModulePosition[deltaCount]; for (int i = 0; i < deltaCount; i++) { - double positionMeters = m_inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; + double positionMeters = m_inputs.getOdometryDrivePositionsRad()[i] * WHEEL_RADIUS; Rotation2d angle = - m_inputs.odometryTurnPositions[i].plus( + m_inputs.getOdometryTurnPositions()[i].plus( m_turnRelativeOffset != null ? m_turnRelativeOffset : new Rotation2d()); m_positionDeltas[i] = new SwerveModulePosition(positionMeters - m_lastPositionMeters, angle); m_lastPositionMeters = positionMeters; @@ -169,18 +169,18 @@ public Rotation2d getAngle() { if (m_turnRelativeOffset == null) { return new Rotation2d(); } else { - return m_inputs.turnPosition.plus(m_turnRelativeOffset); + return m_inputs.getTurnPosition().plus(m_turnRelativeOffset); } } /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return m_inputs.drivePositionRad * WHEEL_RADIUS; + return m_inputs.getDrivePositionRad() * WHEEL_RADIUS; } /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return m_inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + return m_inputs.getDriveVelocityRadPerSec() * WHEEL_RADIUS; } /** Returns the module position (turn angle and drive position). */ @@ -200,6 +200,6 @@ public SwerveModulePosition[] getPositionDeltas() { /** Returns the drive velocity in radians/sec. */ public double getCharacterizationVelocity() { - return m_inputs.driveVelocityRadPerSec; + return m_inputs.getDriveVelocityRadPerSec(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index c4a80124..4181df2f 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -19,19 +19,107 @@ public interface ModuleIO { @AutoLog class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; - - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; - - public double[] odometryDrivePositionsRad = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + protected double drivePositionRad = 0.0; + protected double driveVelocityRadPerSec = 0.0; + protected double driveAppliedVolts = 0.0; + protected double[] driveCurrentAmps = new double[] {}; + + protected Rotation2d turnAbsolutePosition = new Rotation2d(); + protected Rotation2d turnPosition = new Rotation2d(); + protected double turnVelocityRadPerSec = 0.0; + protected double turnAppliedVolts = 0.0; + protected double[] turnCurrentAmps = new double[] {}; + + protected double[] odometryDrivePositionsRad = new double[] {}; + protected Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + + public double getDrivePositionRad() { + return drivePositionRad; + } + + public void setDrivePositionRad(double drivePositionRad) { + this.drivePositionRad = drivePositionRad; + } + + public double getDriveVelocityRadPerSec() { + return driveVelocityRadPerSec; + } + + public void setDriveVelocityRadPerSec(double driveVelocityRadPerSec) { + this.driveVelocityRadPerSec = driveVelocityRadPerSec; + } + + public double getDriveAppliedVolts() { + return driveAppliedVolts; + } + + public void setDriveAppliedVolts(double driveAppliedVolts) { + this.driveAppliedVolts = driveAppliedVolts; + } + + public double[] getDriveCurrentAmps() { + return driveCurrentAmps; + } + + public void setDriveCurrentAmps(double[] driveCurrentAmps) { + this.driveCurrentAmps = driveCurrentAmps; + } + + public Rotation2d getTurnAbsolutePosition() { + return turnAbsolutePosition; + } + + public void setTurnAbsolutePosition(Rotation2d turnAbsolutePosition) { + this.turnAbsolutePosition = turnAbsolutePosition; + } + + public Rotation2d getTurnPosition() { + return turnPosition; + } + + public void setTurnPosition(Rotation2d turnPosition) { + this.turnPosition = turnPosition; + } + + public double getTurnVelocityRadPerSec() { + return turnVelocityRadPerSec; + } + + public void setTurnVelocityRadPerSec(double turnVelocityRadPerSec) { + this.turnVelocityRadPerSec = turnVelocityRadPerSec; + } + + public double getTurnAppliedVolts() { + return turnAppliedVolts; + } + + public void setTurnAppliedVolts(double turnAppliedVolts) { + this.turnAppliedVolts = turnAppliedVolts; + } + + public double[] getTurnCurrentAmps() { + return turnCurrentAmps; + } + + public void setTurnCurrentAmps(double[] turnCurrentAmps) { + this.turnCurrentAmps = turnCurrentAmps; + } + + public double[] getOdometryDrivePositionsRad() { + return odometryDrivePositionsRad; + } + + public void setOdometryDrivePositionsRad(double[] odometryDrivePositionsRad) { + this.odometryDrivePositionsRad = odometryDrivePositionsRad; + } + + public Rotation2d[] getOdometryTurnPositions() { + return odometryTurnPositions; + } + + public void setOdometryTurnPositions(Rotation2d[] odometryTurnPositions) { + this.odometryTurnPositions = odometryTurnPositions; + } } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index 5fb2d65d..3fafb6e2 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -47,20 +47,19 @@ public void updateInputs(ModuleIOInputs inputs) { m_driveSim.update(LOOP_PERIOD_SECS); m_turnSim.update(LOOP_PERIOD_SECS); - inputs.drivePositionRad = m_driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = m_driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(m_driveSim.getCurrentDrawAmps())}; + inputs.setDrivePositionRad(m_driveSim.getAngularPositionRad()); + inputs.setDriveVelocityRadPerSec(m_driveSim.getAngularVelocityRadPerSec()); + inputs.setDriveAppliedVolts(driveAppliedVolts); + inputs.setDriveCurrentAmps(new double[] {Math.abs(m_driveSim.getCurrentDrawAmps())}); - inputs.turnAbsolutePosition = - new Rotation2d(m_turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(m_turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = m_turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(m_turnSim.getCurrentDrawAmps())}; + inputs.setTurnAbsolutePosition(new Rotation2d(m_turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition)); + inputs.setTurnPosition(new Rotation2d(m_turnSim.getAngularPositionRad())); + inputs.setTurnVelocityRadPerSec(m_turnSim.getAngularVelocityRadPerSec()); + inputs.setTurnAppliedVolts(turnAppliedVolts); + inputs.setTurnCurrentAmps(new double[] {Math.abs(m_turnSim.getCurrentDrawAmps())}); - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + inputs.setOdometryDrivePositionsRad(new double[] {inputs.getDrivePositionRad()}); + inputs.setOdometryTurnPositions(new Rotation2d[] {inputs.getTurnPosition()}); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java index 8a9ed623..cda0fbd6 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java @@ -124,33 +124,26 @@ public ModuleIOSparkMax(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = - new Rotation2d( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); + inputs.setDrivePositionRad(Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO); + inputs.setDriveVelocityRadPerSec(Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO); + inputs.setDriveAppliedVolts(driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage()); + inputs.setDriveCurrentAmps(new double[] {driveSparkMax.getOutputCurrent()}); + + inputs.setTurnAbsolutePosition(new Rotation2d( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) + .minus(absoluteEncoderOffset)); + inputs.setTurnPosition(Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO)); + inputs.setTurnVelocityRadPerSec(Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / TURN_GEAR_RATIO); + inputs.setTurnAppliedVolts(turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage()); + inputs.setTurnCurrentAmps(new double[] {turnSparkMax.getOutputCurrent()}); + + inputs.setOdometryDrivePositionsRad(drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray()); + inputs.setOdometryTurnPositions(turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new)); drivePositionQueue.clear(); turnPositionQueue.clear(); } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 3fb04620..77a5dbf5 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -168,31 +168,24 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnAppliedVolts, m_turnCurrent); - inputs.drivePositionRad = - Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO(); - inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble()) - .minus(m_moduleConstants.ENCODER_OFFSET()); - inputs.turnPosition = - Rotation2d.fromRotations((m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO())); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO(); - inputs.turnAppliedVolts = m_turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {m_turnCurrent.getValueAsDouble()}; - - inputs.odometryDrivePositionsRad = - m_drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO()) - .toArray(); - inputs.odometryTurnPositions = - m_turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO())) - .toArray(Rotation2d[]::new); + inputs.setDrivePositionRad(Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); + inputs.setDriveVelocityRadPerSec(Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); + inputs.setDriveAppliedVolts(m_driveAppliedVolts.getValueAsDouble()); + inputs.setDriveCurrentAmps(new double[] {m_driveCurrent.getValueAsDouble()}); + + inputs.setTurnAbsolutePosition(Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble()) + .minus(m_moduleConstants.ENCODER_OFFSET())); + inputs.setTurnPosition(Rotation2d.fromRotations((m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO()))); + inputs.setTurnVelocityRadPerSec(Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); + inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble()); + inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()}); + + inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO()) + .toArray()); + inputs.setOdometryTurnPositions(m_turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO())) + .toArray(Rotation2d[]::new)); m_drivePositionQueue.clear(); m_turnPositionQueue.clear(); } From b7bbc6890203b6d3aa5bbb5d1058902a2b1a165b Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 21 Jan 2024 10:14:45 -0800 Subject: [PATCH 16/16] one final sonarqube thing --- .../subsystems/drive/module/ModuleIOSparkMax.java | 3 ++- .../subsystems/drive/module/ModuleIOTalonFX.java | 12 ++++++++---- .../drive/module/PhoenixOdometryThread.java | 3 ++- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java index cda0fbd6..50e8484e 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java @@ -125,7 +125,8 @@ public ModuleIOSparkMax(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.setDrivePositionRad(Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO); - inputs.setDriveVelocityRadPerSec(Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO); + inputs.setDriveVelocityRadPerSec( + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO); inputs.setDriveAppliedVolts(driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage()); inputs.setDriveCurrentAmps(new double[] {driveSparkMax.getOutputCurrent()}); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 77a5dbf5..62e59b6b 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -168,15 +168,19 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnAppliedVolts, m_turnCurrent); - inputs.setDrivePositionRad(Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); - inputs.setDriveVelocityRadPerSec(Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); + inputs.setDrivePositionRad( + Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); + inputs.setDriveVelocityRadPerSec( + Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); inputs.setDriveAppliedVolts(m_driveAppliedVolts.getValueAsDouble()); inputs.setDriveCurrentAmps(new double[] {m_driveCurrent.getValueAsDouble()}); inputs.setTurnAbsolutePosition(Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble()) .minus(m_moduleConstants.ENCODER_OFFSET())); - inputs.setTurnPosition(Rotation2d.fromRotations((m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO()))); - inputs.setTurnVelocityRadPerSec(Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); + inputs.setTurnPosition(Rotation2d.fromRotations(( + m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO()))); + inputs.setTurnVelocityRadPerSec( + Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble()); inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()}); diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index 2328dfb1..2a6bde0a 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.subsystems.drive.DriveSubsystem; import java.util.ArrayList; @@ -93,7 +94,7 @@ public void run() { } } } catch (InterruptedException e) { - e.printStackTrace(); + DriverStation.reportError(e.toString(), true); } finally { signalsLock.unlock(); }