diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml new file mode 100644 index 000000000..21ee0a051 --- /dev/null +++ b/.run/Build Robot.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index c7fd0cb88..6087530e0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -15,5 +15,12 @@ "coverage-gutters.coverageFileNames": [ "jacocoTestReport.xml" ], - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m", + "files.watcherExclude": { + "**/target": true + }, + "workbench.colorCustomizations": { + "minimap.background": "#00000000", + "scrollbar.shadow": "#00000000" + } } diff --git a/src/main/java/edu/wpi/first/wpilibj/MockDigitalInput.java b/src/main/java/edu/wpi/first/wpilibj/MockDigitalInput.java index 2236f7ca1..d442f1164 100644 --- a/src/main/java/edu/wpi/first/wpilibj/MockDigitalInput.java +++ b/src/main/java/edu/wpi/first/wpilibj/MockDigitalInput.java @@ -6,6 +6,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XDigitalInputs; import xbot.common.controls.sensors.XDigitalInput; import xbot.common.injection.DevicePolice; @@ -29,8 +30,9 @@ public abstract MockDigitalInput create( public MockDigitalInput( @Assisted("info") DeviceInfo info, @Assisted("owningSystemPrefix") String owningSystemPrefix, - DevicePolice police) { - super(police, info, owningSystemPrefix); + DevicePolice police, + DataFrameRegistry dataFrameRegistry) { + super(police, info, owningSystemPrefix, dataFrameRegistry); this.channel = info.channel; } diff --git a/src/main/java/edu/wpi/first/wpilibj/MockServo.java b/src/main/java/edu/wpi/first/wpilibj/MockServo.java index c5658287d..ab41738d1 100644 --- a/src/main/java/edu/wpi/first/wpilibj/MockServo.java +++ b/src/main/java/edu/wpi/first/wpilibj/MockServo.java @@ -3,7 +3,7 @@ import dagger.assisted.Assisted; import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; - +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.actuators.XServo; import xbot.common.injection.DevicePolice; @@ -17,8 +17,8 @@ public abstract static class MockServoFactory implements XServoFactory { } @AssistedInject - public MockServo(@Assisted("channel") int channel, @Assisted("name") String name, DevicePolice police) { - super(channel, name, police); + public MockServo(@Assisted("channel") int channel, @Assisted("name") String name, DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(channel, name, police, dataFrameRegistry); } @Override diff --git a/src/main/java/xbot/common/command/BaseRobot.java b/src/main/java/xbot/common/command/BaseRobot.java index 97caa33b7..8dc606bc0 100644 --- a/src/main/java/xbot/common/command/BaseRobot.java +++ b/src/main/java/xbot/common/command/BaseRobot.java @@ -13,7 +13,6 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -49,9 +48,10 @@ public abstract class BaseRobot extends LoggedRobot { protected AutonomousCommandSelector autonomousCommandSelector; protected WebotsClient webots; + protected DevicePolice devicePolice; protected SimulationPayloadDistributor simulationPayloadDistributor; - + protected DataFrameRegistry deviceDataFrameRegistry; protected List dataFrameRefreshables = new ArrayList<>(); boolean forceWebots = true; // TODO: figure out a better way to swap between simulation and replay. @@ -124,6 +124,8 @@ public void robotInit() { PropertyFactory pf = injectorComponent.propertyFactory(); devicePolice = injectorComponent.devicePolice(); + deviceDataFrameRegistry = injectorComponent.dataFrameRegistry(); + if (forceWebots) { simulationPayloadDistributor = injectorComponent.simulationPayloadDistributor(); } @@ -261,9 +263,7 @@ protected void sharedPeriodic() { // Then, refresh any Subsystem or other components that implement DataFrameRefreshable. double dataFrameStart = getPerformanceTimestampInMs(); - for (DataFrameRefreshable refreshable : dataFrameRefreshables) { - refreshable.refreshDataFrame(); - } + refreshAllDataFrames(); double dataFrameEnd = getPerformanceTimestampInMs(); Logger.recordOutput("RefreshDevicesMs", dataFrameEnd - dataFrameStart); @@ -275,6 +275,15 @@ protected void sharedPeriodic() { outsidePeriodicStart = getPerformanceTimestampInMs(); } + public void refreshAllDataFrames() { + // all devices are refreshed first, order doesn't matter + deviceDataFrameRegistry.refreshAll(); + + // other things like subsystems refreshed here, they should be done in order + for (DataFrameRefreshable refreshable : dataFrameRefreshables) { + refreshable.refreshDataFrame(); + } + } @Override public void simulationInit() { diff --git a/src/main/java/xbot/common/command/BaseSubsystem.java b/src/main/java/xbot/common/command/BaseSubsystem.java index 74d4d3bea..78ff5fedc 100644 --- a/src/main/java/xbot/common/command/BaseSubsystem.java +++ b/src/main/java/xbot/common/command/BaseSubsystem.java @@ -1,23 +1,19 @@ package xbot.common.command; -import java.util.ArrayList; -import java.util.List; - import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; import edu.wpi.first.wpilibj2.command.SubsystemBase; import xbot.common.advantage.AKitLogger; -import xbot.common.advantage.DataFrameRefreshable; import xbot.common.properties.IPropertySupport; -public abstract class BaseSubsystem extends SubsystemBase implements IPropertySupport, DataFrameRefreshable { +public abstract class BaseSubsystem extends SubsystemBase implements IPropertySupport { protected final Logger log; protected final AKitLogger aKitLog; - protected final List dataFrameRefreshables = new ArrayList<>(); public BaseSubsystem() { + super(); log = LogManager.getLogger(this.getName()); aKitLog = new AKitLogger(this); } @@ -26,10 +22,6 @@ public String getPrefix() { return this.getName() + "/"; } - protected void registerDataFrameRefreshable(DataFrameRefreshable refreshable) { - dataFrameRefreshables.add(refreshable); - } - /** * This method is called on each {@link edu.wpi.first.wpilibj2.command.CommandScheduler} loop. * @apiNote Subsystem periodic() methods are not executed in a predictable order. @@ -40,11 +32,4 @@ protected void registerDataFrameRefreshable(DataFrameRefreshable refreshable) { public void periodic() { super.periodic(); } - - @Override - public void refreshDataFrame() { - for (DataFrameRefreshable refreshable : dataFrameRefreshables) { - refreshable.refreshDataFrame(); - } - } } diff --git a/src/main/java/xbot/common/command/DataFrameRegistry.java b/src/main/java/xbot/common/command/DataFrameRegistry.java new file mode 100644 index 000000000..82c2f9050 --- /dev/null +++ b/src/main/java/xbot/common/command/DataFrameRegistry.java @@ -0,0 +1,33 @@ +package xbot.common.command; + +import java.util.ArrayList; +import java.util.List; + +import javax.inject.Inject; +import javax.inject.Singleton; + +import xbot.common.advantage.DataFrameRefreshable; + +/** + * A registry for components that implement {@link DataFrameRefreshable}, which allows them to be refreshed on a regular basis by the {@link BaseRobot}. + */ +@Singleton +public final class DataFrameRegistry { + private final List refreshables = new ArrayList<>(); + + @Inject + public DataFrameRegistry() {} + + public void register(DataFrameRefreshable refreshable) { + if (refreshables.contains(refreshable)) { + return; + } + refreshables.add(refreshable); + } + + public void refreshAll() { + for (DataFrameRefreshable refreshable : refreshables) { + refreshable.refreshDataFrame(); + } + } +} \ No newline at end of file diff --git a/src/main/java/xbot/common/controls/actuators/XCANMotorController.java b/src/main/java/xbot/common/controls/actuators/XCANMotorController.java index ad47a9afc..940511d93 100644 --- a/src/main/java/xbot/common/controls/actuators/XCANMotorController.java +++ b/src/main/java/xbot/common/controls/actuators/XCANMotorController.java @@ -21,6 +21,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XCANMotorControllerInputs; import xbot.common.controls.io_inputs.XCANMotorControllerInputsAutoLogged; import xbot.common.injection.DevicePolice; @@ -135,7 +136,8 @@ protected XCANMotorController( PropertyFactory propertyFactory, DevicePolice police, String pidPropertyPrefix, - XCANMotorControllerPIDProperties defaultPIDProperties + XCANMotorControllerPIDProperties defaultPIDProperties, + DataFrameRegistry dataFrameRegistry ) { this.busId = info.busId(); this.deviceId = info.deviceId(); @@ -189,6 +191,8 @@ protected XCANMotorController( } this.propertyFactory.setPrefix(this.defaultPropertyPrefix); } + + dataFrameRegistry.register(this); } public abstract void setConfiguration(CANMotorControllerOutputConfig outputConfig); diff --git a/src/main/java/xbot/common/controls/actuators/XServo.java b/src/main/java/xbot/common/controls/actuators/XServo.java index b085ad7e3..a6f4348b5 100644 --- a/src/main/java/xbot/common/controls/actuators/XServo.java +++ b/src/main/java/xbot/common/controls/actuators/XServo.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.XBaseIO; import xbot.common.injection.DevicePolice; import xbot.common.injection.DevicePolice.DeviceType; @@ -32,10 +33,11 @@ default XServo create(int channel) { * @param name The name of the servo for logging. * @param police The device police to register the servo with. */ - protected XServo(int channel, String name, DevicePolice police) { + protected XServo(int channel, String name, DevicePolice police, DataFrameRegistry dataFrameRegistry) { this.channel = channel; this.name = name; police.registerDevice(DeviceType.PWM, channel, this); + dataFrameRegistry.register(this); } /** diff --git a/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java b/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java index e3e1d5100..3beb1d55b 100644 --- a/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java +++ b/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java @@ -16,6 +16,7 @@ import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; import xbot.common.controls.io_inputs.XCANMotorControllerInputs; @@ -74,9 +75,10 @@ public MockCANMotorController( PropertyFactory propertyFactory, DevicePolice police, @Assisted("pidPropertyPrefix") String pidPropertyPrefix, - @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties + @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties, + DataFrameRegistry dataFrameRegistry ) { - super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties); + super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties, dataFrameRegistry); } @Override diff --git a/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANSparkMaxWpiAdapter.java b/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANSparkMaxWpiAdapter.java index b23ff308d..efc0060f7 100644 --- a/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANSparkMaxWpiAdapter.java +++ b/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANSparkMaxWpiAdapter.java @@ -23,6 +23,7 @@ import edu.wpi.first.units.measure.Voltage; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; import xbot.common.controls.io_inputs.XCANMotorControllerInputs; @@ -69,9 +70,10 @@ public CANSparkMaxWpiAdapter( DevicePolice police, RobotAssertionManager assertionManager, @Assisted("pidPropertyPrefix") String pidPropertyPrefix, - @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties + @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties, + DataFrameRegistry dataFrameRegistry ) { - super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties); + super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties, dataFrameRegistry); this.internalSparkMax = new SparkMax(info.deviceId(), SparkLowLevel.MotorType.kBrushless); this.assertionManager = assertionManager; diff --git a/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANTalonFxWpiAdapter.java b/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANTalonFxWpiAdapter.java index 3ca168d19..140ea8a50 100644 --- a/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANTalonFxWpiAdapter.java +++ b/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANTalonFxWpiAdapter.java @@ -30,6 +30,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import org.apache.logging.log4j.LogManager; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; import xbot.common.controls.io_inputs.XCANMotorControllerInputs; @@ -77,9 +78,10 @@ public CANTalonFxWpiAdapter( PropertyFactory propertyFactory, DevicePolice police, @Assisted("pidPropertyPrefix") String pidPropertyPrefix, - @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties + @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties, + DataFrameRegistry dataFrameRegistry ) { - super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties); + super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties, dataFrameRegistry); this.internalTalonFx = new TalonFX(info.deviceId(), info.busId().toPhoenixCANBus()); this.rotorPositionSignal = this.internalTalonFx.getRotorPosition(false); diff --git a/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANVictorSPXWpiAdapter.java b/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANVictorSPXWpiAdapter.java index 3a6649bb7..d3c7e2905 100644 --- a/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANVictorSPXWpiAdapter.java +++ b/src/main/java/xbot/common/controls/actuators/wpi_adapters/CANVictorSPXWpiAdapter.java @@ -15,6 +15,7 @@ import edu.wpi.first.units.measure.Voltage; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; import xbot.common.controls.io_inputs.XCANMotorControllerInputs; @@ -57,9 +58,10 @@ public CANVictorSPXWpiAdapter( DevicePolice police, RobotAssertionManager assertionManager, @Assisted("pidPropertyPrefix") String pidPropertyPrefix, - @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties + @Assisted("defaultPIDProperties") XCANMotorControllerPIDProperties defaultPIDProperties, + DataFrameRegistry dataFrameRegistry ) { - super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties); + super(info, owningSystemPrefix, propertyFactory, police, pidPropertyPrefix, defaultPIDProperties, dataFrameRegistry); this.internalVictor = new VictorSPX(info.deviceId()); this.assertionManager = assertionManager; diff --git a/src/main/java/xbot/common/controls/actuators/wpi_adapters/ServoWPIAdapter.java b/src/main/java/xbot/common/controls/actuators/wpi_adapters/ServoWPIAdapter.java index fba39dcbe..70a882d2a 100644 --- a/src/main/java/xbot/common/controls/actuators/wpi_adapters/ServoWPIAdapter.java +++ b/src/main/java/xbot/common/controls/actuators/wpi_adapters/ServoWPIAdapter.java @@ -1,5 +1,6 @@ package xbot.common.controls.actuators.wpi_adapters; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.actuators.XServo; import xbot.common.injection.DevicePolice; import dagger.assisted.Assisted; @@ -18,8 +19,8 @@ public abstract static class ServoWPIAdapterFactory implements XServoFactory { } @AssistedInject - public ServoWPIAdapter(@Assisted("channel") int channel, @Assisted("name") String name, DevicePolice police) { - super(channel, name, police); + public ServoWPIAdapter(@Assisted("channel") int channel, @Assisted("name") String name, DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(channel, name, police, dataFrameRegistry); this.servo = new Servo(channel); } diff --git a/src/main/java/xbot/common/controls/sensors/XAbsoluteEncoder.java b/src/main/java/xbot/common/controls/sensors/XAbsoluteEncoder.java index 920ef7f70..f7694daf1 100644 --- a/src/main/java/xbot/common/controls/sensors/XAbsoluteEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/XAbsoluteEncoder.java @@ -5,6 +5,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputsAutoLogged; import xbot.common.injection.electrical_contract.DeviceInfo; @@ -19,9 +20,10 @@ public interface XAbsoluteEncoderFactory { XAbsoluteEncoder create(DeviceInfo deviceInfo, String owningSystemPrefix); } - public XAbsoluteEncoder(DeviceInfo info) { + public XAbsoluteEncoder(DeviceInfo info, DataFrameRegistry dataFrameRegistry) { inputs = new XAbsoluteEncoderInputsAutoLogged(); this.info = info; + dataFrameRegistry.register(this); } public abstract int getDeviceId(); diff --git a/src/main/java/xbot/common/controls/sensors/XCANCoder.java b/src/main/java/xbot/common/controls/sensors/XCANCoder.java index e61dbf165..6cc18e6e4 100644 --- a/src/main/java/xbot/common/controls/sensors/XCANCoder.java +++ b/src/main/java/xbot/common/controls/sensors/XCANCoder.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.StatusCode; import edu.wpi.first.wpilibj.Alert; import org.littletonrobotics.junction.Logger; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XCANCoderInputs; import xbot.common.controls.io_inputs.XCANCoderInputsAutoLogged; import xbot.common.injection.electrical_contract.DeviceInfo; @@ -19,8 +20,8 @@ public interface XCANCoderFactory extends XAbsoluteEncoderFactory { XCANCoder create(DeviceInfo deviceInfo, String owningSystemPrefix); } - public XCANCoder(DeviceInfo info) { - super(info); + public XCANCoder(DeviceInfo info, DataFrameRegistry dataFrameRegistry) { + super(info, dataFrameRegistry); inputs = new XCANCoderInputsAutoLogged(); unhealthyAlert = new Alert(AlertGroups.DEVICE_HEALTH, "CANCoder " + info.channel + " on CAN bus " + info.canBusId + " is unhealthy", Alert.AlertType.kError); diff --git a/src/main/java/xbot/common/controls/sensors/XDigitalInput.java b/src/main/java/xbot/common/controls/sensors/XDigitalInput.java index 20f95ec7c..cd5f9ae55 100644 --- a/src/main/java/xbot/common/controls/sensors/XDigitalInput.java +++ b/src/main/java/xbot/common/controls/sensors/XDigitalInput.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.XBaseIO; import xbot.common.controls.io_inputs.XDigitalInputs; import xbot.common.controls.io_inputs.XDigitalInputsAutoLogged; @@ -21,12 +22,13 @@ public interface XDigitalInputFactory { XDigitalInput create(DeviceInfo info, String owningSystemPrefix); } - public XDigitalInput(DevicePolice police, DeviceInfo info, String owningSystemPrefix) { + public XDigitalInput(DevicePolice police, DeviceInfo info, String owningSystemPrefix, DataFrameRegistry dataFrameRegistry) { police.registerDevice(DeviceType.DigitalIO, info.channel, this); inputs = new XDigitalInputsAutoLogged(); this.info = info; akitName = owningSystemPrefix + info.name + "DigitalInput"; this.setInverted(info.inverted); + dataFrameRegistry.register(this); } public boolean get() { diff --git a/src/main/java/xbot/common/controls/sensors/XDutyCycleEncoder.java b/src/main/java/xbot/common/controls/sensors/XDutyCycleEncoder.java index b5c272d7c..085f496f3 100644 --- a/src/main/java/xbot/common/controls/sensors/XDutyCycleEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/XDutyCycleEncoder.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.XBaseIO; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.io_inputs.XDutyCycleEncoderInputs; @@ -24,13 +25,14 @@ public interface XDutyCycleEncoderFactory { XDutyCycleEncoder create(DeviceInfo deviceInfo); } - public XDutyCycleEncoder(DeviceInfo info, DevicePolice police) { + public XDutyCycleEncoder(DeviceInfo info, DevicePolice police, DataFrameRegistry dataFrameRegistry) { this.info = info; this.channel = info.channel; police.registerDevice(DevicePolice.DeviceType.DigitalIO, channel, this); setInverted(info.inverted); inputs = new XDutyCycleEncoderInputsAutoLogged(); + dataFrameRegistry.register(this); } /** diff --git a/src/main/java/xbot/common/controls/sensors/XEncoder.java b/src/main/java/xbot/common/controls/sensors/XEncoder.java index cbad0f690..42be3ecce 100644 --- a/src/main/java/xbot/common/controls/sensors/XEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/XEncoder.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XEncoderInputs; import xbot.common.controls.io_inputs.XEncoderInputsAutoLogged; import xbot.common.injection.DevicePolice; @@ -36,7 +37,8 @@ public XEncoder( double defaultDistancePerPulse, String owningSystemPrefix, PropertyFactory propMan, - DevicePolice police) { + DevicePolice police, + DataFrameRegistry dataFrameRegistry) { propMan.setPrefix(name); distancePerPulse = propMan.createPersistentProperty("DistancePerPulse", defaultDistancePerPulse); setDistancePerPulseSupplier(() -> distancePerPulse.get()); @@ -45,6 +47,8 @@ public XEncoder( akitName = owningSystemPrefix + name + "Encoder"; inputs = new XEncoderInputsAutoLogged(); + + dataFrameRegistry.register(this); } public void setDistancePerPulseSupplier(DoubleSupplier supplier) { diff --git a/src/main/java/xbot/common/controls/sensors/XLaserCAN.java b/src/main/java/xbot/common/controls/sensors/XLaserCAN.java index 813a0e600..bcf681718 100644 --- a/src/main/java/xbot/common/controls/sensors/XLaserCAN.java +++ b/src/main/java/xbot/common/controls/sensors/XLaserCAN.java @@ -4,6 +4,7 @@ import edu.wpi.first.units.measure.Time; import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.LaserCANInputs; import xbot.common.controls.io_inputs.LaserCANInputsAutoLogged; import xbot.common.injection.DevicePolice; @@ -22,11 +23,12 @@ public interface XLaserCANFactory { XLaserCAN create(DeviceInfo info, String owningSystemPrefix); } - public XLaserCAN(DevicePolice police, DeviceInfo info, String owningSystemPrefix) { + public XLaserCAN(DevicePolice police, DeviceInfo info, String owningSystemPrefix, DataFrameRegistry dataFrameRegistry) { this.info = info; police.registerDevice(DeviceType.CAN, info.channel, this); akitName = owningSystemPrefix + info.name + "LaserCAN"; inputs = new LaserCANInputsAutoLogged(); + dataFrameRegistry.register(this); } public Optional getDistance() { diff --git a/src/main/java/xbot/common/controls/sensors/XSparkAbsoluteEncoder.java b/src/main/java/xbot/common/controls/sensors/XSparkAbsoluteEncoder.java index 611c00228..56b45b04a 100644 --- a/src/main/java/xbot/common/controls/sensors/XSparkAbsoluteEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/XSparkAbsoluteEncoder.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputsAutoLogged; @@ -14,10 +15,11 @@ public abstract class XSparkAbsoluteEncoder implements DataFrameRefreshable { String nameWithPrefix; - public XSparkAbsoluteEncoder(String nameWithPrefix, boolean inverted) { + public XSparkAbsoluteEncoder(String nameWithPrefix, boolean inverted, DataFrameRegistry dataFrameRegistry) { inputs = new XAbsoluteEncoderInputsAutoLogged(); this.nameWithPrefix = nameWithPrefix; this.inverted = inverted; + dataFrameRegistry.register(this); } public Angle getPosition() { diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockAbsoluteEncoder.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockAbsoluteEncoder.java index fc73b213d..6b412cec5 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockAbsoluteEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockAbsoluteEncoder.java @@ -9,6 +9,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.sensors.XAbsoluteEncoder; import xbot.common.injection.DevicePolice; @@ -42,8 +43,8 @@ public abstract MockAbsoluteEncoder create( @AssistedInject public MockAbsoluteEncoder(@Assisted("deviceInfo") DeviceInfo deviceInfo, @Assisted("owningSystemPrefix") String owningSystemPrefix, - DevicePolice police, PropertyFactory pf) { - super(deviceInfo); + DevicePolice police, PropertyFactory pf, DataFrameRegistry dataFrameRegistry) { + super(deviceInfo, dataFrameRegistry); pf.setPrefix(owningSystemPrefix); this.deviceId = deviceInfo.channel; diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java index f09f7d109..b6708e011 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java @@ -12,6 +12,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.io_inputs.XCANCoderInputs; import xbot.common.controls.sensors.XCANCoder; @@ -47,8 +48,8 @@ public abstract MockCANCoder create( @AssistedInject public MockCANCoder(@Assisted("deviceInfo") DeviceInfo deviceInfo, @Assisted("owningSystemPrefix") String owningSystemPrefix, - DevicePolice police, PropertyFactory pf) { - super(deviceInfo); + DevicePolice police, PropertyFactory pf, DataFrameRegistry dataFrameRegistry) { + super(deviceInfo, dataFrameRegistry); pf.setPrefix(owningSystemPrefix); this.deviceId = deviceInfo.channel; diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockDutyCycleEncoder.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockDutyCycleEncoder.java index 2e8e6db28..1c192a632 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockDutyCycleEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockDutyCycleEncoder.java @@ -4,6 +4,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; import edu.wpi.first.wpilibj.MockAnalogInput; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XDutyCycleEncoderInputs; import xbot.common.controls.sensors.XAnalogInput; import xbot.common.controls.sensors.XDutyCycleEncoder; @@ -20,8 +21,8 @@ public abstract static class MockDutyCycleEncoderFactory implements XDutyCycleEn } @AssistedInject - public MockDutyCycleEncoder(@Assisted("info") DeviceInfo info, DevicePolice police) { - super(info, police); + public MockDutyCycleEncoder(@Assisted("info") DeviceInfo info, DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(info, police, dataFrameRegistry); } public void setRawPosition(double rawPosition) { diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockEncoder.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockEncoder.java index e4810652f..85388a514 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockEncoder.java @@ -6,6 +6,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XEncoderInputs; import xbot.common.controls.sensors.XEncoder; import xbot.common.injection.DevicePolice; @@ -31,8 +32,8 @@ public abstract MockEncoder create( public MockEncoder(@Assisted("name") String name, @Assisted("aChannel") int aChannel, @Assisted("bChannel") int bChannel, @Assisted("defaultDistancePerPulse") double defaultDistancePerPulse, @Assisted("owningSystemPrefix") String owningSystemPrefix, - PropertyFactory propMan, DevicePolice police) { - super(name, aChannel, bChannel, defaultDistancePerPulse, owningSystemPrefix, propMan, police); + PropertyFactory propMan, DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(name, aChannel, bChannel, defaultDistancePerPulse, owningSystemPrefix, propMan, police, dataFrameRegistry); } public void setDistance(double distance) { diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockGyro.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockGyro.java index 901894333..e6e0bf834 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockGyro.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockGyro.java @@ -11,6 +11,7 @@ import dagger.assisted.AssistedInject; import xbot.common.controls.sensors.XGyro; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XGyroIoInputs; import xbot.common.injection.DevicePolice; import xbot.common.injection.DevicePolice.DeviceType; @@ -40,9 +41,10 @@ public abstract static class MockGyroFactory extends XGyroFactory { } @AssistedInject - public MockGyro(DevicePolice police, @Assisted IMUInfo imuInfo) { + public MockGyro(DevicePolice police, DataFrameRegistry dataFrameRegistry, @Assisted IMUInfo imuInfo) { super(IMUInfo.createMock(imuInfo)); police.registerDevice(DeviceType.IMU, imuInfo.deviceId(), this); + dataFrameRegistry.register(this); } public boolean isConnected() { diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockLaserCAN.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockLaserCAN.java index 408523b0e..33ed01132 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockLaserCAN.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockLaserCAN.java @@ -3,6 +3,7 @@ import dagger.assisted.Assisted; import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.LaserCANInputs; import xbot.common.controls.sensors.XLaserCAN; import xbot.common.controls.sensors.XTimer; @@ -27,8 +28,8 @@ public abstract MockLaserCAN create( public MockLaserCAN( @Assisted("info") DeviceInfo info, @Assisted("owningSystemPrefix") String owningSystemPrefix, - DevicePolice police) { - super(police, info, owningSystemPrefix); + DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(police, info, owningSystemPrefix, dataFrameRegistry); } public void setDistance(double distanceMeters) { diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockSparkAbsoluteEncoder.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockSparkAbsoluteEncoder.java index caa0f1c14..3c4bebc72 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockSparkAbsoluteEncoder.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockSparkAbsoluteEncoder.java @@ -1,13 +1,14 @@ package xbot.common.controls.sensors.mock_adapters; import edu.wpi.first.units.measure.Angle; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.sensors.XSparkAbsoluteEncoder; public class MockSparkAbsoluteEncoder extends XSparkAbsoluteEncoder { - public MockSparkAbsoluteEncoder(String nameWithPrefix, boolean inverted) { - super(nameWithPrefix, inverted); + public MockSparkAbsoluteEncoder(String nameWithPrefix, boolean inverted, DataFrameRegistry dataFrameRegistry) { + super(nameWithPrefix, inverted, dataFrameRegistry); } public void setMockPosition(Angle position) { diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/CANCoderAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/CANCoderAdapter.java index 35cfa5b35..b5ebd028f 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/CANCoderAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/CANCoderAdapter.java @@ -16,6 +16,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XAbsoluteEncoderInputs; import xbot.common.controls.io_inputs.XCANCoderInputs; import xbot.common.controls.sensors.XCANCoder; @@ -53,8 +54,8 @@ public abstract CANCoderAdapter create( @AssistedInject public CANCoderAdapter(@Assisted("deviceInfo") DeviceInfo deviceInfo, @Assisted("owningSystemPrefix") String owningSystemPrefix, - DevicePolice police, PropertyFactory pf) { - super(deviceInfo); + DevicePolice police, PropertyFactory pf, DataFrameRegistry dataFrameRegistry) { + super(deviceInfo, dataFrameRegistry); pf.setPrefix(owningSystemPrefix); this.inverted = deviceInfo.inverted; diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/DigitalInputWPIAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/DigitalInputWPIAdapter.java index 05173ed0f..6d35d5b4a 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/DigitalInputWPIAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/DigitalInputWPIAdapter.java @@ -5,6 +5,7 @@ import dagger.assisted.AssistedInject; import edu.wpi.first.wpilibj.DigitalInput; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XDigitalInputs; import xbot.common.controls.sensors.XDigitalInput; import xbot.common.injection.DevicePolice; @@ -30,8 +31,8 @@ public abstract static class DigitalInputWPIAdapterFactory implements XDigitalIn public DigitalInputWPIAdapter( @Assisted("info") DeviceInfo info, @Assisted("owningSystemPrefix")String owningSystemPrefix, - DevicePolice police) { - super(police, info, owningSystemPrefix); + DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(police, info, owningSystemPrefix, dataFrameRegistry); adapter = new DigitalInput(info.channel); } diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/DutyCycleEncoderWpiAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/DutyCycleEncoderWpiAdapter.java index fda2dc9a0..313e37cbc 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/DutyCycleEncoderWpiAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/DutyCycleEncoderWpiAdapter.java @@ -4,6 +4,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XDutyCycleEncoderInputs; import xbot.common.controls.sensors.XDutyCycleEncoder; import xbot.common.controls.sensors.mock_adapters.MockDutyCycleEncoder; @@ -20,8 +21,8 @@ public abstract static class DutyCycleEncoderWpiAdapterFactory implements XDutyC } @AssistedInject - public DutyCycleEncoderWpiAdapter(@Assisted("info") DeviceInfo info, DevicePolice police) { - super(info, police); + public DutyCycleEncoderWpiAdapter(@Assisted("info") DeviceInfo info, DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(info, police, dataFrameRegistry); internalEncoder = new DutyCycleEncoder(info.channel); } diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/EncoderWPIAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/EncoderWPIAdapter.java index 1fa9d60e6..94572ab17 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/EncoderWPIAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/EncoderWPIAdapter.java @@ -4,6 +4,7 @@ import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; import edu.wpi.first.wpilibj.Encoder; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XEncoderInputs; import xbot.common.controls.sensors.XEncoder; import xbot.common.injection.DevicePolice; @@ -31,8 +32,9 @@ public EncoderWPIAdapter( @Assisted("defaultDistancePerPulse") double defaultDistancePerPulse, @Assisted("owningSystemPrefix") String owningSystemPrefix, PropertyFactory propMan, - DevicePolice police) { - super(name, aChannel, bChannel, defaultDistancePerPulse, owningSystemPrefix, propMan, police); + DevicePolice police, + DataFrameRegistry dataFrameRegistry) { + super(name, aChannel, bChannel, defaultDistancePerPulse, owningSystemPrefix, propMan, police, dataFrameRegistry); internalEncoder = new Encoder(aChannel, bChannel); } diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/InertialMeasurementUnitAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/InertialMeasurementUnitAdapter.java index afa7f0fa2..b17ea2984 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/InertialMeasurementUnitAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/InertialMeasurementUnitAdapter.java @@ -12,6 +12,7 @@ import dagger.assisted.AssistedInject; import xbot.common.controls.sensors.XGyro; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XGyroIoInputs; import xbot.common.injection.DevicePolice; import xbot.common.injection.DevicePolice.DeviceType; @@ -33,7 +34,7 @@ public abstract static class InertialMeasurementUnitAdapterFactory extends XGyro } @AssistedInject - public InertialMeasurementUnitAdapter(DevicePolice police, @Assisted IMUInfo imuInfo) { + public InertialMeasurementUnitAdapter(DevicePolice police, DataFrameRegistry registry, @Assisted IMUInfo imuInfo) { super(imuInfo); /* Options: Port.kMXP, SPI.kMXP, I2C.kMXP or SerialPort.kUSB */ try { @@ -43,6 +44,7 @@ public InertialMeasurementUnitAdapter(DevicePolice police, @Assisted IMUInfo imu case i2c -> this.ahrs = new AHRS(AHRS.NavXComType.kI2C); default -> this.ahrs = new AHRS(AHRS.NavXComType.kMXP_SPI); } + registry.register(this); police.registerDevice(DeviceType.IMU, 1, this); log.info("AHRS successfully created"); } diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java index 753a6c061..d27b0d6d7 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java @@ -8,6 +8,7 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.Alert; +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.LaserCANInputs; import xbot.common.controls.sensors.XLaserCAN; import xbot.common.controls.sensors.XTimer; @@ -37,8 +38,8 @@ public abstract LaserCANWpiAdapter create( public LaserCANWpiAdapter( @Assisted("info") DeviceInfo info, @Assisted("owningSystemPrefix")String owningSystemPrefix, - DevicePolice police) { - super(police, info, owningSystemPrefix); + DevicePolice police, DataFrameRegistry dataFrameRegistry) { + super(police, info, owningSystemPrefix, dataFrameRegistry); healthAlert = new Alert(AlertGroups.DEVICE_HEALTH, "Failed to set LaserCAN configuration", Alert.AlertType.kError); laserCan = new LaserCan(info.channel); try { diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/Pigeon2Adapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/Pigeon2Adapter.java index ec44a8cb8..9df4503bf 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/Pigeon2Adapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/Pigeon2Adapter.java @@ -11,6 +11,8 @@ import edu.wpi.first.units.measure.LinearAcceleration; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; + +import xbot.common.command.DataFrameRegistry; import xbot.common.controls.io_inputs.XGyroIoInputs; import xbot.common.controls.sensors.XGyro; import xbot.common.injection.DevicePolice; @@ -36,10 +38,11 @@ public abstract static class Pigeon2AdapterFactory extends XGyroFactory { } @AssistedInject - public Pigeon2Adapter(DevicePolice police, @Assisted IMUInfo imuInfo) { + public Pigeon2Adapter(DevicePolice police, DataFrameRegistry registry, @Assisted IMUInfo imuInfo) { super(imuInfo); this.pigeon = new Pigeon2(imuInfo.deviceId(), imuInfo.canBusId().toPhoenixCANBus()); police.registerDevice(DevicePolice.DeviceType.CAN, imuInfo.canBusId(), imuInfo.deviceId(), this); + registry.register(this); this.yawSignal = pigeon.getYaw(); this.pitchSignal = pigeon.getPitch(); diff --git a/src/main/java/xbot/common/injection/components/BaseComponent.java b/src/main/java/xbot/common/injection/components/BaseComponent.java index 0fd19b16b..1f842229a 100644 --- a/src/main/java/xbot/common/injection/components/BaseComponent.java +++ b/src/main/java/xbot/common/injection/components/BaseComponent.java @@ -2,6 +2,7 @@ import javax.inject.Named; +import xbot.common.command.DataFrameRegistry; import xbot.common.command.SmartDashboardCommandPutter; import xbot.common.command.XScheduler; import xbot.common.controls.actuators.XCANLightController; @@ -79,6 +80,8 @@ public abstract class BaseComponent { public abstract DevicePolice devicePolice(); + public abstract DataFrameRegistry dataFrameRegistry(); + public abstract SmartDashboardCommandPutter smartDashboardCommandPutter(); public abstract XScheduler scheduler(); diff --git a/src/main/java/xbot/common/properties/IPropertySupport.java b/src/main/java/xbot/common/properties/IPropertySupport.java index cdabbcf7e..c231e9373 100644 --- a/src/main/java/xbot/common/properties/IPropertySupport.java +++ b/src/main/java/xbot/common/properties/IPropertySupport.java @@ -2,4 +2,6 @@ public interface IPropertySupport { public String getPrefix(); + +// void refreshDataFrame(); } \ No newline at end of file diff --git a/src/main/java/xbot/common/subsystems/drive/swerve/SwerveDriveSubsystem.java b/src/main/java/xbot/common/subsystems/drive/swerve/SwerveDriveSubsystem.java index 5213e59d0..5574bc417 100644 --- a/src/main/java/xbot/common/subsystems/drive/swerve/SwerveDriveSubsystem.java +++ b/src/main/java/xbot/common/subsystems/drive/swerve/SwerveDriveSubsystem.java @@ -3,6 +3,8 @@ import edu.wpi.first.units.measure.Distance; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; + +import xbot.common.advantage.DataFrameRefreshable; import xbot.common.command.BaseSimpleSetpointSubsystem; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; @@ -189,8 +191,4 @@ public void periodic() { setupStatusFramesAsNeeded(); getMotorController().ifPresent(XCANMotorController::periodic); } - - public void refreshDataFrame() { - getMotorController().ifPresent(XCANMotorController::refreshDataFrame); - } } \ No newline at end of file diff --git a/src/main/java/xbot/common/subsystems/drive/swerve/SwerveModuleSubsystem.java b/src/main/java/xbot/common/subsystems/drive/swerve/SwerveModuleSubsystem.java index 9ec0fbf1a..84b39d6e8 100644 --- a/src/main/java/xbot/common/subsystems/drive/swerve/SwerveModuleSubsystem.java +++ b/src/main/java/xbot/common/subsystems/drive/swerve/SwerveModuleSubsystem.java @@ -1,6 +1,7 @@ package xbot.common.subsystems.drive.swerve; import javax.inject.Inject; +import javax.xml.crypto.Data; import edu.wpi.first.wpilibj.Alert; import org.apache.logging.log4j.LogManager; @@ -9,6 +10,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import xbot.common.advantage.DataFrameRefreshable; import xbot.common.command.BaseSubsystem; import xbot.common.injection.electrical_contract.XSwerveDriveElectricalContract; import xbot.common.injection.swerve.SwerveInstance; @@ -23,7 +25,7 @@ import static edu.wpi.first.units.Units.Inches; @SwerveSingleton -public class SwerveModuleSubsystem extends BaseSubsystem { +public class SwerveModuleSubsystem extends BaseSubsystem implements DataFrameRefreshable { private static final Logger log = LogManager.getLogger(SwerveModuleSubsystem.class); private final String label; @@ -164,7 +166,6 @@ public void periodic() { } public void refreshDataFrame() { - getDriveSubsystem().refreshDataFrame(); getSteeringSubsystem().refreshDataFrame(); this.currentState.speedMetersPerSecond = getDriveSubsystem().getCurrentValue(); diff --git a/src/main/java/xbot/common/subsystems/drive/swerve/SwerveSteeringSubsystem.java b/src/main/java/xbot/common/subsystems/drive/swerve/SwerveSteeringSubsystem.java index 485f14d2c..d62338ea5 100644 --- a/src/main/java/xbot/common/subsystems/drive/swerve/SwerveSteeringSubsystem.java +++ b/src/main/java/xbot/common/subsystems/drive/swerve/SwerveSteeringSubsystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.MathUtil; import xbot.common.advantage.AKitLogger; +import xbot.common.advantage.DataFrameRefreshable; import xbot.common.command.BaseSimpleSetpointSubsystem; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; @@ -33,7 +34,7 @@ import static edu.wpi.first.units.Units.Rotations; @SwerveSingleton -public class SwerveSteeringSubsystem extends BaseSimpleSetpointSubsystem { +public class SwerveSteeringSubsystem extends BaseSimpleSetpointSubsystem implements DataFrameRefreshable { private static final Logger log = LogManager.getLogger(SwerveSteeringSubsystem.class); private final String label; @@ -249,9 +250,6 @@ public void periodic() { @Override public void refreshDataFrame() { - getMotorController().ifPresent(XCANMotorController::refreshDataFrame); - getEncoder().ifPresent(XCANCoder::refreshDataFrame); - // TODO: Once we've moved to an architecture where we control the order periodic() is called in // (so we can guarantee that child components, like this SwerveSteeringElement, are called before // the the parent component, like the DriveSubsystem), this will be moved to periodic. diff --git a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java index 33662e051..eb914d1d3 100644 --- a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java +++ b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java @@ -6,7 +6,6 @@ import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.math.geometry.Rotation2d; -import xbot.common.advantage.DataFrameRefreshable; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import xbot.common.command.BaseSubsystem; @@ -28,7 +27,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; -public abstract class BasePoseSubsystem extends BaseSubsystem implements DataFrameRefreshable, ISwerveAdvisorPoseSupport { +public abstract class BasePoseSubsystem extends BaseSubsystem implements ISwerveAdvisorPoseSupport { public final XGyro imu; protected double leftDriveDistance; @@ -378,11 +377,6 @@ public void periodic() { updatePose(); } - @Override - public void refreshDataFrame() { - imu.refreshDataFrame(); - } - public Pose2d getSimulatedFieldPose() { return this.getCurrentPose2d(); } diff --git a/src/main/java/xbot/common/subsystems/pose/MockBasePoseSubsystem.java b/src/main/java/xbot/common/subsystems/pose/MockBasePoseSubsystem.java index a1379e967..bdf1152fc 100644 --- a/src/main/java/xbot/common/subsystems/pose/MockBasePoseSubsystem.java +++ b/src/main/java/xbot/common/subsystems/pose/MockBasePoseSubsystem.java @@ -39,7 +39,8 @@ protected double getRightDriveDistance() { public void setDriveEncoderDistances(int left, int right) { ((MockCANMotorController)this.left).setPosition(Rotations.of(left)); ((MockCANMotorController)this.right).setPosition(Rotations.of(right)); - refreshDataFrame(); + ((MockCANMotorController)this.left).refreshDataFrame(); + ((MockCANMotorController)this.right).refreshDataFrame(); periodic(); } @@ -47,11 +48,4 @@ public void forceTotalXandY(double x, double y) { totalDistanceX = x; totalDistanceY = y; } - - @Override - public void refreshDataFrame() { - super.refreshDataFrame(); - left.refreshDataFrame(); - right.refreshDataFrame(); - } } diff --git a/src/main/java/xbot/common/subsystems/vision/AprilTagVisionCameraHelper.java b/src/main/java/xbot/common/subsystems/vision/AprilTagVisionCameraHelper.java index 484df0a3f..bd10156e7 100644 --- a/src/main/java/xbot/common/subsystems/vision/AprilTagVisionCameraHelper.java +++ b/src/main/java/xbot/common/subsystems/vision/AprilTagVisionCameraHelper.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.Alert; import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.logging.AlertGroups; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; @@ -51,10 +52,12 @@ class AprilTagVisionCameraHelper implements DataFrameRefreshable { private final List robotPosesRejected = new LinkedList<>(); private final List poseObservations = new LinkedList<>(); - public AprilTagVisionCameraHelper(String prefix, PropertyFactory pf, AprilTagVisionIO io, AprilTagFieldLayout fieldLayout, boolean useForPoseEstimates) { + public AprilTagVisionCameraHelper(String prefix, PropertyFactory pf, AprilTagVisionIO io, + AprilTagFieldLayout fieldLayout, DataFrameRegistry registry, boolean useForPoseEstimates) { this.logPath = prefix; this.io = io; this.inputs = new VisionIOInputsAutoLogged(); + registry.register(this); this.aprilTagFieldLayout = fieldLayout; this.disconnectedAlert = new Alert(AlertGroups.DEVICE_HEALTH, "Vision camera " + prefix + " is disconnected.", Alert.AlertType.kError); diff --git a/src/main/java/xbot/common/subsystems/vision/AprilTagVisionSubsystem.java b/src/main/java/xbot/common/subsystems/vision/AprilTagVisionSubsystem.java index 3fb41409e..523760fab 100644 --- a/src/main/java/xbot/common/subsystems/vision/AprilTagVisionSubsystem.java +++ b/src/main/java/xbot/common/subsystems/vision/AprilTagVisionSubsystem.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.DataFrameRegistry; import xbot.common.injection.electrical_contract.CameraInfo; import xbot.common.injection.electrical_contract.XCameraElectricalContract; import xbot.common.properties.PropertyFactory; @@ -40,7 +41,7 @@ * Based on the AdvantageKit sample implementation by team 6328. */ @Singleton -public class AprilTagVisionSubsystem extends SubsystemBase implements DataFrameRefreshable { +public class AprilTagVisionSubsystem extends SubsystemBase { private final CameraInfo[] cameras; private final AprilTagFieldLayout aprilTagFieldLayout; final AprilTagVisionIO[] io; @@ -49,7 +50,7 @@ public class AprilTagVisionSubsystem extends SubsystemBase implements DataFrameR @Inject public AprilTagVisionSubsystem(PropertyFactory pf, AprilTagFieldLayout fieldLayout, XCameraElectricalContract contract, - AprilTagVisionIOFactory visionIOFactory) { + AprilTagVisionIOFactory visionIOFactory, DataFrameRegistry registry) { this.aprilTagFieldLayout = fieldLayout; this.cameras = contract.getAprilTagCameras(); @@ -59,7 +60,7 @@ public AprilTagVisionSubsystem(PropertyFactory pf, AprilTagFieldLayout fieldLayo var cameraInfo = this.cameras[i]; io[i] = visionIOFactory.create(cameraInfo.networkTablesName(), cameraInfo.position()); cameraHelpers[i] = new AprilTagVisionCameraHelper(this.getName() + "/Cameras/" + cameraInfo.friendlyName(), - pf, io[i], fieldLayout, cameraInfo.useForPoseEstimates()); + pf, io[i], fieldLayout, registry, cameraInfo.useForPoseEstimates()); } } @@ -203,13 +204,6 @@ public List getAllPoseObservations() { return result; } - @Override - public void refreshDataFrame() { - for (int i = 0; i < cameraHelpers.length; i++) { - cameraHelpers[i].refreshDataFrame(); - } - } - @Override public void periodic() { // Loop over cameras diff --git a/src/test/java/xbot/common/subsystems/drive/HeadingAssistModuleTest.java b/src/test/java/xbot/common/subsystems/drive/HeadingAssistModuleTest.java index 43cd722da..b4fd8ba47 100644 --- a/src/test/java/xbot/common/subsystems/drive/HeadingAssistModuleTest.java +++ b/src/test/java/xbot/common/subsystems/drive/HeadingAssistModuleTest.java @@ -36,7 +36,7 @@ public void setUp() { ((MockBasePoseSubsystem)pose).setDriveMotors(left, right); ((MockTimer)getInjectorComponent().timerImplementation()).advanceTimeInSecondsBy(10); - pose.refreshDataFrame(); + getInjectorComponent().dataFrameRegistry().refreshAll(); pose.periodic(); } @@ -171,7 +171,7 @@ public void testDecay() { protected void setHeading(double heading) { ((MockGyro)pose.imu).setYaw(Degrees.of(heading)); - pose.refreshDataFrame(); + ((MockGyro)pose.imu).refreshDataFrame(); pose.periodic(); } } diff --git a/src/test/java/xbot/common/subsystems/drive/HeadingModuleTest.java b/src/test/java/xbot/common/subsystems/drive/HeadingModuleTest.java index 2bf4649a8..5099a2ad6 100644 --- a/src/test/java/xbot/common/subsystems/drive/HeadingModuleTest.java +++ b/src/test/java/xbot/common/subsystems/drive/HeadingModuleTest.java @@ -79,7 +79,7 @@ public void onTarget() { protected void setHeading(double heading) { ((MockGyro)pose.imu).setYaw(Degrees.of(heading)); - pose.refreshDataFrame(); + ((MockGyro)pose.imu).refreshDataFrame(); pose.periodic(); } } diff --git a/src/test/java/xbot/common/subsystems/pose/PoseSubsystemTest.java b/src/test/java/xbot/common/subsystems/pose/PoseSubsystemTest.java index 0dd92b6ff..84126b772 100644 --- a/src/test/java/xbot/common/subsystems/pose/PoseSubsystemTest.java +++ b/src/test/java/xbot/common/subsystems/pose/PoseSubsystemTest.java @@ -125,7 +125,8 @@ public void setPosition() { } private void refreshPoseSubsystem() { - pose.refreshDataFrame(); + // this.getInjectorComponent().dataFrameRegistry().refreshAll(); + pose.imu.refreshDataFrame(); pose.periodic(); } } diff --git a/src/test/java/xbot/common/subsystems/vision/AprilTagVisionSubsystemTest.java b/src/test/java/xbot/common/subsystems/vision/AprilTagVisionSubsystemTest.java index 77d18c6b8..0de5914ca 100644 --- a/src/test/java/xbot/common/subsystems/vision/AprilTagVisionSubsystemTest.java +++ b/src/test/java/xbot/common/subsystems/vision/AprilTagVisionSubsystemTest.java @@ -18,9 +18,11 @@ public class AprilTagVisionSubsystemTest extends BaseCommonLibTest { @Test public void testBasicOperation() { var subsystem = this.getInjectorComponent().getAprilTagVisionSubsystem(); + var registry = this.getInjectorComponent().dataFrameRegistry(); + assertEquals(1, subsystem.getCameraCount()); assertEquals(1, subsystem.io.length); - subsystem.refreshDataFrame(); + registry.refreshAll(); subsystem.periodic(); assertEquals(0, subsystem.getAllPoseObservations().size()); } @@ -28,6 +30,7 @@ public void testBasicOperation() { @Test public void testHandleResult() { var subsystem = this.getInjectorComponent().getAprilTagVisionSubsystem(); + var registry = this.getInjectorComponent().dataFrameRegistry(); var io = (MockAprilTagVisionIO) subsystem.io[0]; io.poseObservations = new AprilTagVisionIO.PoseObservation[] { @@ -42,7 +45,7 @@ public void testHandleResult() { AprilTagVisionIO.PoseObservationType.PHOTONVISION) }; io.tagIds = new int[] { 1, 2, 3 }; - subsystem.refreshDataFrame(); + registry.refreshAll(); subsystem.periodic(); assertEquals(3, subsystem.getAllPoseObservations().size()); @@ -52,7 +55,7 @@ public void testHandleResult() { io.poseObservations = new AprilTagVisionIO.PoseObservation[0]; - subsystem.refreshDataFrame(); + registry.refreshAll(); subsystem.periodic(); assertEquals(0, subsystem.getAllPoseObservations().size()); @@ -61,6 +64,7 @@ public void testHandleResult() { @Test public void testHandleTargetObservations() { var subsystem = this.getInjectorComponent().getAprilTagVisionSubsystem(); + var registry = this.getInjectorComponent().dataFrameRegistry(); var io = (MockAprilTagVisionIO) subsystem.io[0]; io.latestTargetObservation = new AprilTagVisionIO.TargetObservation(0, 1, new Rotation2d(), new Rotation2d(), @@ -74,7 +78,7 @@ public void testHandleTargetObservations() { false) }; io.tagIds = new int[] { 1, 2, 3 }; - subsystem.refreshDataFrame(); + registry.refreshAll(); subsystem.periodic(); assertEquals(1, subsystem.getBestTargetId(0).getAsInt());