From a4285f00a503d074881e5e3644795416ae4cb626 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sun, 28 Jan 2024 16:00:41 -0800 Subject: [PATCH] fix+feat: signal refresher no longer static, better periodic updating, and error checking --- .../java/org/team1540/robot2024/Robot.java | 1 + .../team1540/robot2024/RobotContainer.java | 13 +++-- .../subsystems/drive/GyroIOPigeon2.java | 9 ++- .../subsystems/drive/ModuleIOTalonFX.java | 10 +++- .../util/PhoenixTimeSyncSignalRefresher.java | 56 +++++++++++++++---- 5 files changed, 66 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index e4126ac9..54819bb4 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -89,6 +89,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + if (Constants.currentMode == Constants.Mode.REAL) robotContainer.timeSyncSignalRefresher.periodic(); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 85649feb..d6253137 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -14,6 +14,7 @@ import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.shooter.*; +import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -37,6 +38,8 @@ public class RobotContainer { // Dashboard inputs public final LoggedDashboardChooser autoChooser; + public final PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS); + // TODO: testing dashboard inputs, remove for comp public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000); public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000); @@ -50,11 +53,11 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations drivetrain = new Drivetrain( - new GyroIOPigeon2(), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT))); + new GyroIOPigeon2(timeSyncSignalRefresher), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), timeSyncSignalRefresher), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), timeSyncSignalRefresher), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), timeSyncSignalRefresher), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), timeSyncSignalRefresher)); shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); break; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java index 62fcef40..6e6ff339 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java @@ -18,19 +18,22 @@ public class GyroIOPigeon2 implements GyroIO { private final StatusSignal yaw = pigeon.getYaw(); private final StatusSignal yawVelocity = pigeon.getAngularVelocityZDevice(); - public GyroIOPigeon2() { + private final PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher; + + public GyroIOPigeon2(PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher) { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(100.0); yawVelocity.setUpdateFrequency(100.0); pigeon.optimizeBusUtilization(); - PhoenixTimeSyncSignalRefresher.registerSignals(yaw, yawVelocity); + this.timeSyncSignalRefresher = timeSyncSignalRefresher; + timeSyncSignalRefresher.registerSignals(yaw, yawVelocity); } @Override public void updateInputs(GyroIOInputs inputs) { - PhoenixTimeSyncSignalRefresher.refreshSignals(); + timeSyncSignalRefresher.refreshSignals(); inputs.connected = BaseStatusSignal.isAllGood(yaw, yawVelocity); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index 618a09d7..47b04e44 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java @@ -43,7 +43,9 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; - public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) { + private final PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher; + + public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw, PhoenixTimeSyncSignalRefresher timeSyncSignalRefresher) { driveTalon = hw.driveMotor; turnTalon = hw.turnMotor; cancoder = hw.cancoder; @@ -75,12 +77,14 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) { driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); - PhoenixTimeSyncSignalRefresher.registerSignals(drivePosition, turnPosition); + this.timeSyncSignalRefresher = timeSyncSignalRefresher; + + timeSyncSignalRefresher.registerSignals(drivePosition, turnPosition); } @Override public void updateInputs(ModuleIOInputs inputs) { - PhoenixTimeSyncSignalRefresher.refreshSignals(); + timeSyncSignalRefresher.refreshSignals(); BaseStatusSignal.refreshAll( driveVelocity, driveAppliedVolts, diff --git a/src/main/java/org/team1540/robot2024/util/PhoenixTimeSyncSignalRefresher.java b/src/main/java/org/team1540/robot2024/util/PhoenixTimeSyncSignalRefresher.java index 40595d27..4dec6833 100644 --- a/src/main/java/org/team1540/robot2024/util/PhoenixTimeSyncSignalRefresher.java +++ b/src/main/java/org/team1540/robot2024/util/PhoenixTimeSyncSignalRefresher.java @@ -2,6 +2,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import org.team1540.robot2024.Constants; @@ -13,26 +15,56 @@ * Helper class for refreshing CANivore TimeSynced status signals. */ public class PhoenixTimeSyncSignalRefresher { - private static BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private static final boolean isCANFD = CANBus.isNetworkFD(CAN_BUS); + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private double refreshFrequencyHz = -1; + private boolean useTimeSync; + private boolean isRefreshed; - private static double lastRefreshTimestamp = 0; + public PhoenixTimeSyncSignalRefresher(String canbus) { + useTimeSync = CANBus.isNetworkFD(canbus); + } /** - * Registers status signals to be refreshed when {@link PhoenixTimeSyncSignalRefresher#refreshSignals()}is called + * Registers status signals to be refreshed when {@link PhoenixTimeSyncSignalRefresher#refreshSignals()} is called */ - public static void registerSignals(BaseStatusSignal... newSignals) { + public void registerSignals(BaseStatusSignal... newSignals) { + if (newSignals.length < 1) return; + if (refreshFrequencyHz < 0) refreshFrequencyHz = newSignals[0].getAppliedUpdateFrequency(); + signals = Arrays.copyOf(signals, signals.length + newSignals.length); - for (int i = 0; i < newSignals.length; i++) signals[signals.length - 1 - i] = newSignals[i]; + for (int i = 0; i < newSignals.length; i++) { + // Check if we have mismatched update frequencies + if (newSignals[i].getAppliedUpdateFrequency() != refreshFrequencyHz && useTimeSync) { + useTimeSync = false; + DriverStation.reportWarning( + "Status signals received have varying update rates, TimeSynced updates are now unavailable", + true); + } + signals[signals.length - 1 - i] = newSignals[i]; + } } /** - * Refreshes all registered signals if not already done so in the current loop period + * Refreshes all registered signals if not done so since {@link PhoenixTimeSyncSignalRefresher#periodic()} call */ - public static void refreshSignals() { - if (Timer.getFPGATimestamp() - lastRefreshTimestamp < Constants.LOOP_PERIOD_SECS) return; - lastRefreshTimestamp = Timer.getFPGATimestamp(); - if (isCANFD) BaseStatusSignal.waitForAll(0.01, signals); - else BaseStatusSignal.refreshAll(signals); + public void refreshSignals() { + if (isRefreshed) return; + + // Refresh signals + StatusCode status; + if (useTimeSync) status = BaseStatusSignal.waitForAll(0.01, signals); + else status = BaseStatusSignal.refreshAll(signals); + + // Check errors + if (status == StatusCode.InvalidNetwork) + DriverStation.reportError("One or more status signals passed to signal refresher has a different CAN bus, unable to refresh", true); + if (status == StatusCode.RxTimeout) + DriverStation.reportWarning("Refreshing signals timed out, check CAN bus wiring", true); + + isRefreshed = true; + } + + public void periodic() { + isRefreshed = false; } }