Led control (#109)

* Refactor vision source manager to not suck

* Run spotless

* Fix settings not being saved

* run spotless

* add braudcast

* set LEDs based on pipeline

* GPIO refactor, hardwaremanager singleton change

* PiGPIO cleanup, add repeating blink, StatusLED

* Refactor CustomGPIO

* add ledMode NT Entry

* Add blink reset

* Blink fixes

* Move logic to VisionLED class

* Resolve comments, apply spotless

* "Fix' HardwareManagerTest

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

photon-server/src/main/java/org/photonvision/Main.java

@@ -25,7 +25,6 @@ import org.apache.commons.cli.*;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
-import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
@@ -177,10 +176,6 @@ public class Main {
             addTestModeSources();
         }
 
-        // Add hardware config to hardware manager
-        HardwareManager.getInstance()
-                .setConfig(ConfigManager.getInstance().getConfig().getHardwareConfig());
-
         Server.main(DEFAULT_WEBPORT);
     }
 }

photon-server/src/main/java/org/photonvision/common/ProgramStatus.java

@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2020 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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common;
+
+public enum ProgramStatus {
+    UHOH,
+    RUNNING,
+    RUNNING_NT,
+    RUNNING_NT_TARGET
+}

photon-server/src/main/java/org/photonvision/common/hardware/GPIO/CustomGPIO.java

