diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue index 11f20f0810..ff624fb77e 100644 --- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue +++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue @@ -3,12 +3,32 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore"; import { PipelineType } from "@/types/PipelineTypes"; import { useStateStore } from "@/stores/StateStore"; +const wrapToPi = (delta: number): number => { + let ret = delta; + while (ret < -Math.PI) ret += Math.PI * 2; + while (ret > Math.PI) ret -= Math.PI * 2; + return ret; +}; + const calculateStdDev = (values: number[]): number => { if (values.length < 2) return 0; - const mean = values.reduce((sum, number) => sum + number, 0) / values.length; + // Use mean of cosine/sine components to handle angle wrapping + const cosines = values.map((it) => Math.cos(it)); + const sines = values.map((it) => Math.sin(it)); + const cosmean = cosines.reduce((sum, number) => sum + number, 0) / values.length; + const sinmean = sines.reduce((sum, number) => sum + number, 0) / values.length; + + // Borrowed from WPILib's Rotation2d + const hypot = Math.hypot(cosmean, sinmean); + let mean; + if (hypot > 1e-6) { + mean = Math.atan2(sinmean / hypot, cosmean / hypot); + } else { + mean = 0; + } - return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length); + return Math.sqrt(values.map((x) => Math.pow(wrapToPi(x - mean), 2)).reduce((a, b) => a + b) / values.length); }; const resetCurrentBuffer = () => { // Need to clear the array in place @@ -119,7 +139,7 @@ const resetCurrentBuffer = () => { {{ ( - useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x * + (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x || 0) * (180.0 / Math.PI) ).toFixed(2) }}° @@ -127,7 +147,7 @@ const resetCurrentBuffer = () => { {{ ( - useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y * + (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y || 0) * (180.0 / Math.PI) ).toFixed(2) }}° @@ -135,7 +155,7 @@ const resetCurrentBuffer = () => { {{ ( - useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z * + (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z || 0) * (180.0 / Math.PI) ).toFixed(2) }}° @@ -146,8 +166,8 @@ const resetCurrentBuffer = () => { Multi-tag pose standard deviation over the last {{ useStateStore().currentMultitagBuffer.length }}/100 - samples + >Multi-tag pose standard deviation over the last + {{ useStateStore().currentMultitagBuffer?.length || "NaN" }}/100 samples Reset Samples { {{ - calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5) + calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x) || []).toFixed(5) }} m {{ - calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5) + calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y) || []).toFixed(5) }} m {{ - calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5) + calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z) || []).toFixed(5) }} m {{ calculateStdDev( - useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI)) + useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI)) || [] ).toFixed(5) }}° {{ calculateStdDev( - useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI)) + useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI)) || [] ).toFixed(5) }}° {{ calculateStdDev( - useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) + useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) || [] ).toFixed(5) }}° diff --git a/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java b/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java index a1ba6d091d..bc3bc1e1f0 100644 --- a/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java +++ b/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java @@ -20,6 +20,5 @@ public enum ProgramStatus { UHOH, RUNNING, - RUNNING_NT, - RUNNING_NT_TARGET + RUNNING_NT } diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 883b3607d8..95f01e4079 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -343,6 +343,29 @@ private void addFile(PreparedStatement ps, String key, String value) throws SQLE ps.setString(2, value); } + // NOTE to Future Developers: + // These booleans form a mechanism to prevent saveGlobal() and + // saveOneFile() from stepping on each other's toes. Both write + // to the database on disk, and both write to the same keys, but + // they use different sources. Generally, if the user has done something + // to trigger saveOneFile() to get called, it implies they want that + // configuration, and not whatever is in RAM right now (which is what + // saveGlobal() uses to write). Therefor, once saveOneFile() is invoked, + // we record which entry was overwritten in the database and prevent + // overwriting it when saveGlobal() is invoked (likely by the shutdown + // that should almost almost almost happen right after saveOneFile() is + // invoked). + // + // In the future, this may not be needed. A better architecture would involve + // manipulating the RAM representation of configuration when new .json files + // are uploaded in the UI, and eliminate all other usages of saveOneFile(). + // But, seeing as it's Dec 28 and kickoff is nigh, we put this here and moved on. + // Thank you for coming to my TED talk. + private boolean skipSavingHWCfg = false; + private boolean skipSavingHWSet = false; + private boolean skipSavingNWCfg = false; + private boolean skipSavingAPRTG = false; + private void saveGlobal(Connection conn) { PreparedStatement statement1 = null; PreparedStatement statement2 = null; @@ -351,28 +374,34 @@ private void saveGlobal(Connection conn) { // Replace this camera's row with the new settings var sqlString = "REPLACE INTO global (filename, contents) VALUES " + "(?,?);"; - statement1 = conn.prepareStatement(sqlString); - addFile( - statement1, - TableKeys.HARDWARE_SETTINGS, - JacksonUtils.serializeToString(config.getHardwareSettings())); - statement1.executeUpdate(); + if (!skipSavingHWSet) { + statement1 = conn.prepareStatement(sqlString); + addFile( + statement1, + TableKeys.HARDWARE_SETTINGS, + JacksonUtils.serializeToString(config.getHardwareSettings())); + statement1.executeUpdate(); + } - statement2 = conn.prepareStatement(sqlString); - addFile( - statement2, - TableKeys.NETWORK_CONFIG, - JacksonUtils.serializeToString(config.getNetworkConfig())); - statement2.executeUpdate(); - statement2.close(); - - statement3 = conn.prepareStatement(sqlString); - addFile( - statement3, - TableKeys.HARDWARE_CONFIG, - JacksonUtils.serializeToString(config.getHardwareConfig())); - statement3.executeUpdate(); - statement3.close(); + if (!skipSavingNWCfg) { + statement2 = conn.prepareStatement(sqlString); + addFile( + statement2, + TableKeys.NETWORK_CONFIG, + JacksonUtils.serializeToString(config.getNetworkConfig())); + statement2.executeUpdate(); + statement2.close(); + } + + if (!skipSavingHWCfg) { + statement3 = conn.prepareStatement(sqlString); + addFile( + statement3, + TableKeys.HARDWARE_CONFIG, + JacksonUtils.serializeToString(config.getHardwareConfig())); + statement3.executeUpdate(); + statement3.close(); + } } catch (SQLException | IOException e) { logger.error("Err saving global", e); @@ -431,21 +460,25 @@ private boolean saveOneFile(String fname, Path path) { @Override public boolean saveUploadedHardwareConfig(Path uploadPath) { + skipSavingHWCfg = true; return saveOneFile(TableKeys.HARDWARE_CONFIG, uploadPath); } @Override public boolean saveUploadedHardwareSettings(Path uploadPath) { + skipSavingHWSet = true; return saveOneFile(TableKeys.HARDWARE_SETTINGS, uploadPath); } @Override public boolean saveUploadedNetworkConfig(Path uploadPath) { + skipSavingNWCfg = true; return saveOneFile(TableKeys.NETWORK_CONFIG, uploadPath); } @Override public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) { + skipSavingAPRTG = true; return saveOneFile(TableKeys.ATFL_CONFIG_FILE, uploadPath); } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index a62e0f3b4c..4cd2d1e0bd 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -32,6 +32,7 @@ import org.photonvision.common.configuration.NetworkConfig; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; +import org.photonvision.common.hardware.HardwareManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.scripting.ScriptEventType; @@ -91,6 +92,7 @@ public void accept(NetworkTableEvent event) { event.connInfo.remote_port, event.connInfo.protocol_version); logger.error(msg); + HardwareManager.getInstance().setNTConnected(false); hasReportedConnectionFailure = true; getInstance().broadcastConnectedStatus(); @@ -102,6 +104,7 @@ public void accept(NetworkTableEvent event) { event.connInfo.remote_port, event.connInfo.protocol_version); logger.info(msg); + HardwareManager.getInstance().setNTConnected(true); hasReportedConnectionFailure = false; ScriptManager.queueEvent(ScriptEventType.kNTConnected); diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java b/photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java new file mode 100644 index 0000000000..175b4c4d91 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java @@ -0,0 +1,35 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.common.dataflow.statusLEDs; + +import org.photonvision.common.dataflow.CVPipelineResultConsumer; +import org.photonvision.common.hardware.HardwareManager; +import org.photonvision.vision.pipeline.result.CVPipelineResult; + +public class StatusLEDConsumer implements CVPipelineResultConsumer { + private final int index; + + public StatusLEDConsumer(int index) { + this.index = index; + } + + @Override + public void accept(CVPipelineResult t) { + HardwareManager.getInstance().setTargetsVisibleStatus(this.index, t.hasTargets()); + } +} 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 2a049cccae..dcb0ac69dc 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 @@ -20,7 +20,8 @@ import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; import java.io.IOException; -import org.photonvision.common.ProgramStatus; +import java.util.HashMap; +import java.util.Map; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.HardwareConfig; import org.photonvision.common.configuration.HardwareSettings; @@ -32,6 +33,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ShellExec; +import org.photonvision.common.util.TimedTaskManager; public class HardwareManager { private static HardwareManager instance; @@ -96,6 +98,10 @@ private HardwareManager(HardwareConfig hardwareConfig, HardwareSettings hardware ? new StatusLED(hardwareConfig.statusRGBPins) : null; + if (statusLED != null) { + TimedTaskManager.getInstance().addTask("StatusLEDUpdate", this::statusLEDUpdate, 150); + } + var hasBrightnessRange = hardwareConfig.ledBrightnessRange.size() == 2; visionLED = hardwareConfig.ledPins.isEmpty() @@ -160,21 +166,61 @@ public boolean restartDevice() { } } - public void setStatus(ProgramStatus status) { - switch (status) { - case UHOH: - // red flashing, green off - break; - case RUNNING: - // red solid, green off - break; - case RUNNING_NT: - // red off, green solid - break; - case RUNNING_NT_TARGET: - // red off, green flashing - break; + // API's supporting status LEDs + + private Map pipelineTargets = new HashMap(); + private boolean ntConnected = false; + private boolean systemRunning = false; + private int blinkCounter = 0; + + public void setTargetsVisibleStatus(int pipelineIdx, boolean hasTargets) { + pipelineTargets.put(pipelineIdx, hasTargets); + } + + public void setNTConnected(boolean isConnected) { + this.ntConnected = isConnected; + } + + public void setRunning(boolean isRunning) { + this.systemRunning = isRunning; + } + + private void statusLEDUpdate() { + // make blinky + boolean blinky = ((blinkCounter % 3) > 0); + + // check if any pipeline has a visible target + boolean anyTarget = false; + for (var t : this.pipelineTargets.values()) { + if (t) { + anyTarget = true; + } + } + + if (this.systemRunning) { + if (!this.ntConnected) { + if (anyTarget) { + // Blue Flashing + statusLED.setRGB(false, false, blinky); + } else { + // Yellow flashing + statusLED.setRGB(blinky, blinky, false); + } + } else { + if (anyTarget) { + // Blue + statusLED.setRGB(false, false, blinky); + } else { + // blinky green + statusLED.setRGB(false, blinky, false); + } + } + } else { + // Faulted, not running... blinky red + statusLED.setRGB(blinky, false, false); } + + blinkCounter++; } public HardwareConfig getConfig() { diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java b/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java index 0c10b3548e..efef23454e 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java @@ -45,4 +45,14 @@ public StatusLED(List statusLedPins) { blueLED = new CustomGPIO(statusLedPins.get(2)); } } + + public void setRGB(boolean r, boolean g, boolean b) { + // Outputs are active-low, so invert the level applied + redLED.setState(!r); + redLED.setBrightness(r ? 0 : 100); + greenLED.setState(!g); + greenLED.setBrightness(g ? 0 : 100); + blueLED.setState(!b); + blueLED.setBrightness(b ? 0 : 100); + } } 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 483f6a7c0c..38becf9227 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 @@ -33,6 +33,7 @@ import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.dataflow.networktables.NTDataPublisher; +import org.photonvision.common.dataflow.statusLEDs.StatusLEDConsumer; import org.photonvision.common.dataflow.websocket.UIDataPublisher; import org.photonvision.common.hardware.HardwareManager; import org.photonvision.common.logging.LogGroup; @@ -72,6 +73,7 @@ public class VisionModule { new LinkedList<>(); private final NTDataPublisher ntConsumer; private final UIDataPublisher uiDataConsumer; + private final StatusLEDConsumer statusLEDsConsumer; protected final int moduleIndex; protected final QuirkyCamera cameraQuirks; @@ -143,8 +145,10 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, pipelineManager::getDriverMode, this::setDriverMode); uiDataConsumer = new UIDataPublisher(index); + statusLEDsConsumer = new StatusLEDConsumer(index); addResultConsumer(ntConsumer); addResultConsumer(uiDataConsumer); + addResultConsumer(statusLEDsConsumer); addResultConsumer( (result) -> lastPipelineResultBestTarget = result.hasTargets() ? result.targets.get(0) : null); diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index f01e431d30..daf8c243ca 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -375,6 +375,7 @@ public static void main(String[] args) { } logger.info("Starting server..."); + HardwareManager.getInstance().setRunning(true); Server.start(DEFAULT_WEBPORT); } } diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index d277f2904f..1d490c1875 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -89,8 +89,8 @@ public static void onSettingsImportRequest(Context ctx) { if (ConfigManager.saveUploadedSettingsZip(tempFilePath.get())) { ctx.status(200); - ctx.result("Successfully saved the uploaded settings zip, rebooting"); - logger.info("Successfully saved the uploaded settings zip, rebooting"); + ctx.result("Successfully saved the uploaded settings zip, rebooting..."); + logger.info("Successfully saved the uploaded settings zip, rebooting..."); restartProgram(); } else { ctx.status(500); @@ -153,8 +153,9 @@ public static void onHardwareConfigRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedHardwareConfig(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded hardware config"); - logger.info("Successfully saved the uploaded hardware config"); + ctx.result("Successfully saved the uploaded hardware config, rebooting..."); + logger.info("Successfully saved the uploaded hardware config, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded hardware config"); @@ -195,8 +196,9 @@ public static void onHardwareSettingsRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedHardwareSettings(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded hardware settings"); - logger.info("Successfully saved the uploaded hardware settings"); + ctx.result("Successfully saved the uploaded hardware settings, rebooting..."); + logger.info("Successfully saved the uploaded hardware settings, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded hardware settings"); @@ -237,8 +239,9 @@ public static void onNetworkConfigRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedNetworkConfig(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded network config"); - logger.info("Successfully saved the uploaded network config"); + ctx.result("Successfully saved the uploaded network config, rebooting..."); + logger.info("Successfully saved the uploaded network config, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded network config"); @@ -279,8 +282,9 @@ public static void onAprilTagFieldLayoutRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedAprilTagFieldLayout(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded AprilTagFieldLayout"); - logger.info("Successfully saved the uploaded AprilTagFieldLayout"); + ctx.result("Successfully saved the uploaded AprilTagFieldLayout, rebooting..."); + logger.info("Successfully saved the uploaded AprilTagFieldLayout, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded AprilTagFieldLayout");