Skip to content

Commit

Permalink
fix+feat: signal refresher no longer static, better periodic updating…
Browse files Browse the repository at this point in the history
…, and error checking
  • Loading branch information
mimizh2418 committed Jan 29, 2024
1 parent 2c65005 commit a4285f0
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 23 deletions.
1 change: 1 addition & 0 deletions src/main/java/org/team1540/robot2024/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

/**
Expand Down
13 changes: 8 additions & 5 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -37,6 +38,8 @@ public class RobotContainer {
// Dashboard inputs
public final LoggedDashboardChooser<Command> 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);
Expand All @@ -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;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,19 +18,22 @@ public class GyroIOPigeon2 implements GyroIO {
private final StatusSignal<Double> yaw = pigeon.getYaw();
private final StatusSignal<Double> 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());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,9 @@ public class ModuleIOTalonFX implements ModuleIO {
private final StatusSignal<Double> turnAppliedVolts;
private final StatusSignal<Double> 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;
Expand Down Expand Up @@ -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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;
}
}

0 comments on commit a4285f0

Please sign in to comment.