From 1e90118c3984c7ae99f682e81c0f13ef429ae39a Mon Sep 17 00:00:00 2001 From: SWDevilTech Date: Wed, 28 Jan 2026 19:13:45 -0500 Subject: [PATCH 1/2] Added CAN utilization logging and optomization --- src/main/java/frc/lib/CanRefreshRate.java | 11 ++ src/main/java/frc/lib/LoggableSubsystem.java | 7 ++ src/main/java/frc/lib/swerve/Pigeon2Io.java | 8 +- .../frc/lib/swerve/SdsSwerveModuleIo.java | 17 ++- .../frc/robot/subsystems/SwerveDrive2026.java | 118 ++++++++++-------- 5 files changed, 95 insertions(+), 66 deletions(-) create mode 100644 src/main/java/frc/lib/CanRefreshRate.java diff --git a/src/main/java/frc/lib/CanRefreshRate.java b/src/main/java/frc/lib/CanRefreshRate.java new file mode 100644 index 0000000..8d6a98b --- /dev/null +++ b/src/main/java/frc/lib/CanRefreshRate.java @@ -0,0 +1,11 @@ +package frc.lib; + +public enum CanRefreshRate { + SLOW(10), DEFAULT(100), FAST(500); + + public final int rateHz; + + private CanRefreshRate(int rateHz) { + this.rateHz = rateHz; + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/LoggableSubsystem.java b/src/main/java/frc/lib/LoggableSubsystem.java index 0d7040f..6f1ad59 100644 --- a/src/main/java/frc/lib/LoggableSubsystem.java +++ b/src/main/java/frc/lib/LoggableSubsystem.java @@ -7,9 +7,11 @@ public abstract class LoggableSubsystem extends SubsystemBase { private final Set children = new LinkedHashSet<>(); + private final String name; protected LoggableSubsystem(String name) { super(name); + this.name = name; } protected void addChildren(String folder, LoggableComponent... children) { @@ -31,6 +33,11 @@ protected String getOutputLogPath(String suffix) { return getName() + "/Outputs/" + suffix; } + @Override + public String getName() { + return name; + } + @Override public void periodic() { for (LoggableComponent child : children) { diff --git a/src/main/java/frc/lib/swerve/Pigeon2Io.java b/src/main/java/frc/lib/swerve/Pigeon2Io.java index 366af5b..489f502 100644 --- a/src/main/java/frc/lib/swerve/Pigeon2Io.java +++ b/src/main/java/frc/lib/swerve/Pigeon2Io.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; +import frc.lib.CanRefreshRate; public class Pigeon2Io extends GyroIo { @@ -22,14 +23,13 @@ public Pigeon2Io(String ioName, Pigeon2 gyro) { pitch = gyro.getPitch(); roll = gyro.getRoll(); - yaw.setUpdateFrequency(100); - pitch.setUpdateFrequency(100); - roll.setUpdateFrequency(100); + yaw.setUpdateFrequency(CanRefreshRate.FAST.rateHz); + pitch.setUpdateFrequency(CanRefreshRate.SLOW.rateHz); + roll.setUpdateFrequency(CanRefreshRate.SLOW.rateHz); } @Override protected void updateInputs(GyroInputs inputs) { - StatusSignal.refreshAll(roll, pitch, yaw); inputs.pitch = Rotation2d.fromDegrees(pitch.getValueAsDouble()); inputs.roll = Rotation2d.fromDegrees(roll.getValueAsDouble()); inputs.yaw = Rotation2d.fromDegrees(yaw.getValueAsDouble()); diff --git a/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java b/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java index 3842a7c..9d442c6 100644 --- a/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java +++ b/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java @@ -22,6 +22,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; +import frc.lib.CanRefreshRate; public class SdsSwerveModuleIo extends SwerveModuleIo { @@ -103,13 +104,13 @@ public SdsSwerveModuleIo(String name, Translation2d location, ModuleType moduleT driveMotorCurrent = driveMotor.getStatorCurrent(); steerMotorCurrent = steerMotor.getStatorCurrent(); - driveMotorVelocity.setUpdateFrequency(50); - canCoderAbsolutePosition.setUpdateFrequency(100); - driveMotorPosition.setUpdateFrequency(100); - driveMotorTemperature.setUpdateFrequency(5); - steerMotorTemperature.setUpdateFrequency(5); - driveMotorCurrent.setUpdateFrequency(10); - steerMotorCurrent.setUpdateFrequency(10); + driveMotorVelocity.setUpdateFrequency(CanRefreshRate.DEFAULT.rateHz); + canCoderAbsolutePosition.setUpdateFrequency(CanRefreshRate.FAST.rateHz); + driveMotorPosition.setUpdateFrequency(CanRefreshRate.FAST.rateHz); + driveMotorTemperature.setUpdateFrequency(CanRefreshRate.SLOW.rateHz); + steerMotorTemperature.setUpdateFrequency(CanRefreshRate.SLOW.rateHz); + driveMotorCurrent.setUpdateFrequency(CanRefreshRate.DEFAULT.rateHz); + steerMotorCurrent.setUpdateFrequency(CanRefreshRate.DEFAULT.rateHz); } @Override @@ -129,8 +130,6 @@ public void setAngle(Rotation2d angle) { @Override protected void updateInputs(SwerveInputs inputs) { - StatusSignal.refreshAll(driveMotorVelocity, canCoderAbsolutePosition, driveMotorPosition, steerMotorTemperature, - driveMotorTemperature, steerMotorCurrent, driveMotorCurrent); inputs.speed = driveMotorVelocity.getValueAsDouble() / driveGearRatio.driveRatio * (2 * WHEEL_RADIUS * Math.PI); inputs.angle = Rotation2d.fromRotations(canCoderAbsolutePosition.getValueAsDouble()).minus(cancoderOffset); inputs.distance = driveMotorPosition.getValueAsDouble() / driveGearRatio.driveRatio diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive2026.java b/src/main/java/frc/robot/subsystems/SwerveDrive2026.java index 508dc49..bb600d1 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive2026.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive2026.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.hardware.CANcoder; @@ -15,71 +17,81 @@ import frc.lib.swerve.GyroIo; import frc.lib.swerve.Pigeon2Io; import frc.lib.swerve.SdsSwerveModuleIo; +import frc.lib.swerve.SdsSwerveModuleIo.ModuleType; import frc.lib.swerve.SwerveDrive; import frc.lib.swerve.SwerveModule; import frc.lib.swerve.SwerveModuleIo; -import frc.lib.swerve.SdsSwerveModuleIo.ModuleType; public class SwerveDrive2026 extends SwerveDrive { - private static final CANBus CANIVORE_BUS = new CANBus("1559_Canivore"); - private static final double MASS = Units.lbsToKilograms(50); - private static final double RADIUS = Units.inchesToMeters(27/2.0); // Give or take - private static final double MOI = MASS * RADIUS * RADIUS; - private static final double SWERVE_MAX_LINEAR_VELOCITY = 5.21; - private static final double SWERVE_MAX_LINEAR_ACCEL = 3; - private static final double SWERVE_MAX_ANGULAR_VELOCITY = 18; - - private static final double SWERVE_MAX_ANGULAR_ACCEL = SWERVE_MAX_ANGULAR_VELOCITY / .01; - public static final SwerveConstraints SWERVE_CONSTRAINTS = new SwerveConstraints(SWERVE_MAX_ANGULAR_VELOCITY, SWERVE_MAX_ANGULAR_ACCEL, SWERVE_MAX_LINEAR_VELOCITY, SWERVE_MAX_LINEAR_ACCEL); - public static final SwerveConstraints SLOW_SWERVE_CONSTRAINTS = new SwerveConstraints(SWERVE_MAX_ANGULAR_VELOCITY / 6, SWERVE_MAX_ANGULAR_ACCEL, SWERVE_MAX_LINEAR_VELOCITY / 6, SWERVE_MAX_LINEAR_ACCEL); + private static final CANBus CANIVORE_BUS = new CANBus("1559_Canivore"); + private static final double MASS = Units.lbsToKilograms(50); + private static final double RADIUS = Units.inchesToMeters(27 / 2.0); // Give or take + private static final double MOI = MASS * RADIUS * RADIUS; + private static final double SWERVE_MAX_LINEAR_VELOCITY = 5.21; + private static final double SWERVE_MAX_LINEAR_ACCEL = 3; + private static final double SWERVE_MAX_ANGULAR_VELOCITY = 18; + + private static final double SWERVE_MAX_ANGULAR_ACCEL = SWERVE_MAX_ANGULAR_VELOCITY / .01; + public static final SwerveConstraints SWERVE_CONSTRAINTS = new SwerveConstraints(SWERVE_MAX_ANGULAR_VELOCITY, + SWERVE_MAX_ANGULAR_ACCEL, SWERVE_MAX_LINEAR_VELOCITY, SWERVE_MAX_LINEAR_ACCEL); + public static final SwerveConstraints SLOW_SWERVE_CONSTRAINTS = new SwerveConstraints( + SWERVE_MAX_ANGULAR_VELOCITY / 6, SWERVE_MAX_ANGULAR_ACCEL, SWERVE_MAX_LINEAR_VELOCITY / 6, + SWERVE_MAX_LINEAR_ACCEL); - public SwerveDrive2026() { - super("SwerveDrive", createGyro(), createModules()); + public SwerveDrive2026() { + super("SwerveDrive", createGyro(), createModules()); - SwerveModule[] modules = getModules(); - Translation2d[] locations = new Translation2d[modules.length]; - for (int i = 0; i < locations.length; i++) { - locations[i] = modules[i].getLocation(); - } - RobotConfig config = new RobotConfig(MASS, MOI, - new ModuleConfig(SdsSwerveModuleIo.WHEEL_RADIUS, 5, 1.0, DCMotor.getKrakenX60(1).withReduction(50d / 14 * 16 / 28 * 45 / 15), 80.0, - 1), - locations); - configureAuto(config); + SwerveModule[] modules = getModules(); + Translation2d[] locations = new Translation2d[modules.length]; + for (int i = 0; i < locations.length; i++) { + locations[i] = modules[i].getLocation(); } + RobotConfig config = new RobotConfig(MASS, MOI, + new ModuleConfig(SdsSwerveModuleIo.WHEEL_RADIUS, 5, 1.0, + DCMotor.getKrakenX60(1).withReduction(50d / 14 * 16 / 28 * 45 / 15), 80.0, + 1), + locations); + configureAuto(config); + } - private static SwerveModuleIo createSwerveModule(String name, int steerMotorId, int driveMotorId, - int canCoderId, - Rotation2d canCoderOffset, Translation2d locationOffset) { + @Override + public void periodic() { + super.periodic(); + Logger.recordOutput(getOutputLogPath("CAN Utilization"), CANIVORE_BUS.getStatus().BusUtilization); + } - CANcoder canCoder = new CANcoder(canCoderId, CANIVORE_BUS); - TalonFX steerMotor = new TalonFX(steerMotorId, CANIVORE_BUS); - TalonFX driveMotor = new TalonFX(driveMotorId, CANIVORE_BUS); + private static SwerveModuleIo createSwerveModule(String name, int steerMotorId, int driveMotorId, + int canCoderId, + Rotation2d canCoderOffset, Translation2d locationOffset) { - Slot0Configs steerMotorPid = new Slot0Configs().withKP(80); - Slot0Configs driveMotorPid = new Slot0Configs().withKV(12 / (6380.0 / 60)); // TODO: add the kd + CANcoder canCoder = new CANcoder(canCoderId, CANIVORE_BUS); + TalonFX steerMotor = new TalonFX(steerMotorId, CANIVORE_BUS); + TalonFX driveMotor = new TalonFX(driveMotorId, CANIVORE_BUS); - return new SdsSwerveModuleIo(name, locationOffset, ModuleType.MK4i_L3, steerMotor, steerMotorPid, - driveMotor, - driveMotorPid, - canCoder, canCoderOffset); - } + Slot0Configs steerMotorPid = new Slot0Configs().withKP(80); + Slot0Configs driveMotorPid = new Slot0Configs().withKV(12 / (6380.0 / 60)); // TODO: add the kd - private static GyroIo createGyro() { - return new Pigeon2Io("Gyro", new Pigeon2(13, CANIVORE_BUS)); - } + return new SdsSwerveModuleIo(name, locationOffset, ModuleType.MK4i_L3, steerMotor, steerMotorPid, + driveMotor, + driveMotorPid, + canCoder, canCoderOffset); + } - private static SwerveModuleIo[] createModules() { //TODO: calibrate the cancoder offset - double swerveModuleX = Units.inchesToMeters(10.875); - double swerveModuleY = Units.inchesToMeters(10.875); - SwerveModuleIo frontLeft = createSwerveModule("frontLeft", 1, 3, 2, Rotation2d.fromRadians(-0.904), - new Translation2d(swerveModuleX, swerveModuleY)); - SwerveModuleIo frontRight = createSwerveModule("frontRight", 4, 6, 5, Rotation2d.fromRadians(1.729), - new Translation2d(swerveModuleX, -swerveModuleY)); - SwerveModuleIo rearLeft = createSwerveModule("rearLeft", 10, 12, 11, Rotation2d.fromRadians(-3.077), - new Translation2d(-swerveModuleX, swerveModuleY)); - SwerveModuleIo rearRight = createSwerveModule("rearRight", 7, 9, 8, Rotation2d.fromRadians(0.873), - new Translation2d(-swerveModuleX, -swerveModuleY)); - return new SwerveModuleIo[] { frontLeft, frontRight, rearLeft, rearRight }; - } + private static GyroIo createGyro() { + return new Pigeon2Io("Gyro", new Pigeon2(13, CANIVORE_BUS)); + } + + private static SwerveModuleIo[] createModules() { // TODO: calibrate the cancoder offset + double swerveModuleX = Units.inchesToMeters(10.875); + double swerveModuleY = Units.inchesToMeters(10.875); + SwerveModuleIo frontLeft = createSwerveModule("frontLeft", 1, 3, 2, Rotation2d.fromRadians(-0.904), + new Translation2d(swerveModuleX, swerveModuleY)); + SwerveModuleIo frontRight = createSwerveModule("frontRight", 4, 6, 5, Rotation2d.fromRadians(1.729), + new Translation2d(swerveModuleX, -swerveModuleY)); + SwerveModuleIo rearLeft = createSwerveModule("rearLeft", 10, 12, 11, Rotation2d.fromRadians(-3.077), + new Translation2d(-swerveModuleX, swerveModuleY)); + SwerveModuleIo rearRight = createSwerveModule("rearRight", 7, 9, 8, Rotation2d.fromRadians(0.873), + new Translation2d(-swerveModuleX, -swerveModuleY)); + return new SwerveModuleIo[] { frontLeft, frontRight, rearLeft, rearRight }; + } } From 1ba795086840f7d48683bdc2bf442da41314e8c6 Mon Sep 17 00:00:00 2001 From: SWDevilTech Date: Wed, 28 Jan 2026 20:02:41 -0500 Subject: [PATCH 2/2] Added back status refresh all, as it was causing swerve drive issues --- src/main/java/frc/lib/swerve/Pigeon2Io.java | 1 + src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/main/java/frc/lib/swerve/Pigeon2Io.java b/src/main/java/frc/lib/swerve/Pigeon2Io.java index 489f502..d3ec90b 100644 --- a/src/main/java/frc/lib/swerve/Pigeon2Io.java +++ b/src/main/java/frc/lib/swerve/Pigeon2Io.java @@ -30,6 +30,7 @@ public Pigeon2Io(String ioName, Pigeon2 gyro) { @Override protected void updateInputs(GyroInputs inputs) { + StatusSignal.refreshAll(roll, pitch, yaw); inputs.pitch = Rotation2d.fromDegrees(pitch.getValueAsDouble()); inputs.roll = Rotation2d.fromDegrees(roll.getValueAsDouble()); inputs.yaw = Rotation2d.fromDegrees(yaw.getValueAsDouble()); diff --git a/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java b/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java index 9d442c6..272f41b 100644 --- a/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java +++ b/src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java @@ -130,6 +130,8 @@ public void setAngle(Rotation2d angle) { @Override protected void updateInputs(SwerveInputs inputs) { + StatusSignal.refreshAll(driveMotorVelocity, canCoderAbsolutePosition, driveMotorPosition, steerMotorTemperature, + driveMotorTemperature, steerMotorCurrent, driveMotorCurrent); inputs.speed = driveMotorVelocity.getValueAsDouble() / driveGearRatio.driveRatio * (2 * WHEEL_RADIUS * Math.PI); inputs.angle = Rotation2d.fromRotations(canCoderAbsolutePosition.getValueAsDouble()).minus(cancoderOffset); inputs.distance = driveMotorPosition.getValueAsDouble() / driveGearRatio.driveRatio