@@ -26,7 +26,7 @@ public class CustomGPIO extends GPIOBase {
 
     private boolean currentState;
     private List<Integer> pwmRange = new ArrayList<>();
-    private int port;
+    private final int port;
 
     public CustomGPIO(int port) {
         this.port = port;
@@ -45,37 +45,18 @@ public class CustomGPIO extends GPIOBase {
     }
 
     @Override
-    public void setLow() {
-        if (this.port != -1) {
-            execute(
-                    commands
-                            .get("setState")
-                            .replace("{s}", String.valueOf(false))
-                            .replace("{p}", String.valueOf(this.port)));
-            currentState = false;
-        }
+    public int getPinNumber() {
+        return port;
     }
 
     @Override
-    public void setHigh() {
-        if (this.port != -1) {
-            execute(
-                    commands
-                            .get("setState")
-                            .replace("{s}", String.valueOf(true))
-                            .replace("{p}", String.valueOf(this.port)));
-            currentState = true;
-        }
-    }
-
-    @Override
-    public void setState(boolean state) {
+    public void setStateImpl(boolean state) {
         if (this.port != -1) {
             execute(
                     commands
                             .get("setState")
                             .replace("{s}", String.valueOf(state))
-                            .replace("{p}", String.valueOf(this.port)));
+                            .replace("{p}", String.valueOf(port)));
             currentState = state;
         }
     }
@@ -90,51 +71,45 @@ public class CustomGPIO extends GPIOBase {
     }
 
     @Override
-    public boolean getState() {
+    public boolean getStateImpl() {
         return currentState;
     }
 
     @Override
-    public void setPwmRange(List<Integer> range) {
-        if (this.port != -1) {
-            execute(
-                    commands
-                            .get("setRange")
-                            .replace("{lower_range}", String.valueOf(range.get(0)))
-                            .replace("{upper_range}", String.valueOf(range.get(1)))
-                            .replace("{p}", String.valueOf(port)));
-            pwmRange = range;
-        }
+    public void setPwmRangeImpl(List<Integer> range) {
+        execute(
+                commands
+                        .get("setRange")
+                        .replace("{lower_range}", String.valueOf(range.get(0)))
+                        .replace("{upper_range}", String.valueOf(range.get(1)))
+                        .replace("{p}", String.valueOf(port)));
+        pwmRange = range;
     }
 
     @Override
-    public List<Integer> getPwmRange() {
+    public List<Integer> getPwmRangeImpl() {
         return pwmRange;
     }
 
     @Override
-    public void blink(int pulseTimeMillis, int blinks) {
-        if (this.port != -1) {
-            execute(
-                    commands
-                            .get("blink")
-                            .replace("{pulseTime}", String.valueOf(pulseTimeMillis))
-                            .replace("{blinks}", String.valueOf(blinks))
-                            .replace("{p}", String.valueOf(this.port)));
-        }
+    public void blinkImpl(int pulseTimeMillis, int blinks) {
+        execute(
+                commands
+                        .get("blink")
+                        .replace("{pulseTime}", String.valueOf(pulseTimeMillis))
+                        .replace("{blinks}", String.valueOf(blinks))
+                        .replace("{p}", String.valueOf(this.port)));
     }
 
     @Override
-    public void dimLED(int dimValue) {
-        if (this.port != -1) {
-            // Check to see if dimValue is within the range
-            if (dimValue < pwmRange.get(0) || dimValue > pwmRange.get(1)) return;
-            execute(
-                    commands
-                            .get("dim")
-                            .replace("{p}", String.valueOf(port))
-                            .replace("{v}", String.valueOf(dimValue)));
-        }
+    public void setBrightnessImpl(int brightness) {
+        // Check to see if dimValue is within the range
+        if (brightness < pwmRange.get(0) || brightness > pwmRange.get(1)) return;
+        execute(
+                commands
+                        .get("dim")
+                        .replace("{p}", String.valueOf(port))
+                        .replace("{v}", String.valueOf(brightness)));
     }
 
     public static void setConfig(HardwareConfig config) {

photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java

@@ -28,11 +28,10 @@ public abstract class GPIOBase {
     private static final Logger logger = new Logger(GPIOBase.class, LogGroup.General);
     private static final ShellExec runCommand = new ShellExec(true, true);
 
-    public static HashMap<String, String> commands =
+    protected static HashMap<String, String> commands =
             new HashMap<>() {
                 {
                     put("setState", "");
-                    put("shutdown", "");
                     put("setRange", "");
                     put("shutdown", "");
                     put("dim", "");
@@ -40,7 +39,7 @@ public abstract class GPIOBase {
                 }
             };
 
-    public static String execute(String command) {
+    protected static String execute(String command) {
         try {
             runCommand.executeBashCommand(command);
         } catch (Exception e) {
@@ -50,23 +49,67 @@ public abstract class GPIOBase {
         return runCommand.getOutput();
     }
 
-    public abstract void togglePin();
+    public abstract int getPinNumber();
 
-    public abstract void setLow();
+    public void setState(boolean state) {
+        if (getPinNumber() != -1) {
+            setStateImpl(state);
+        }
+    }
 
-    public abstract void setHigh();
+    protected abstract void setStateImpl(boolean state);
 
-    public abstract void setState(boolean state);
+    public final void setOff() {
+        setState(false);
+    }
+
+    public final void setOn() {
+        setState(true);
+    }
+
+    public void togglePin() {
+        setState(!getStateImpl());
+    }
 
     public abstract boolean shutdown();
 
-    public abstract boolean getState();
+    public final boolean getState() {
+        if (getPinNumber() != -1) {
+            return getStateImpl();
+        } else return false;
+    }
 
-    public abstract void setPwmRange(List<Integer> range);
+    public abstract boolean getStateImpl();
 
-    public abstract List<Integer> getPwmRange();
+    public final void setPwmRange(List<Integer> range) {
+        if (getPinNumber() != -1) {
+            setPwmRangeImpl(range);
+        }
+    }
 
-    public abstract void blink(int pulseTimeMillis, int blinks);
+    protected abstract void setPwmRangeImpl(List<Integer> range);
 
-    public abstract void dimLED(int dimPercentage);
+    public final List<Integer> getPwmRange() {
+        if (getPinNumber() != -1) {
+            return getPwmRangeImpl();
+        } else return List.of(0, 255);
+    }
+
+    protected abstract List<Integer> getPwmRangeImpl();
+
+    public final void blink(int pulseTimeMillis, int blinks) {
+        if (getPinNumber() != -1) {
+            blinkImpl(pulseTimeMillis, blinks);
+        }
+    }
+
+    protected abstract void blinkImpl(int pulseTimeMillis, int blinks);
+
+    public final void setBrightness(int brightness) {
+        if (getPinNumber() != -1) {
+            setBrightnessImpl(brightness);
+        }
+    }
+
+    protected abstract void setBrightnessImpl(int brightness);
 }

photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java

@@ -17,6 +17,8 @@
 
 package org.photonvision.common.hardware.GPIO;
 
+import static eu.xeli.jpigpio.PigpioException.*;
+
 import eu.xeli.jpigpio.JPigpio;
 import eu.xeli.jpigpio.PigpioException;
 import eu.xeli.jpigpio.PigpioSocket;
@@ -31,149 +33,154 @@ public class PiGPIO extends GPIOBase {
     private final ArrayList<Pulse> pulses = new ArrayList<>();
     private final int port;
 
+    private int activeWaveId = -1;
+
     public static JPigpio getPigpioDaemon() {
         return Singleton.INSTANCE;
     }
 
+    public PiGPIO(int address) {
+        this(address, 8000, 255);
+    }
+
     public PiGPIO(int address, int frequency, int range) {
-        this.port = address;
+        port = address;
+        if (port != -1) {
+            try {
+                //            var pigpioRange = (int) (range / 255.0) * 40000; // TODO: is this conversion
+                // correct/necessary?
+                getPigpioDaemon().setPWMFrequency(port, frequency);
+                getPigpioDaemon().setPWMRange(port, range);
+            } catch (PigpioException e) {
+                logger.error("Could not set PWM settings on port " + port, e);
+            }
+        }
+    }
+
+    private void cancelWave() throws PigpioException {
+        if (activeWaveId != -1) {
+            logger.debug("Cancelling wave with id " + activeWaveId);
+            getPigpioDaemon().waveDelete(activeWaveId);
+            getPigpioDaemon().waveTxStop();
+            activeWaveId = -1;
+        }
+    }
+
+    @Override
+    public int getPinNumber() {
+        return port;
+    }
+
+    @Override
+    public void setStateImpl(boolean state) {
         try {
-            getPigpioDaemon().setPWMFrequency(this.port, frequency);
-            getPigpioDaemon().setPWMRange(this.port, range);
+            cancelWave();
+            getPigpioDaemon().gpioWrite(port, state);
         } catch (PigpioException e) {
-            logger.error("Could not set PWM settings on port " + this.port);
-            e.printStackTrace();
-        }
-    }
-
-    @Override
-    public void togglePin() {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().gpioWrite(this.port, !getPigpioDaemon().gpioRead(this.port));
-            } catch (PigpioException e) {
-                logger.error("Could not toggle on pin " + this.port);
-                e.printStackTrace();
-            }
-        }
-    }
-
-    @Override
-    public void setLow() {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().gpioWrite(this.port, false);
-            } catch (PigpioException e) {
-                logger.error("Could not set pin low on port " + this.port);
-                e.printStackTrace();
-            }
-        }
-    }
-
-    @Override
-    public void setHigh() {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().gpioWrite(this.port, true);
-            } catch (PigpioException e) {
-                logger.error("Could not set pin high on port " + this.port);
-                e.printStackTrace();
-            }
-        }
-    }
-
-    @Override
-    public void setState(boolean state) {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().gpioWrite(this.port, state);
-            } catch (PigpioException e) {
-                logger.error("Could not set pin state on port " + this.port);
-                e.printStackTrace();
-            }
+            logger.error("Could not set pin state on port " + port, e);
         }
     }
 
     @Override
     public boolean shutdown() {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().gpioTerminate();
-            } catch (PigpioException e) {
-                logger.error("Could not terminate GPIO instance");
-                e.printStackTrace();
-            }
-            return true;
+        try {
+            getPigpioDaemon().gpioTerminate();
+        } catch (PigpioException e) {
+            logger.error("Could not terminate GPIO instance", e);
+            return false;
         }
-        return false;
+        return true;
     }
 
     @Override
-    public boolean getState() {
-        if (this.port != -1) {
-            try {
-                return getPigpioDaemon().gpioRead(this.port);
-            } catch (PigpioException e) {
-                logger.error("Could not read pin on port " + this.port);
-                e.printStackTrace();
-                return false;
-            }
-        }
-        return false;
-    }
-
-    @Override
-    public void setPwmRange(List<Integer> range) {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().setPWMRange(this.port, range.get(0));
-            } catch (PigpioException e) {
-                logger.error("Could not set PWM range on port " + this.port);
-                e.printStackTrace();
-            }
+    public boolean getStateImpl() {
+        try {
+            return getPigpioDaemon().gpioRead(port);
+        } catch (PigpioException e) {
+            logger.error("Could not read pin on port " + port, e);
+            return false;
         }
     }
 
     @Override
-    public List<Integer> getPwmRange() {
-        if (this.port != -1) {
-            try {
-                return List.of(0, getPigpioDaemon().getPWMRange(this.port));
-            } catch (PigpioException e) {
-                logger.error("Could not get PWM range on port " + this.port);
-                e.printStackTrace();
-                return null;
-            }
+    public void setPwmRangeImpl(List<Integer> range) {
+        try {
+            cancelWave();
+            getPigpioDaemon().setPWMRange(port, range.get(0));
+        } catch (PigpioException e) {
+            logger.error("Could not set PWM range on port " + port, e);
         }
-        return null;
     }
 
     @Override
-    public void blink(int pulseTimeMillis, int blinks) {
-        if (this.port != -1) {
-            try {
-                pulses.clear();
-                for (int i = 0; i < blinks; i++) {
-                    pulses.add(new Pulse(1 << this.port, 0, pulseTimeMillis * 100));
-                    pulses.add(new Pulse(0, 1 << this.port, pulseTimeMillis * 100));
+    public List<Integer> getPwmRangeImpl() {
+        try {
+            return List.of(0, getPigpioDaemon().getPWMRange(port));
+        } catch (PigpioException e) {
+            logger.error("Could not get PWM range on port " + port, e);
+            return List.of(0, 255);
+        }
+    }
+
+    @Override
+    public void blinkImpl(int pulseTimeMillis, int blinks) {
+        boolean repeat = blinks == -1;
+
+        if (repeat) {
+            blinks = 1;
+        }
+
+        try {
+            cancelWave();
+            pulses.clear();
+
+            var startPulse = new Pulse(1 << port, 0, pulseTimeMillis * 1000);
+            var endPulse = new Pulse(0, 1 << port, pulseTimeMillis * 1000);
+
+            for (int i = 0; i < blinks; i++) {
+                pulses.add(startPulse);
+                pulses.add(endPulse);
+            }
+
+            getPigpioDaemon().waveAddGeneric(pulses);
+            var waveId = getPigpioDaemon().waveCreate();
+
+            if (waveId >= 0) {
+                if (repeat) getPigpioDaemon().waveSendRepeat(waveId);
+                else getPigpioDaemon().waveSendOnce(waveId);
+                activeWaveId = waveId;
+            } else {
+                String error = "";
+                switch (waveId) {
+                    case PI_EMPTY_WAVEFORM:
+                        error = "Waveform empty";
+                        break;
+                    case PI_TOO_MANY_CBS:
+                        error = "Too many CBS";
+                        break;
+                    case PI_TOO_MANY_OOL:
+                        error = "Too many OOL";
+                        break;
+                    case PI_NO_WAVEFORM_ID:
+                        error = "No waveform ID";
+                        break;
                 }
-                getPigpioDaemon().waveAddGeneric(this.pulses);
-                getPigpioDaemon().waveSendOnce(getPigpioDaemon().waveCreate());
-            } catch (PigpioException e) {
-                e.printStackTrace();
+                logger.error("Failed to send wave: " + error);
             }
+
+        } catch (PigpioException e) {
+            logger.error("Could not set blink on port " + port, e);
         }
     }
 
     @Override
-    public void dimLED(int dimPercentage) {
-        if (this.port != -1) {
-            try {
-                getPigpioDaemon().setPWMDutycycle(this.port, getPwmRange().get(1) * (dimPercentage / 100));
-            } catch (PigpioException e) {
-                logger.error("Could not dim PWM on port " + this.port);
-                e.printStackTrace();
-            }
+    public void setBrightnessImpl(int brightness) {
+        try {
+            cancelWave();
+            getPigpioDaemon().setPWMDutycycle(port, getPwmRangeImpl().get(1) * (brightness / 100));
+        } catch (PigpioException e) {
+            logger.error("Could not dim PWM on port " + port);
+            e.printStackTrace();
         }
     }
 

photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java

@@ -17,12 +17,17 @@
 
 package org.photonvision.common.hardware;
 
+import edu.wpi.first.networktables.NetworkTableEntry;
 import java.io.IOException;
 import java.util.HashMap;
+import org.photonvision.common.ProgramStatus;
+import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.HardwareConfig;
+import org.photonvision.common.dataflow.networktables.NTDataChangeListener;
+import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.hardware.GPIO.CustomGPIO;
 import org.photonvision.common.hardware.GPIO.GPIOBase;
-import org.photonvision.common.hardware.GPIO.PiGPIO;
+import org.photonvision.common.hardware.VisionLED.VisionLEDMode;
 import org.photonvision.common.hardware.metrics.MetricsBase;
 import org.photonvision.common.hardware.metrics.MetricsPublisher;
 import org.photonvision.common.logging.LogGroup;
@@ -30,91 +35,46 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
 
 public class HardwareManager {
-    HardwareConfig hardwareConfig;
-    private final HashMap<Integer, GPIOBase> LEDs = new HashMap<>();
+    private static HardwareManager instance;
+
+    private final HashMap<Integer, GPIOBase> VisionLEDs = new HashMap<>();
     private final ShellExec shellExec = new ShellExec(true, false);
     private final Logger logger = new Logger(HardwareManager.class, LogGroup.General);
 
+    private final HardwareConfig hardwareConfig;
+    private final StatusLED statusLED;
+    private final NetworkTableEntry ledModeEntry;
+    private final NTDataChangeListener ledModeListener;
+
+    public final VisionLED visionLED;
+
     public static HardwareManager getInstance() {
-        if (Singleton.INSTANCE == null) {
-            Singleton.INSTANCE = new HardwareManager();
+        if (instance == null) {
+            instance = new HardwareManager(ConfigManager.getInstance().getConfig().getHardwareConfig());
         }
-        return Singleton.INSTANCE;
+        return instance;
     }
 
-    public void setConfig(HardwareConfig hardwareConfig) {
+    private HardwareManager(HardwareConfig hardwareConfig) {
         this.hardwareConfig = hardwareConfig;
         CustomGPIO.setConfig(hardwareConfig);
         MetricsBase.setConfig(hardwareConfig);
 
-        hardwareConfig.ledPins.forEach(
-                pin -> {
-                    if (Platform.isRaspberryPi()) {
-                        LEDs.put(
-                                pin,
-                                new PiGPIO(pin, hardwareConfig.ledPWMFrequency, hardwareConfig.ledPWMRange.get(1)));
-                    } else {
-                        LEDs.put(pin, new CustomGPIO(pin));
-                    }
-                });
+        statusLED = new StatusLED(hardwareConfig.statusRGBPins);
+        visionLED =
+                new VisionLED(
+                        hardwareConfig.ledPins,
+                        hardwareConfig.ledPWMFrequency,
+                        hardwareConfig.ledPWMRange.get(1));
+
+        ledModeEntry = NetworkTablesManager.getInstance().kRootTable.getEntry("ledMode");
+        ledModeEntry.setNumber(VisionLEDMode.VLM_DEFAULT.value);
+        ledModeListener = new NTDataChangeListener(ledModeEntry, visionLED::onLedModeChange);
 
         // Start hardware metrics thread
         if (Platform.isLinux()) MetricsPublisher.getInstance().startTask();
     }
 
-    /** Example: HardwareManager.getInstance().getPWM(port).dimLEDs(int dimValue); */
-    public GPIOBase getGPIO(int pin) {
-        return LEDs.get(pin);
-    }
-
-    public void blinkLEDs(int pulseTimeMillis, int blinks) {
-        LEDs.values().forEach(led -> led.blink(pulseTimeMillis, blinks));
-    }
-
-    public void setBrightnessPercentage(int percentage) {
-        LEDs.values().forEach(led -> led.dimLED(percentage));
-    }
-
-    public void turnLEDsOn() {
-        LEDs.values().forEach(GPIOBase::setHigh);
-    }
-
-    public void turnLEDsOff() {
-        LEDs.values().forEach(GPIOBase::setLow);
-    }
-
-    public void toggleLEDs() {
-        LEDs.values().forEach(GPIOBase::togglePin);
-    }
-
-    public void shutdown() {
-        LEDs.values().forEach(GPIOBase::shutdown);
-    }
-
-    public GPIOBase redStatusLED() {
-        try {
-            return LEDs.get(hardwareConfig.statusRGBPins.get(0));
-        } catch (ArrayIndexOutOfBoundsException e) {
-            return LEDs.get(-1);
-        }
-    }
-
-    public GPIOBase greenStatusLED() {
-        try {
-            return LEDs.get(hardwareConfig.statusRGBPins.get(1));
-        } catch (ArrayIndexOutOfBoundsException e) {
-            return LEDs.get(-1);
-        }
-    }
-
-    public GPIOBase blueStatusLED() {
-        try {
-            return LEDs.get(hardwareConfig.statusRGBPins.get(2));
-        } catch (ArrayIndexOutOfBoundsException e) {
-            return LEDs.get(-1);
-        }
-    }
-
     public boolean restartDevice() {
         try {
             return shellExec.executeBashCommand(hardwareConfig.restartHardwareCommand) == 0;
@@ -124,11 +84,24 @@ public class HardwareManager {
         }
     }
 
+    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;
+        }
+    }
+
     public HardwareConfig getConfig() {
         return hardwareConfig;
     }
-
-    private static class Singleton {
-        private static HardwareManager INSTANCE;
-    }
 }

photon-server/src/main/java/org/photonvision/common/hardware/StatusLED.java

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2020 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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.hardware;
+
+import java.util.List;
+import org.photonvision.common.hardware.GPIO.CustomGPIO;
+import org.photonvision.common.hardware.GPIO.GPIOBase;
+import org.photonvision.common.hardware.GPIO.PiGPIO;
+
+public class StatusLED {
+    public final GPIOBase redLED;
+    public final GPIOBase greenLED;
+    public final GPIOBase blueLED;
+
+    public StatusLED(List<Integer> statusLedPins) {
+        // fill unassigned pins with -1 to disable
+        if (statusLedPins.size() != 3) {
+            for (int i = 0; i < 3 - statusLedPins.size(); i++) {
+                statusLedPins.add(-1);
+            }
+        }
+
+        if (Platform.isRaspberryPi()) {
+            redLED = new PiGPIO(statusLedPins.get(0));
+            greenLED = new PiGPIO(statusLedPins.get(1));
+            blueLED = new PiGPIO(statusLedPins.get(2));
+        } else {
+            redLED = new CustomGPIO(statusLedPins.get(0));
+            greenLED = new CustomGPIO(statusLedPins.get(1));
+            blueLED = new CustomGPIO(statusLedPins.get(2));
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/hardware/VisionLED.java

@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2020 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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.hardware;
+
+import edu.wpi.first.networktables.EntryNotification;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+import org.photonvision.common.hardware.GPIO.CustomGPIO;
+import org.photonvision.common.hardware.GPIO.GPIOBase;
+import org.photonvision.common.hardware.GPIO.PiGPIO;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class VisionLED {
+    private static final Logger logger = new Logger(VisionLED.class, LogGroup.VisionModule);
+
+    public final List<GPIOBase> leds = new ArrayList<>();
+
+    private VisionLEDMode currentLedMode = VisionLEDMode.VLM_DEFAULT;
+    private BooleanSupplier pipelineModeSupplier;
+
+    public VisionLED(List<Integer> ledPins, int pwmFreq, int pwmRangeMax) {
+        ledPins.forEach(
+                pin -> {
+                    if (Platform.isRaspberryPi()) {
+                        leds.add(new PiGPIO(pin, pwmFreq, pwmRangeMax));
+                    } else {
+                        leds.add(new CustomGPIO(pin));
+                    }
+                });
+        pipelineModeSupplier = () -> false;
+    }
+
+    public void setPipelineModeSupplier(BooleanSupplier pipelineModeSupplier) {
+        this.pipelineModeSupplier = pipelineModeSupplier;
+    }
+
+    public void setBrightness(int percentage) {
+        leds.forEach((led) -> led.setBrightness(percentage));
+    }
+
+    private void blinkImpl(int pulseLengthMillis, int blinkCount) {
+        leds.forEach((led) -> led.blink(pulseLengthMillis, blinkCount));
+    }
+
+    private void setStateImpl(boolean state) {
+        leds.forEach((led) -> led.setState(state));
+    }
+
+    public void setState(boolean on) {
+        setInternal(on ? VisionLEDMode.VLM_ON : VisionLEDMode.VLM_OFF, false);
+    }
+
+    void onLedModeChange(EntryNotification entryNotification) {
+        var newLedModeRaw = (int) entryNotification.value.getDouble();
+        if (newLedModeRaw != currentLedMode.value) {
+            VisionLEDMode newLedMode;
+            switch (newLedModeRaw) {
+                case -1:
+                    newLedMode = VisionLEDMode.VLM_DEFAULT;
+                    break;
+                case 0:
+                    newLedMode = VisionLEDMode.VLM_OFF;
+                    break;
+                case 1:
+                    newLedMode = VisionLEDMode.VLM_ON;
+                    break;
+                case 2:
+                    newLedMode = VisionLEDMode.VLM_BLINK;
+                    break;
+                default:
+                    logger.warn("User supplied invalid LED mode, falling back to Default");
+                    newLedMode = VisionLEDMode.VLM_DEFAULT;
+                    break;
+            }
+            setInternal(newLedMode, true);
+        }
+    }
+
+    private void setInternal(VisionLEDMode newLedMode, boolean fromNT) {
+        var lastLedMode = currentLedMode;
+
+        if (fromNT) {
+            switch (newLedMode) {
+                case VLM_DEFAULT:
+                    setStateImpl(pipelineModeSupplier.getAsBoolean());
+                    break;
+                case VLM_OFF:
+                    setStateImpl(false);
+                    break;
+                case VLM_ON:
+                    setStateImpl(true);
+                    break;
+                case VLM_BLINK:
+                    blinkImpl(175, -1);
+                    break;
+            }
+            currentLedMode = newLedMode;
+            logger.info(
+                    "Changing LED mode from \""
+                            + lastLedMode.toString()
+                            + "\" to \""
+                            + newLedMode.toString()
+                            + "\"");
+        } else {
+            if (currentLedMode == VisionLEDMode.VLM_DEFAULT) {
+                switch (newLedMode) {
+                    case VLM_OFF:
+                        setStateImpl(false);
+                        break;
+                    case VLM_ON:
+                        setStateImpl(true);
+                        break;
+                }
+            }
+            logger.info("Changing LED internal state to " + newLedMode.toString());
+        }
+    }
+
+    public enum VisionLEDMode {
+        VLM_DEFAULT(-1),
+        VLM_OFF(0),
+        VLM_ON(1),
+        VLM_BLINK(2);
+
+        public final int value;
+
+        VisionLEDMode(int value) {
+            this.value = value;
+        }
+
+        @Override
+        public String toString() {
+            switch (this) {
+                case VLM_DEFAULT:
+                    return "Default";
+                case VLM_OFF:
+                    return "Off";
+                case VLM_ON:
+                    return "On";
+                case VLM_BLINK:
+                    return "Blink";
+            }
+            return "";
+        }
+    }
+}

photon-server/src/main/java/org/photonvision/common/util/TimedTaskManager.java

@@ -71,6 +71,10 @@ public class TimedTaskManager {
         }
     }
 
+    public void addOneShotTask(Runnable runnable, long millisStartDelay) {
+        timedTaskExecutorPool.schedule(runnable, millisStartDelay, TimeUnit.MILLISECONDS);
+    }
+
     public void cancelTask(String identifier) {
         var future = activeTasks.getOrDefault(identifier, null);
         if (future != null) {

photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -29,6 +29,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.websocket.UIDataPublisher;
+import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
@@ -129,6 +130,11 @@ public class VisionModule {
             var fov = ConfigManager.getInstance().getConfig().getHardwareConfig().vendorFOV;
             logger.info("Setting FOV of vendor camera to " + fov);
             visionSource.getSettables().setFOV(fov);
+
+            HardwareManager.getInstance()
+                    .visionLED
+                    .setPipelineModeSupplier(() -> pipelineManager.getCurrentPipelineSettings().ledMode);
+            setVisionLEDs(pipelineManager.getCurrentPipelineSettings().ledMode);
         }
 
         saveAndBroadcastAll();
@@ -136,6 +142,7 @@ public class VisionModule {
 
     void setDriverMode(boolean isDriverMode) {
         pipelineManager.setDriverMode(isDriverMode);
+        setVisionLEDs(!isDriverMode);
         saveAndBroadcastAll();
     }
 
@@ -225,10 +232,16 @@ public class VisionModule {
             visionSource.getSettables().setGain(config.cameraGain);
         }
 
+        setVisionLEDs(config.ledMode);
+
         visionSource.getSettables().getConfiguration().currentPipelineIndex =
                 pipelineManager.getCurrentPipelineIndex();
     }
 
+    private void setVisionLEDs(boolean on) {
+        if (isVendorCamera()) HardwareManager.getInstance().visionLED.setState(on);
+    }
+
     public void saveModule() {
         ConfigManager.getInstance()
                 .saveModule(

photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java

@@ -24,12 +24,10 @@ import org.opencv.core.Point;
 import org.photonvision.common.dataflow.DataChangeSubscriber;
 import org.photonvision.common.dataflow.events.DataChangeEvent;
 import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
-import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.common.util.numbers.IntegerCouple;
-import org.photonvision.vision.camera.CameraQuirk;
 import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.UICalibrationData;
@@ -102,35 +100,6 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
                         parentModule.setPipeline(index);
                         parentModule.saveAndBroadcastAll();
                         return;
-                    case "dimLED":
-                        if (parentModule.cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-                            var dimPercentage = (int) newPropValue;
-                            HardwareManager.getInstance().setBrightnessPercentage(dimPercentage);
-                        }
-                        return;
-                    case "blinkLED":
-                        if (parentModule.cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-                            var params = (Pair<Integer, Integer>) newPropValue;
-                            HardwareManager.getInstance().blinkLEDs(params.getLeft(), params.getRight());
-                        }
-                        return;
-                    case "setLED":
-                        if (parentModule.cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-                            var state = (boolean) newPropValue;
-                            if (state) HardwareManager.getInstance().turnLEDsOn();
-                            else HardwareManager.getInstance().turnLEDsOff();
-                        }
-                        return;
-                    case "toggleLED":
-                        if (parentModule.cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-                            HardwareManager.getInstance().toggleLEDs();
-                        }
-                        return;
-                    case "shutdownLEDs":
-                        if (parentModule.cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-                            HardwareManager.getInstance().shutdown();
-                        }
-                        return;
                     case "startcalibration":
                         var data = UICalibrationData.fromMap((Map<String, Object>) newPropValue);
                         parentModule.startCalibration(data);