Skip to content

Added CAN utilization logging and optomization #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
KE-Holtz merged 3 commits into from
Jan 30, 2026
Merged

Added CAN utilization logging and optomization #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1e90118
Added CAN utilization logging and optomization
SWDevilTech Jan 29, 2026
1ba7950
Added back status refresh all, as it was causing swerve drive issues
SWDevilTech Jan 29, 2026
614ae14
fix conflict
KE-Holtz Jan 30, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions src/main/java/frc/lib/CanRefreshRate.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
}
}
7 changes: 7 additions & 0 deletions src/main/java/frc/lib/LoggableSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,11 @@

public abstract class LoggableSubsystem extends SubsystemBase {
private final Set<LoggableComponent> children = new LinkedHashSet<>();
private final String name;

protected LoggableSubsystem(String name) {
super(name);
this.name = name;
}

protected void addChildren(String folder, LoggableComponent... children) {
Expand All @@ -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) {
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/lib/swerve/Pigeon2Io.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -22,9 +23,9 @@ 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
Expand Down
17 changes: 9 additions & 8 deletions src/main/java/frc/lib/swerve/SdsSwerveModuleIo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand Down Expand Up @@ -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
Expand All @@ -129,7 +130,7 @@ public void setAngle(Rotation2d angle) {

@Override
protected void updateInputs(SwerveInputs inputs) {
StatusSignal.refreshAll(driveMotorVelocity, canCoderAbsolutePosition, driveMotorPosition, steerMotorTemperature,
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);
Expand Down
118 changes: 65 additions & 53 deletions src/main/java/frc/robot/subsystems/SwerveDrive2026.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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 };
}
}