diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 0c76b93d70..cd14a685bd 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -89,7 +89,7 @@ private void onPipelineIndexChange(NetworkTableEvent entryNotification) { ts.pipelineIndexPublisher.set(setIndex); // TODO: Log } - logger.debug("Successfully set pipeline index to " + newIndex); + logger.debug("Set pipeline index to " + newIndex); } private void onDriverModeChange(NetworkTableEvent entryNotification) { @@ -102,7 +102,7 @@ private void onDriverModeChange(NetworkTableEvent entryNotification) { } driverModeConsumer.accept(newDriverMode); - logger.debug("Successfully set driver mode to " + newDriverMode); + logger.debug("Set driver mode to " + newDriverMode); } private void removeEntries() { @@ -119,7 +119,7 @@ private void updateEntries() { pipelineIndexListener = new NTDataChangeListener( - ts.subTable.getInstance(), ts.pipelineIndexSubscriber, this::onPipelineIndexChange); + ts.subTable.getInstance(), ts.pipelineIndexRequestSub, this::onPipelineIndexChange); driverModeListener = new NTDataChangeListener( diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java index 799e488740..24e06cc7cd 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -17,7 +17,8 @@ package org.photonvision.common.hardware; -import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.IntegerSubscriber; import java.io.IOException; import org.photonvision.common.ProgramStatus; import org.photonvision.common.configuration.ConfigManager; @@ -47,7 +48,9 @@ public class HardwareManager { private final StatusLED statusLED; @SuppressWarnings("FieldCanBeLocal") - private final IntegerEntry ledModeEntry; + private IntegerSubscriber ledModeRequest; + + private IntegerPublisher ledModeState; @SuppressWarnings({"FieldCanBeLocal", "unused"}) private final NTDataChangeListener ledModeListener; @@ -71,6 +74,15 @@ private HardwareManager(HardwareConfig hardwareConfig, HardwareSettings hardware this.metricsManager = new MetricsManager(); this.metricsManager.setConfig(hardwareConfig); + ledModeRequest = + NetworkTablesManager.getInstance() + .kRootTable + .getIntegerTopic("ledModeRequest") + .subscribe(-1); + ledModeState = + NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledModeState").publish(); + ledModeState.set(VisionLEDMode.kDefault.value); + CustomGPIO.setConfig(hardwareConfig); if (Platform.isRaspberryPi()) { @@ -92,17 +104,15 @@ private HardwareManager(HardwareConfig hardwareConfig, HardwareSettings hardware hardwareConfig.ledPins, hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0, hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100, - pigpioSocket); + pigpioSocket, + ledModeState::set); - ledModeEntry = - NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledMode").getEntry(0); - ledModeEntry.set(VisionLEDMode.kDefault.value); ledModeListener = visionLED == null ? null : new NTDataChangeListener( NetworkTablesManager.getInstance().kRootTable.getInstance(), - ledModeEntry, + ledModeRequest, visionLED::onLedModeChange); Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit)); diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java b/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java index 7dfd23cf6d..96d9937504 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java @@ -21,6 +21,7 @@ import java.util.ArrayList; import java.util.List; import java.util.function.BooleanSupplier; +import java.util.function.Consumer; import org.photonvision.common.hardware.GPIO.CustomGPIO; import org.photonvision.common.hardware.GPIO.GPIOBase; import org.photonvision.common.hardware.GPIO.pi.PigpioException; @@ -45,11 +46,18 @@ public class VisionLED { private int mappedBrightnessPercentage; + private Consumer modeConsumer; + public VisionLED( - List ledPins, int brightnessMin, int brightnessMax, PigpioSocket pigpioSocket) { + List ledPins, + int brightnessMin, + int brightnessMax, + PigpioSocket pigpioSocket, + Consumer visionLEDmode) { this.brightnessMin = brightnessMin; this.brightnessMax = brightnessMax; this.pigpioSocket = pigpioSocket; + this.modeConsumer = visionLEDmode; this.ledPins = ledPins.stream().mapToInt(i -> i).toArray(); ledPins.forEach( pin -> { @@ -123,7 +131,8 @@ public void setState(boolean on) { } void onLedModeChange(NetworkTableEvent entryNotification) { - var newLedModeRaw = (int) entryNotification.valueData.value.getDouble(); + var newLedModeRaw = (int) entryNotification.valueData.value.getInteger(); + logger.debug("Got LED mode " + newLedModeRaw); if (newLedModeRaw != currentLedMode.value) { VisionLEDMode newLedMode; switch (newLedModeRaw) { @@ -145,6 +154,8 @@ void onLedModeChange(NetworkTableEvent entryNotification) { break; } setInternal(newLedMode, true); + + if (modeConsumer != null) modeConsumer.accept(newLedMode.value); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index c8d5e7baa6..09229885ad 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -371,15 +371,15 @@ public CameraCalibrationCoefficients endCalibration() { return ret; } - void setPipeline(int index) { + boolean setPipeline(int index) { logger.info("Setting pipeline to " + index); logger.info("Pipeline name: " + pipelineManager.getPipelineNickname(index)); pipelineManager.setIndex(index); var pipelineSettings = pipelineManager.getPipelineSettings(index); if (pipelineSettings == null) { - logger.error("Config for index " + index + " was null!"); - return; + logger.error("Config for index " + index + " was null! Not changing pipelines"); + return false; } visionSource.getSettables().setVideoModeInternal(pipelineSettings.cameraVideoModeIndex); @@ -422,6 +422,8 @@ void setPipeline(int index) { visionSource.getSettables().getConfiguration().currentPipelineIndex = pipelineManager.getCurrentPipelineIndex(); + + return true; } private boolean camShouldControlLEDs() { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 4067ef888d..d4be529ef0 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -29,6 +29,7 @@ import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; import edu.wpi.first.networktables.MultiSubscriber; import edu.wpi.first.networktables.NetworkTable; @@ -60,7 +61,8 @@ public class PhotonCamera { DoublePublisher targetSkewEntry; StringSubscriber versionEntry; IntegerEntry inputSaveImgEntry, outputSaveImgEntry; - IntegerEntry pipelineIndexEntry, ledModeEntry; + IntegerPublisher pipelineIndexRequest, ledModeRequest; + IntegerSubscriber pipelineIndexState, ledModeState; IntegerSubscriber heartbeatEntry; public void close() { @@ -77,8 +79,11 @@ public void close() { versionEntry.close(); inputSaveImgEntry.close(); outputSaveImgEntry.close(); - pipelineIndexEntry.close(); - ledModeEntry.close(); + pipelineIndexRequest.close(); + pipelineIndexState.close(); + ledModeRequest.close(); + ledModeState.close(); + pipelineIndexRequest.close(); } private final String path; @@ -122,9 +127,11 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { driverModeSubscriber = rootTable.getBooleanTopic("driverModeRequest").subscribe(false); inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0); - pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0); + pipelineIndexRequest = rootTable.getIntegerTopic("pipelineIndexRequest").publish(); + pipelineIndexState = rootTable.getIntegerTopic("pipelineIndexState").subscribe(0); heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1); - ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1); + ledModeRequest = mainTable.getIntegerTopic("ledModeRequest").publish(); + ledModeState = mainTable.getIntegerTopic("ledModeState").subscribe(-1); versionEntry = mainTable.getStringTopic("version").subscribe(""); m_topicNameSubscriber = @@ -215,7 +222,7 @@ public void takeOutputSnapshot() { * @return The active pipeline index. */ public int getPipelineIndex() { - return (int) pipelineIndexEntry.get(0); + return (int) pipelineIndexState.get(0); } /** @@ -224,7 +231,7 @@ public int getPipelineIndex() { * @param index The active pipeline index. */ public void setPipelineIndex(int index) { - pipelineIndexEntry.set(index); + pipelineIndexRequest.set(index); } /** @@ -233,7 +240,7 @@ public void setPipelineIndex(int index) { * @return The current LED mode. */ public VisionLEDMode getLEDMode() { - int value = (int) ledModeEntry.get(-1); + int value = (int) ledModeState.get(-1); switch (value) { case 0: return VisionLEDMode.kOff; @@ -253,7 +260,7 @@ public VisionLEDMode getLEDMode() { * @param led The mode to set to. */ public void setLED(VisionLEDMode led) { - ledModeEntry.set(led.value); + ledModeRequest.set(led.value); } /** diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 482267acc7..d0732cd04b 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -50,16 +50,17 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, rootTable->GetIntegerTopic("outputSaveImgCmd").Publish()), outputSaveImgSubscriber( rootTable->GetIntegerTopic("outputSaveImgCmd").Subscribe(0)), - pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()), - ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()), + pipelineIndexPub( + rootTable->GetIntegerTopic("pipelineIndexRequest").Publish()), + pipelineIndexSub( + rootTable->GetIntegerTopic("pipelineIndexState").Subscribe(0)), + ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()), + ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)), versionEntry(mainTable->GetStringTopic("version").Subscribe("")), driverModeSubscriber( rootTable->GetBooleanTopic("driverMode").Subscribe(false)), driverModePublisher( rootTable->GetBooleanTopic("driverModeRequest").Publish()), - pipelineIndexSubscriber( - rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)), - ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)), m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}), path(rootTable->GetPath()), m_cameraName(cameraName) {} @@ -107,19 +108,19 @@ void PhotonCamera::TakeOutputSnapshot() { bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); } void PhotonCamera::SetPipelineIndex(int index) { - pipelineIndexEntry.Set(static_cast(index)); + pipelineIndexPub.Set(static_cast(index)); } int PhotonCamera::GetPipelineIndex() const { - return static_cast(pipelineIndexSubscriber.Get()); + return static_cast(pipelineIndexSub.Get()); } LEDMode PhotonCamera::GetLEDMode() const { - return static_cast(static_cast(ledModeSubscriber.Get())); + return static_cast(static_cast(ledModeSub.Get())); } void PhotonCamera::SetLEDMode(LEDMode mode) { - ledModeEntry.Set(static_cast(static_cast(mode))); + ledModePub.Set(static_cast(static_cast(mode))); } const std::string_view PhotonCamera::GetCameraName() const { diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index 781c5727ce..2c641ebc4f 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -172,13 +172,14 @@ class PhotonCamera { nt::IntegerSubscriber inputSaveImgSubscriber; nt::IntegerPublisher outputSaveImgEntry; nt::IntegerSubscriber outputSaveImgSubscriber; - nt::IntegerPublisher pipelineIndexEntry; - nt::IntegerPublisher ledModeEntry; + nt::IntegerPublisher pipelineIndexPub; + nt::IntegerSubscriber pipelineIndexSub; + nt::IntegerPublisher ledModePub; + nt::IntegerSubscriber ledModeSub; nt::StringSubscriber versionEntry; nt::BooleanSubscriber driverModeSubscriber; nt::BooleanPublisher driverModePublisher; - nt::IntegerSubscriber pipelineIndexSubscriber; nt::IntegerSubscriber ledModeSubscriber; nt::MultiSubscriber m_topicNameSubscriber; diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index edaa65d34c..8107b4d175 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -41,9 +41,8 @@ public class NTTopicSet { public NetworkTable subTable; public RawPublisher rawBytesEntry; - public IntegerTopic pipelineIndexTopic; public IntegerPublisher pipelineIndexPublisher; - public IntegerSubscriber pipelineIndexSubscriber; + public IntegerSubscriber pipelineIndexRequestSub; public BooleanTopic driverModeEntry; public BooleanPublisher driverModePublisher; @@ -71,9 +70,8 @@ public void updateEntries() { .getRawTopic("rawBytes") .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); - pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex"); - pipelineIndexPublisher = pipelineIndexTopic.publish(); - pipelineIndexSubscriber = pipelineIndexTopic.subscribe(0); + pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); + pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); driverModePublisher = subTable.getBooleanTopic("driverMode").publish(); driverModeSubscriber = subTable.getBooleanTopic("driverModeRequest").subscribe(false); @@ -101,7 +99,7 @@ public void updateEntries() { public void removeEntries() { if (rawBytesEntry != null) rawBytesEntry.close(); if (pipelineIndexPublisher != null) pipelineIndexPublisher.close(); - if (pipelineIndexSubscriber != null) pipelineIndexSubscriber.close(); + if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close(); if (driverModePublisher != null) driverModePublisher.close(); if (driverModeSubscriber != null) driverModeSubscriber.close();