diff --git a/photon-server/build.gradle b/photon-server/build.gradle index a8a0cb6fa..64ffe5783 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -30,10 +30,6 @@ ext { dependencies { implementation "io.javalin:javalin:3.7.0" - compile group: 'eu.xeli', name: 'jpigpio_2.12', version: '0.1.0' - - - implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0" implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java index f23776231..9ba231db3 100644 --- a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java +++ b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java @@ -31,9 +31,7 @@ public class HardwareConfig { public final ArrayList ledPins; public final String ledSetCommand; public final boolean ledsCanDim; - public final ArrayList ledPWMRange; - public final String ledPWMSetRange; - public final int ledPWMFrequency; + public final ArrayList ledBrightnessRange; public final String ledDimCommand; public final String ledBlinkCommand; public final ArrayList statusRGBPins; @@ -58,10 +56,8 @@ public class HardwareConfig { ledPins = new ArrayList<>(); ledSetCommand = ""; ledsCanDim = false; - ledPWMRange = new ArrayList<>(); + ledBrightnessRange = new ArrayList<>(); statusRGBPins = new ArrayList<>(); - ledPWMFrequency = 0; - ledPWMSetRange = ""; ledDimCommand = ""; cpuTempCommand = ""; @@ -85,12 +81,10 @@ public class HardwareConfig { ArrayList ledPins, String ledSetCommand, boolean ledsCanDim, - ArrayList statusRGBPins, - String ledPWMSetRange, - int ledPWMFrequency, + ArrayList ledBrightnessRange, String ledDimCommand, String ledBlinkCommand, - ArrayList ledPWMRange, + ArrayList statusRGBPins, String cpuTempCommand, String cpuMemoryCommand, String cpuUtilCommand, @@ -106,9 +100,7 @@ public class HardwareConfig { this.ledPins = ledPins; this.ledSetCommand = ledSetCommand; this.ledsCanDim = ledsCanDim; - this.ledPWMRange = ledPWMRange; - this.ledPWMSetRange = ledPWMSetRange; - this.ledPWMFrequency = ledPWMFrequency; + this.ledBrightnessRange = ledBrightnessRange; this.ledDimCommand = ledDimCommand; this.ledBlinkCommand = ledBlinkCommand; this.statusRGBPins = statusRGBPins; @@ -117,10 +109,10 @@ public class HardwareConfig { this.cpuUtilCommand = cpuUtilCommand; this.gpuMemoryCommand = gpuMemoryCommand; this.ramUtilCommand = ramUtilCommand; + this.gpuMemUsageCommand = gpuMemUsageCommand; this.restartHardwareCommand = restartHardwareCommand; this.vendorFOV = vendorFOV; this.blacklistedResIndices = blacklistedResIndices; - this.gpuMemUsageCommand = gpuMemUsageCommand; } public final boolean hasPresetFOV() { diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index d1314f69d..293a63407 100644 --- a/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java @@ -101,7 +101,8 @@ public class PhotonConfiguration { .collect(Collectors.toList())); var lightingConfig = new UILightingConfig(); - // TODO set constants + lightingConfig.brightness = hardwareSettings.ledBrightnessPercentage; + lightingConfig.supported = Platform.isRaspberryPi(); // TODO check non pi?? settingsSubmap.put("lighting", SerializationUtils.objectToHashMap(lightingConfig)); var generalSubmap = new HashMap(); diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/CustomGPIO.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/CustomGPIO.java index 2f6a2adc7..c2ad93d57 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/CustomGPIO.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/CustomGPIO.java @@ -17,15 +17,12 @@ package org.photonvision.common.hardware.GPIO; -import java.util.ArrayList; -import java.util.List; import org.photonvision.common.configuration.HardwareConfig; import org.photonvision.common.hardware.Platform; public class CustomGPIO extends GPIOBase { private boolean currentState; - private List pwmRange = new ArrayList<>(); private final int port; public CustomGPIO(int port) { @@ -75,22 +72,6 @@ public class CustomGPIO extends GPIOBase { return currentState; } - @Override - public void setPwmRangeImpl(List 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 getPwmRangeImpl() { - return pwmRange; - } - @Override public void blinkImpl(int pulseTimeMillis, int blinks) { execute( @@ -103,8 +84,6 @@ public class CustomGPIO extends GPIOBase { @Override 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") @@ -115,7 +94,6 @@ public class CustomGPIO extends GPIOBase { public static void setConfig(HardwareConfig config) { if (Platform.isRaspberryPi()) return; commands.replace("setState", config.ledSetCommand); - commands.replace("setRange", config.ledPWMSetRange); commands.replace("dim", config.ledDimCommand); commands.replace("blink", config.ledBlinkCommand); } diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java index 0b2ea6b77..cb89ea8ed 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java @@ -19,7 +19,6 @@ package org.photonvision.common.hardware.GPIO; import java.util.Arrays; import java.util.HashMap; -import java.util.List; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ShellExec; @@ -32,7 +31,6 @@ public abstract class GPIOBase { new HashMap<>() { { put("setState", ""); - put("setRange", ""); put("shutdown", ""); put("dim", ""); put("blink", ""); @@ -81,22 +79,6 @@ public abstract class GPIOBase { public abstract boolean getStateImpl(); - public final void setPwmRange(List range) { - if (getPinNumber() != -1) { - setPwmRangeImpl(range); - } - } - - protected abstract void setPwmRangeImpl(List range); - - public final List getPwmRange() { - if (getPinNumber() != -1) { - return getPwmRangeImpl(); - } else return List.of(0, 255); - } - - protected abstract List getPwmRangeImpl(); - public final void blink(int pulseTimeMillis, int blinks) { if (getPinNumber() != -1) { blinkImpl(pulseTimeMillis, blinks); @@ -107,6 +89,8 @@ public abstract class GPIOBase { public final void setBrightness(int brightness) { if (getPinNumber() != -1) { + if (brightness > 100) brightness = 100; + if (brightness < 0) brightness = 0; setBrightnessImpl(brightness); } } diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java deleted file mode 100644 index 28dc61815..000000000 --- a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/PiGPIO.java +++ /dev/null @@ -1,201 +0,0 @@ -/* - * 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 . - */ - -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; -import eu.xeli.jpigpio.Pulse; -import java.util.ArrayList; -import java.util.List; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; - -public class PiGPIO extends GPIOBase { - private static final Logger logger = new Logger(PiGPIO.class, LogGroup.General); - private final ArrayList 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) { - 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 { - cancelWave(); - getPigpioDaemon().gpioWrite(port, state); - } catch (PigpioException e) { - logger.error("Could not set pin state on port " + port, e); - } - } - - @Override - public boolean shutdown() { - try { - getPigpioDaemon().gpioTerminate(); - } catch (PigpioException e) { - logger.error("Could not terminate GPIO instance", e); - return false; - } - return true; - } - - @Override - 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 void setPwmRangeImpl(List range) { - try { - cancelWave(); - getPigpioDaemon().setPWMRange(port, range.get(0)); - } catch (PigpioException e) { - logger.error("Could not set PWM range on port " + port, e); - } - } - - @Override - public List 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; - } - logger.error("Failed to send wave: " + error); - } - - } catch (PigpioException e) { - logger.error("Could not set blink on port " + port, e); - } - } - - @Override - 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(); - } - } - - private static class Singleton { - public static JPigpio INSTANCE; - - static { - try { - // Make sure daemon is running before connecting to it - execute("pigpiod"); - INSTANCE = new PigpioSocket("localhost", 8888); - } catch (PigpioException e) { - logger.error("Could not connect to pigpio daemon."); - e.printStackTrace(); - } - } - } -} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioCommand.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioCommand.java new file mode 100644 index 000000000..1c4e2a0e6 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioCommand.java @@ -0,0 +1,40 @@ +/* + * 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 . + */ + +package org.photonvision.common.hardware.GPIO.pi; + +@SuppressWarnings("SpellCheckingInspection") +public enum PigpioCommand { + PCMD_READ(3), // int gpio_read(unsigned gpio) + PCMD_WRITE(4), // int gpio_write(unsigned gpio, unsigned level) + PCMD_WVCLR(27), // int wave_clear(void) + PCMD_WVAG(28), // int wave_add_generic(unsigned numPulses, gpioPulse_t *pulses) + PCMD_WVHLT(33), // int wave_tx_stop(void) + PCMD_WVCRE(49), // int wave_create(void) + PCMD_WVDEL(50), // int wave_delete(unsigned wave_id) + PCMD_WVTX(51), // int wave_tx_send(unsigned wave_id) (once) + PCMD_WVTXR(52), // int wave_tx_send(unsigned wave_id) (repeat) + PCMD_GDC(83), // int get_duty_cyle(unsigned user_gpio) + PCMD_HP(86), // int hardware_pwm(unsigned gpio, unsigned PWMfreq, unsigned PWMduty) + PCMD_WVTXM(100); // int wave_tx_send(unsigned wave_id, unsigned wave_mode) + + public final int value; + + PigpioCommand(int value) { + this.value = value; + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioException.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioException.java new file mode 100644 index 000000000..8f133c698 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioException.java @@ -0,0 +1,344 @@ +/* + * 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 . + */ + +package org.photonvision.common.hardware.GPIO.pi; + +import java.util.HashMap; + +/** +* A class that defines the exceptions that can be thrown by Pigpio. +* +*

Credit to nkolban +* https://github.com/nkolban/jpigpio/blob/master/JPigpio/src/jpigpio/PigpioException.java +*/ +@SuppressWarnings({"SpellCheckingInspection", "unused", "RedundantSuppression"}) +public class PigpioException extends Exception { + + private int rc = -99999999; + private static final long serialVersionUID = 443595760654129068L; + + public PigpioException() { + super(); + } + + public PigpioException(int rc) { + super(); + this.rc = rc; + } + + public PigpioException(int rc, String msg) { + super(msg); + this.rc = rc; + } + + public PigpioException(String arg0, Throwable arg1, boolean arg2, boolean arg3) { + super(arg0, arg1, arg2, arg3); + } + + public PigpioException(String arg0, Throwable arg1) { + super(arg0, arg1); + } + + public PigpioException(String arg0) { + super(arg0); + } + + public PigpioException(Throwable arg0) { + super(arg0); + } + + @Override + public String getMessage() { + return "(" + rc + ") " + getMessageForError(rc); + } + + /** + * Retrieve the error code that was returned by the underlying Pigpio call. + * + * @return The error code that was returned by the underlying Pigpio call. + */ + public int getErrorCode() { + return rc; + } // End of getErrorCode + + // Public constants for the error codes that can be thrown by Pigpio + public static final int PI_INIT_FAILED = -1; // gpioInitialise failed + public static final int PI_BAD_USER_GPIO = -2; // gpio not 0-31 + public static final int PI_BAD_GPIO = -3; // gpio not 0-53 + public static final int PI_BAD_MODE = -4; // mode not 0-7 + public static final int PI_BAD_LEVEL = -5; // level not 0-1 + public static final int PI_BAD_PUD = -6; // pud not 0-2 + public static final int PI_BAD_PULSEWIDTH = -7; // pulsewidth not 0 or 500-2500 + public static final int PI_BAD_DUTYCYCLE = -8; // dutycycle outside set range + public static final int PI_BAD_TIMER = -9; // timer not 0-9 + public static final int PI_BAD_MS = -10; // ms not 10-60000 + public static final int PI_BAD_TIMETYPE = -11; // timetype not 0-1 + public static final int PI_BAD_SECONDS = -12; // seconds < 0 + public static final int PI_BAD_MICROS = -13; // micros not 0-999999 + public static final int PI_TIMER_FAILED = -14; // gpioSetTimerFunc failed + public static final int PI_BAD_WDOG_TIMEOUT = -15; // timeout not 0-60000 + public static final int PI_NO_ALERT_FUNC = -16; // DEPRECATED + public static final int PI_BAD_CLK_PERIPH = -17; // clock peripheral not 0-1 + public static final int PI_BAD_CLK_SOURCE = -18; // DEPRECATED + public static final int PI_BAD_CLK_MICROS = -19; // clock micros not 1, 2, 4, 5, 8, or 10 + public static final int PI_BAD_BUF_MILLIS = -20; // buf millis not 100-10000 + public static final int PI_BAD_DUTYRANGE = -21; // dutycycle range not 25-40000 + public static final int PI_BAD_DUTY_RANGE = -21; // DEPRECATED (use PI_BAD_DUTYRANGE) + public static final int PI_BAD_SIGNUM = -22; // signum not 0-63 + public static final int PI_BAD_PATHNAME = -23; // can't open pathname + public static final int PI_NO_HANDLE = -24; // no handle available + public static final int PI_BAD_HANDLE = -25; // unknown handle + public static final int PI_BAD_IF_FLAGS = -26; // ifFlags > 3 + public static final int PI_BAD_CHANNEL = -27; // DMA channel not 0-14 + public static final int PI_BAD_PRIM_CHANNEL = -27; // DMA primary channel not 0-14 + public static final int PI_BAD_SOCKET_PORT = -28; // socket port not 1024-32000 + public static final int PI_BAD_FIFO_COMMAND = -29; // unrecognized fifo command + public static final int PI_BAD_SECO_CHANNEL = -30; // DMA secondary channel not 0-6 + public static final int PI_NOT_INITIALISED = -31; // function called before gpioInitialise + public static final int PI_INITIALISED = -32; // function called after gpioInitialise + public static final int PI_BAD_WAVE_MODE = -33; // waveform mode not 0-1 + public static final int PI_BAD_CFG_INTERNAL = -34; // bad parameter in gpioCfgInternals call + public static final int PI_BAD_WAVE_BAUD = -35; // baud rate not 50-250K(RX)/50-1M(TX) + public static final int PI_TOO_MANY_PULSES = -36; // waveform has too many pulses + public static final int PI_TOO_MANY_CHARS = -37; // waveform has too many chars + public static final int PI_NOT_SERIAL_GPIO = -38; // no serial read in progress on gpio + public static final int PI_BAD_SERIAL_STRUC = -39; // bad (null) serial structure parameter + public static final int PI_BAD_SERIAL_BUF = -40; // bad (null) serial buf parameter + public static final int PI_NOT_PERMITTED = -41; // gpio operation not permitted + public static final int PI_SOME_PERMITTED = -42; // one or more gpios not permitted + public static final int PI_BAD_WVSC_COMMND = -43; // bad WVSC subcommand + public static final int PI_BAD_WVSM_COMMND = -44; // bad WVSM subcommand + public static final int PI_BAD_WVSP_COMMND = -45; // bad WVSP subcommand + public static final int PI_BAD_PULSELEN = -46; // trigger pulse length not 1-100 + public static final int PI_BAD_SCRIPT = -47; // invalid script + public static final int PI_BAD_SCRIPT_ID = -48; // unknown script id + public static final int PI_BAD_SER_OFFSET = -49; // add serial data offset > 30 minutes + public static final int PI_GPIO_IN_USE = -50; // gpio already in use + public static final int PI_BAD_SERIAL_COUNT = -51; // must read at least a byte at a time + public static final int PI_BAD_PARAM_NUM = -52; // script parameter id not 0-9 + public static final int PI_DUP_TAG = -53; // script has duplicate tag + public static final int PI_TOO_MANY_TAGS = -54; // script has too many tags + public static final int PI_BAD_SCRIPT_CMD = -55; // illegal script command + public static final int PI_BAD_VAR_NUM = -56; // script variable id not 0-149 + public static final int PI_NO_SCRIPT_ROOM = -57; // no more room for scripts + public static final int PI_NO_MEMORY = -58; // can't allocate temporary memory + public static final int PI_SOCK_READ_FAILED = -59; // socket read failed + public static final int PI_SOCK_WRIT_FAILED = -60; // socket write failed + public static final int PI_TOO_MANY_PARAM = -61; // too many script parameters (> 10) + public static final int PI_NOT_HALTED = -62; // script already running or failed + public static final int PI_BAD_TAG = -63; // script has unresolved tag + public static final int PI_BAD_MICS_DELAY = -64; // bad MICS delay (too large) + public static final int PI_BAD_MILS_DELAY = -65; // bad MILS delay (too large) + public static final int PI_BAD_WAVE_ID = -66; // non existent wave id + public static final int PI_TOO_MANY_CBS = -67; // No more CBs for waveform + public static final int PI_TOO_MANY_OOL = -68; // No more OOL for waveform + public static final int PI_EMPTY_WAVEFORM = -69; // attempt to create an empty waveform + public static final int PI_NO_WAVEFORM_ID = -70; // no more waveforms + public static final int PI_I2C_OPEN_FAILED = -71; // can't open I2C device + public static final int PI_SER_OPEN_FAILED = -72; // can't open serial device + public static final int PI_SPI_OPEN_FAILED = -73; // can't open SPI device + public static final int PI_BAD_I2C_BUS = -74; // bad I2C bus + public static final int PI_BAD_I2C_ADDR = -75; // bad I2C address + public static final int PI_BAD_SPI_CHANNEL = -76; // bad SPI channel + public static final int PI_BAD_FLAGS = -77; // bad i2c/spi/ser open flags + public static final int PI_BAD_SPI_SPEED = -78; // bad SPI speed + public static final int PI_BAD_SER_DEVICE = -79; // bad serial device name + public static final int PI_BAD_SER_SPEED = -80; // bad serial baud rate + public static final int PI_BAD_PARAM = -81; // bad i2c/spi/ser parameter + public static final int PI_I2C_WRITE_FAILED = -82; // i2c write failed + public static final int PI_I2C_READ_FAILED = -83; // i2c read failed + public static final int PI_BAD_SPI_COUNT = -84; // bad SPI count + public static final int PI_SER_WRITE_FAILED = -85; // ser write failed + public static final int PI_SER_READ_FAILED = -86; // ser read failed + public static final int PI_SER_READ_NO_DATA = -87; // ser read no data available + public static final int PI_UNKNOWN_COMMAND = -88; // unknown command + public static final int PI_SPI_XFER_FAILED = -89; // spi xfer/read/write failed + public static final int PI_BAD_POINTER = -90; // bad (NULL) pointer + public static final int PI_NO_AUX_SPI = -91; // need a A+/B+/Pi2 for auxiliary SPI + public static final int PI_NOT_PWM_GPIO = -92; // gpio is not in use for PWM + public static final int PI_NOT_SERVO_GPIO = -93; // gpio is not in use for servo pulses + public static final int PI_NOT_HCLK_GPIO = -94; // gpio has no hardware clock + public static final int PI_NOT_HPWM_GPIO = -95; // gpio has no hardware PWM + public static final int PI_BAD_HPWM_FREQ = -96; // hardware PWM frequency not 1-125M + public static final int PI_BAD_HPWM_DUTY = -97; // hardware PWM dutycycle not 0-1M + public static final int PI_BAD_HCLK_FREQ = -98; // hardware clock frequency not 4689-250M + public static final int PI_BAD_HCLK_PASS = -99; // need password to use hardware clock 1 + public static final int PI_HPWM_ILLEGAL = -100; // illegal, PWM in use for main clock + public static final int PI_BAD_DATABITS = -101; // serial data bits not 1-32 + public static final int PI_BAD_STOPBITS = -102; // serial (half) stop bits not 2-8 + public static final int PI_MSG_TOOBIG = -103; // socket/pipe message too big + public static final int PI_BAD_MALLOC_MODE = -104; // bad memory allocation mode + public static final int PI_TOO_MANY_SEGS = -105; // too many I2C transaction parts + public static final int PI_BAD_I2C_SEG = -106; // a combined I2C transaction failed + public static final int PI_BAD_SMBUS_CMD = -107; + public static final int PI_NOT_I2C_GPIO = -108; + public static final int PI_BAD_I2C_WLEN = -109; + public static final int PI_BAD_I2C_RLEN = -110; + public static final int PI_BAD_I2C_CMD = -111; + public static final int PI_BAD_I2C_BAUD = -112; + public static final int PI_CHAIN_LOOP_CNT = -113; + public static final int PI_BAD_CHAIN_LOOP = -114; + public static final int PI_CHAIN_COUNTER = -115; + public static final int PI_BAD_CHAIN_CMD = -116; + public static final int PI_BAD_CHAIN_DELAY = -117; + public static final int PI_CHAIN_NESTING = -118; + public static final int PI_CHAIN_TOO_BIG = -119; + public static final int PI_DEPRECATED = -120; + public static final int PI_BAD_SER_INVERT = -121; + public static final int PI_BAD_EDGE = -122; + public static final int PI_BAD_ISR_INIT = -123; + public static final int PI_BAD_FOREVER = -124; + public static final int PI_BAD_FILTER = -125; + + public static final int PI_PIGIF_ERR_0 = -2000; + public static final int PI_PIGIF_ERR_99 = -2099; + + public static final int PI_CUSTOM_ERR_0 = -3000; + public static final int PI_CUSTOM_ERR_999 = -3999; + + private static final HashMap errorMessages = new HashMap<>(); + + static { + errorMessages.put(PI_INIT_FAILED, "pigpio initialisation failed"); + errorMessages.put(PI_BAD_USER_GPIO, "GPIO not 0-31"); + errorMessages.put(PI_BAD_GPIO, "GPIO not 0-53"); + errorMessages.put(PI_BAD_MODE, "mode not 0-7"); + errorMessages.put(PI_BAD_LEVEL, "level not 0-1"); + errorMessages.put(PI_BAD_PUD, "pud not 0-2"); + errorMessages.put(PI_BAD_PULSEWIDTH, "pulsewidth not 0 or 500-2500"); + errorMessages.put(PI_BAD_DUTYCYCLE, "dutycycle not 0-range (default 255)"); + errorMessages.put(PI_BAD_TIMER, "timer not 0-9"); + errorMessages.put(PI_BAD_MS, "ms not 10-60000"); + errorMessages.put(PI_BAD_TIMETYPE, "timetype not 0-1"); + errorMessages.put(PI_BAD_SECONDS, "seconds < 0"); + errorMessages.put(PI_BAD_MICROS, "micros not 0-999999"); + errorMessages.put(PI_TIMER_FAILED, "gpioSetTimerFunc failed"); + errorMessages.put(PI_BAD_WDOG_TIMEOUT, "timeout not 0-60000"); + errorMessages.put(PI_NO_ALERT_FUNC, "DEPRECATED"); + errorMessages.put(PI_BAD_CLK_PERIPH, "clock peripheral not 0-1"); + errorMessages.put(PI_BAD_CLK_SOURCE, "DEPRECATED"); + errorMessages.put(PI_BAD_CLK_MICROS, "clock micros not 1, 2, 4, 5, 8, or 10"); + errorMessages.put(PI_BAD_BUF_MILLIS, "buf millis not 100-10000"); + errorMessages.put(PI_BAD_DUTYRANGE, "dutycycle range not 25-40000"); + errorMessages.put(PI_BAD_SIGNUM, "signum not 0-63"); + errorMessages.put(PI_BAD_PATHNAME, "can't open pathname"); + errorMessages.put(PI_NO_HANDLE, "no handle available"); + errorMessages.put(PI_BAD_HANDLE, "unknown handle"); + errorMessages.put(PI_BAD_IF_FLAGS, "ifFlags > 3"); + errorMessages.put(PI_BAD_CHANNEL, "DMA channel not 0-14"); + errorMessages.put(PI_BAD_SOCKET_PORT, "socket port not 1024-30000"); + errorMessages.put(PI_BAD_FIFO_COMMAND, "unknown fifo command"); + errorMessages.put(PI_BAD_SECO_CHANNEL, "DMA secondary channel not 0-14"); + errorMessages.put(PI_NOT_INITIALISED, "function called before gpioInitialise"); + errorMessages.put(PI_INITIALISED, "function called after gpioInitialise"); + errorMessages.put(PI_BAD_WAVE_MODE, "waveform mode not 0-1"); + errorMessages.put(PI_BAD_CFG_INTERNAL, "bad parameter in gpioCfgInternals call"); + errorMessages.put(PI_BAD_WAVE_BAUD, "baud rate not 50-250000(RX)/1000000(TX)"); + errorMessages.put(PI_TOO_MANY_PULSES, "waveform has too many pulses"); + errorMessages.put(PI_TOO_MANY_CHARS, "waveform has too many chars"); + errorMessages.put(PI_NOT_SERIAL_GPIO, "no bit bang serial read in progress on GPIO"); + errorMessages.put(PI_NOT_PERMITTED, "no permission to update GPIO"); + errorMessages.put(PI_SOME_PERMITTED, "no permission to update one or more GPIO"); + errorMessages.put(PI_BAD_WVSC_COMMND, "bad WVSC subcommand"); + errorMessages.put(PI_BAD_WVSM_COMMND, "bad WVSM subcommand"); + errorMessages.put(PI_BAD_WVSP_COMMND, "bad WVSP subcommand"); + errorMessages.put(PI_BAD_PULSELEN, "trigger pulse length not 1-100"); + errorMessages.put(PI_BAD_SCRIPT, "invalid script"); + errorMessages.put(PI_BAD_SCRIPT_ID, "unknown script id"); + errorMessages.put(PI_BAD_SER_OFFSET, "add serial data offset > 30 minute"); + errorMessages.put(PI_GPIO_IN_USE, "GPIO already in use"); + errorMessages.put(PI_BAD_SERIAL_COUNT, "must read at least a byte at a time"); + errorMessages.put(PI_BAD_PARAM_NUM, "script parameter id not 0-9"); + errorMessages.put(PI_DUP_TAG, "script has duplicate tag"); + errorMessages.put(PI_TOO_MANY_TAGS, "script has too many tags"); + errorMessages.put(PI_BAD_SCRIPT_CMD, "illegal script command"); + errorMessages.put(PI_BAD_VAR_NUM, "script variable id not 0-149"); + errorMessages.put(PI_NO_SCRIPT_ROOM, "no more room for scripts"); + errorMessages.put(PI_NO_MEMORY, "can't allocate temporary memory"); + errorMessages.put(PI_SOCK_READ_FAILED, "socket read failed"); + errorMessages.put(PI_SOCK_WRIT_FAILED, "socket write failed"); + errorMessages.put(PI_TOO_MANY_PARAM, "too many script parameters (> 10)"); + errorMessages.put(PI_NOT_HALTED, "script already running or failed"); + errorMessages.put(PI_BAD_TAG, "script has unresolved tag"); + errorMessages.put(PI_BAD_MICS_DELAY, "bad MICS delay (too large)"); + errorMessages.put(PI_BAD_MILS_DELAY, "bad MILS delay (too large)"); + errorMessages.put(PI_BAD_WAVE_ID, "non existent wave id"); + errorMessages.put(PI_TOO_MANY_CBS, "No more CBs for waveform"); + errorMessages.put(PI_TOO_MANY_OOL, "No more OOL for waveform"); + errorMessages.put(PI_EMPTY_WAVEFORM, "attempt to create an empty waveform"); + errorMessages.put(PI_NO_WAVEFORM_ID, "No more waveform ids"); + errorMessages.put(PI_I2C_OPEN_FAILED, "can't open I2C device"); + errorMessages.put(PI_SER_OPEN_FAILED, "can't open serial device"); + errorMessages.put(PI_SPI_OPEN_FAILED, "can't open SPI device"); + errorMessages.put(PI_BAD_I2C_BUS, "bad I2C bus"); + errorMessages.put(PI_BAD_I2C_ADDR, "bad I2C address"); + errorMessages.put(PI_BAD_SPI_CHANNEL, "bad SPI channel"); + errorMessages.put(PI_BAD_FLAGS, "bad i2c/spi/ser open flags"); + errorMessages.put(PI_BAD_SPI_SPEED, "bad SPI speed"); + errorMessages.put(PI_BAD_SER_DEVICE, "bad serial device name"); + errorMessages.put(PI_BAD_SER_SPEED, "bad serial baud rate"); + errorMessages.put(PI_BAD_PARAM, "bad i2c/spi/ser parameter"); + errorMessages.put(PI_I2C_WRITE_FAILED, "I2C write failed"); + errorMessages.put(PI_I2C_READ_FAILED, "I2C read failed"); + errorMessages.put(PI_BAD_SPI_COUNT, "bad SPI count"); + errorMessages.put(PI_SER_WRITE_FAILED, "ser write failed"); + errorMessages.put(PI_SER_READ_FAILED, "ser read failed"); + errorMessages.put(PI_SER_READ_NO_DATA, "ser read no data available"); + errorMessages.put(PI_UNKNOWN_COMMAND, "unknown command"); + errorMessages.put(PI_SPI_XFER_FAILED, "SPI xfer/read/write failed"); + errorMessages.put(PI_BAD_POINTER, "bad (NULL) pointer"); + errorMessages.put(PI_NO_AUX_SPI, "no auxiliary SPI on Pi A or B"); + errorMessages.put(PI_NOT_PWM_GPIO, "GPIO is not in use for PWM"); + errorMessages.put(PI_NOT_SERVO_GPIO, "GPIO is not in use for servo pulses"); + errorMessages.put(PI_NOT_HCLK_GPIO, "GPIO has no hardware clock"); + errorMessages.put(PI_NOT_HPWM_GPIO, "GPIO has no hardware PWM"); + errorMessages.put(PI_BAD_HPWM_FREQ, "hardware PWM frequency not 1-125M"); + errorMessages.put(PI_BAD_HPWM_DUTY, "hardware PWM dutycycle not 0-1M"); + errorMessages.put(PI_BAD_HCLK_FREQ, "hardware clock frequency not 4689-250M"); + errorMessages.put(PI_BAD_HCLK_PASS, "need password to use hardware clock 1"); + errorMessages.put(PI_HPWM_ILLEGAL, "illegal, PWM in use for main clock"); + errorMessages.put(PI_BAD_DATABITS, "serial data bits not 1-32"); + errorMessages.put(PI_BAD_STOPBITS, "serial (half) stop bits not 2-8"); + errorMessages.put(PI_MSG_TOOBIG, "socket/pipe message too big"); + errorMessages.put(PI_BAD_MALLOC_MODE, "bad memory allocation mode"); + errorMessages.put(PI_TOO_MANY_SEGS, "too many I2C transaction segments"); + errorMessages.put(PI_BAD_I2C_SEG, "an I2C transaction segment failed"); + errorMessages.put(PI_BAD_SMBUS_CMD, "SMBus command not supported"); + errorMessages.put(PI_NOT_I2C_GPIO, "no bit bang I2C in progress on GPIO"); + errorMessages.put(PI_BAD_I2C_WLEN, "bad I2C write length"); + errorMessages.put(PI_BAD_I2C_RLEN, "bad I2C read length"); + errorMessages.put(PI_BAD_I2C_CMD, "bad I2C command"); + errorMessages.put(PI_BAD_I2C_BAUD, "bad I2C baud rate, not 50-500k"); + errorMessages.put(PI_CHAIN_LOOP_CNT, "bad chain loop count"); + errorMessages.put(PI_BAD_CHAIN_LOOP, "empty chain loop"); + errorMessages.put(PI_CHAIN_COUNTER, "too many chain counters"); + errorMessages.put(PI_BAD_CHAIN_CMD, "bad chain command"); + errorMessages.put(PI_BAD_CHAIN_DELAY, "bad chain delay micros"); + errorMessages.put(PI_CHAIN_NESTING, "chain counters nested too deeply"); + errorMessages.put(PI_CHAIN_TOO_BIG, "chain is too long"); + errorMessages.put(PI_DEPRECATED, "deprecated function removed"); + errorMessages.put(PI_BAD_SER_INVERT, "bit bang serial invert not 0 or 1"); + errorMessages.put(PI_BAD_EDGE, "bad ISR edge value, not 0-2"); + errorMessages.put(PI_BAD_ISR_INIT, "bad ISR initialisation"); + errorMessages.put(PI_BAD_FOREVER, "loop forever must be last chain command"); + errorMessages.put(PI_BAD_FILTER, "bad filter parameter"); + } + + public static String getMessageForError(int errorCode) { + return errorMessages.get(errorCode); + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioPin.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioPin.java new file mode 100644 index 000000000..638bff728 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioPin.java @@ -0,0 +1,96 @@ +/* + * 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 . + */ + +package org.photonvision.common.hardware.GPIO.pi; + +import static org.photonvision.common.hardware.GPIO.pi.PigpioException.*; + +import org.photonvision.common.hardware.GPIO.GPIOBase; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +public class PigpioPin extends GPIOBase { + + public static final Logger logger = new Logger(PigpioPin.class, LogGroup.General); + private static final PigpioSocket piSocket = new PigpioSocket(); + + private final boolean isHardwarePWMPin; + private final int pinNo; + + private boolean hasFailedHardwarePWM; + + public PigpioPin(int pinNo) { + isHardwarePWMPin = pinNo == 12 || pinNo == 13 || pinNo == 17 || pinNo == 18; + this.pinNo = pinNo; + } + + @Override + public int getPinNumber() { + return pinNo; + } + + @Override + protected void setStateImpl(boolean state) { + try { + piSocket.gpioWrite(pinNo, state); + } catch (PigpioException e) { + logger.error("gpioWrite FAIL - " + e.getMessage()); + } + } + + @Override + public boolean shutdown() { + setState(false); + return true; + } + + @Override + public boolean getStateImpl() { + try { + return piSocket.gpioRead(pinNo); + } catch (PigpioException e) { + logger.error("gpioRead FAIL - " + e.getMessage()); + return false; + } + } + + @Override + protected void blinkImpl(int pulseTimeMillis, int blinks) { + try { + piSocket.generateAndSendWaveform(pulseTimeMillis, blinks, pinNo); + } catch (PigpioException e) { + logger.error("Could not set blink - " + e.getMessage()); + } + } + + @Override + protected void setBrightnessImpl(int brightness) { + if (isHardwarePWMPin) { + try { + piSocket.hardwarePWM(pinNo, 22000, (int) (1000000 * (brightness / 100.0))); + } catch (PigpioException e) { + logger.error("Failed to hardPWM - " + e.getMessage()); + } + } else if (!hasFailedHardwarePWM) { + logger.warn( + "Specified pin (" + + pinNo + + ") is not capable of hardware PWM - no action will be taken."); + hasFailedHardwarePWM = true; + } + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioPulse.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioPulse.java new file mode 100644 index 000000000..779809114 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioPulse.java @@ -0,0 +1,39 @@ +/* + * 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 . + */ + +package org.photonvision.common.hardware.GPIO.pi; + +public class PigpioPulse { + int gpioOn; + int gpioOff; + int delayMicros; + + /** + * Initialises a pulse. + * + * @param gpioOn GPIO number to switch on at the start of the pulse. If zero, then no GPIO will be + * switched on. + * @param gpioOff GPIO number to switch off at the start of the pulse. If zero, then no GPIO will + * be switched off. + * @param delayMicros the delay in microseconds before the next pulse. + */ + public PigpioPulse(int gpioOn, int gpioOff, int delayMicros) { + this.gpioOn = gpioOn != 0 ? 1 << gpioOn : 0; + this.gpioOff = gpioOff != 0 ? 1 << gpioOff : 0; + this.delayMicros = delayMicros; + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioSocket.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioSocket.java new file mode 100644 index 000000000..12b80d322 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioSocket.java @@ -0,0 +1,373 @@ +/* + * 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 . + */ + +package org.photonvision.common.hardware.GPIO.pi; + +import static org.photonvision.common.hardware.GPIO.pi.PigpioException.*; +import static org.photonvision.common.hardware.GPIO.pi.PigpioException.PI_NO_WAVEFORM_ID; + +import java.io.IOException; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +@SuppressWarnings({"SpellCheckingInspection", "unused"}) +public class PigpioSocket { + private static final Logger logger = new Logger(PigpioSocket.class, LogGroup.General); + private static final int PIGPIOD_MESSAGE_SIZE = 12; + + private PigpioSocketLock commandSocket; + private int activeWaveformID = -1; + + /** Creates and starts a socket connection to a pigpio daemon on localhost */ + public PigpioSocket() { + this("127.0.0.1", 8888); + } + + /** + * Creates and starts a socket connection to a pigpio daemon on a remote host with the specified + * address and port + * + * @param addr Address of remote pigpio daemon + * @param port Port of remote pigpio daemon + */ + public PigpioSocket(String addr, int port) { + try { + commandSocket = new PigpioSocketLock(addr, port); + } catch (IOException e) { + logger.error("Failed to create or connect to Pigpio Daemon socket", e); + } + } + + /** + * Reconnects to the pigpio daemon + * + * @throws PigpioException on failure + */ + public void reconnect() throws PigpioException { + try { + commandSocket.reconnect(); + } catch (IOException e) { + logger.error("Failed to reconnect to Pigpio Daemon socket", e); + throw new PigpioException("reconnect", e); + } + } + + /** + * Terminates the connection to the pigpio daemon + * + * @throws PigpioException on failure + */ + public void gpioTerminate() throws PigpioException { + try { + commandSocket.terminate(); + } catch (IOException e) { + logger.error("Failed to terminate connection to Pigpio Daemon socket", e); + throw new PigpioException("gpioTerminate", e); + } + } + + /** + * Read the GPIO level + * + * @param pin Pin to read from + * @return Value of the pin + * @throws PigpioException on failure + */ + public boolean gpioRead(int pin) throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_READ.value, pin); + if (retCode < 0) throw new PigpioException(retCode); + return retCode != 0; + } catch (IOException e) { + logger.error("Failed to read GPIO pin: " + pin, e); + throw new PigpioException("gpioRead", e); + } + } + + /** + * Write the GPIO level + * + * @param pin Pin to write to + * @param value Value to write + * @throws PigpioException on failure + */ + public void gpioWrite(int pin, boolean value) throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WRITE.value, pin, value ? 1 : 0); + if (retCode < 0) throw new PigpioException(retCode); + } catch (IOException e) { + logger.error("Failed to write to GPIO pin: " + pin, e); + throw new PigpioException("gpioWrite", e); + } + } + + /** + * Clears all waveforms and any data added by calls to {@link #waveAddGeneric(ArrayList)} + * + * @throws PigpioException on failure + */ + public void waveClear() throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVCLR.value); + if (retCode < 0) throw new PigpioException(retCode); + } catch (IOException e) { + logger.error("Failed to clear waveforms", e); + throw new PigpioException("waveClear", e); + } + } + + /** + * Adds a number of pulses to the current waveform + * + * @param pulses ArrayList of pulses to add + * @return the new total number of pulses in the current waveform + * @throws PigpioException on failure + */ + private int waveAddGeneric(ArrayList pulses) throws PigpioException { + // pigpio wave message format + + // I p1 0 + // I p2 0 + // I p3 pulses * 12 + // ## extension ## + // III on/off/delay * pulses + + if (pulses == null || pulses.size() == 0) return 0; + + try { + ByteBuffer bb = ByteBuffer.allocate(pulses.size() * 12); + bb.order(ByteOrder.LITTLE_ENDIAN); + for (var pulse : pulses) { + bb.putInt(pulse.gpioOn).putInt(pulse.gpioOff).putInt(pulse.delayMicros); + } + + int retCode = + commandSocket.sendCmd( + PigpioCommand.PCMD_WVAG.value, + 0, + 0, + pulses.size() * PIGPIOD_MESSAGE_SIZE, + bb.array()); + if (retCode < 0) throw new PigpioException(retCode); + + return retCode; + } catch (IOException e) { + logger.error("Failed to add pulse(s) to waveform", e); + throw new PigpioException("waveAddGeneric", e); + } + } + + /** + * Creates pulses and adds them to the current waveform + * + * @param pulseTimeMillis Pulse length in milliseconds + * @param blinks Number of times to pulse. -1 for repeat + * @param pinNo Pin to pulse + */ + private void addBlinkPulsesToWaveform(int pulseTimeMillis, int blinks, int pinNo) { + boolean repeat = blinks == -1; + + if (blinks == 0) return; + + if (repeat) { + blinks = 1; + } + + try { + ArrayList pulses = new ArrayList<>(); + var startPulse = new PigpioPulse(pinNo, 0, pulseTimeMillis * 1000); + var endPulse = new PigpioPulse(0, pinNo, pulseTimeMillis * 1000); + + for (int i = 0; i < blinks; i++) { + pulses.add(startPulse); + pulses.add(endPulse); + } + + waveAddGeneric(pulses); + pulses.clear(); + } catch (Exception e) { + e.printStackTrace(); + } + } + + /** + * Generates and sends a waveform to the given pins with the specified parameters. + * + * @param pulseTimeMillis Pulse length in milliseconds + * @param blinks Number of times to pulse. -1 for repeat + * @param pins Pins to pulse + * @throws PigpioException on failure + */ + public void generateAndSendWaveform(int pulseTimeMillis, int blinks, int... pins) + throws PigpioException { + if (pins.length == 0) return; + boolean repeat = blinks == -1; + if (blinks == 0) return; + + // stop any active waves + waveTxStop(); + waveClear(); + + if (activeWaveformID != -1) { + waveDelete(activeWaveformID); + activeWaveformID = -1; + } + + for (int pin : pins) { + addBlinkPulsesToWaveform(pulseTimeMillis, blinks, pin); + } + + int waveformId = waveCreate(); + + if (waveformId >= 0) { + if (repeat) { + waveSendRepeat(waveformId); + } else { + waveSendOnce(waveformId); + } + } else { + String error = ""; + switch (waveformId) { + 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; + } + logger.error("Failed to send wave: " + error); + } + } + + /** + * Stops the transmission of the current waveform + * + * @return success + * @throws PigpioException on failure + */ + public boolean waveTxStop() throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVHLT.value); + if (retCode < 0) throw new PigpioException(retCode); + return retCode == 0; + } catch (IOException e) { + logger.error("Failed to stop waveform", e); + throw new PigpioException("waveTxStop", e); + } + } + + /** + * Creates a waveform from the data provided by the prior calls to {@link + * #waveAddGeneric(ArrayList)} Upon success a wave ID greater than or equal to 0 is returned + * + * @return ID of the created waveform + * @throws PigpioException on failure + */ + public int waveCreate() throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVCRE.value); + if (retCode < 0) throw new PigpioException(retCode); + return retCode; + } catch (IOException e) { + logger.error("Failed to create new waveform", e); + throw new PigpioException("waveCreate", e); + } + } + + /** + * Deletes the waveform with specified wave ID + * + * @param waveId ID of the waveform to delete + * @throws PigpioException on failure + */ + public void waveDelete(int waveId) throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVDEL.value, waveId); + if (retCode < 0) throw new PigpioException(retCode); + } catch (IOException e) { + logger.error("Failed to delete wave: " + waveId, e); + throw new PigpioException("waveDelete", e); + } + } + + /** + * Transmits the waveform with specified wave ID. The waveform is sent once + * + * @param waveId ID of the waveform to transmit + * @return The number of DMA control blocks in the waveform + * @throws PigpioException on failure + */ + public int waveSendOnce(int waveId) throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVTX.value, waveId); + if (retCode < 0) throw new PigpioException(retCode); + return retCode; + } catch (IOException e) { + throw new PigpioException("waveSendOnce", e); + } + } + + /** + * Transmits the waveform with specified wave ID. The waveform cycles until cancelled (either by + * the sending of a new waveform or {@link #waveTxStop()} + * + * @param waveId ID of the waveform to transmit + * @return The number of DMA control blocks in the waveform + * @throws PigpioException on failure + */ + public int waveSendRepeat(int waveId) throws PigpioException { + try { + int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVTXR.value, waveId); + if (retCode < 0) throw new PigpioException(retCode); + return retCode; + } catch (IOException e) { + throw new PigpioException("waveSendRepeat", e); + } + } + + /** + * Starts hardware PWM on a GPIO at the specified frequency and dutycycle + * + * @param pin GPIO pin to start PWM on + * @param pwmFrequency Frequency to run at (1Hz-125MHz). Frequencies above 30MHz are unlikely to + * work + * @param pwmDuty Duty cycle to run at (0-1,000,000) + * @throws PigpioException on failure + */ + public void hardwarePWM(int pin, int pwmFrequency, int pwmDuty) throws PigpioException { + try { + ByteBuffer bb = ByteBuffer.allocate(4); + bb.order(ByteOrder.LITTLE_ENDIAN); + bb.putInt(pwmDuty); + + int retCode = + commandSocket.sendCmd(PigpioCommand.PCMD_HP.value, pin, pwmFrequency, 4, bb.array()); + if (retCode < 0) throw new PigpioException(retCode); + } catch (IOException e) { + throw new PigpioException("hardwarePWM", e); + } + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioSocketLock.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioSocketLock.java new file mode 100644 index 000000000..ceaf03950 --- /dev/null +++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioSocketLock.java @@ -0,0 +1,147 @@ +/* + * 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 . + */ + +package org.photonvision.common.hardware.GPIO.pi; + +import java.io.DataInputStream; +import java.io.DataOutputStream; +import java.io.IOException; +import java.net.Socket; +import java.nio.ByteBuffer; + +/** +* Credit to nkolban +* https://github.com/nkolban/jpigpio/blob/master/JPigpio/src/jpigpio/SocketLock.java +*/ +final class PigpioSocketLock { + private static final int replyTimeoutMillis = 1000; + + private final String addr; + private final int port; + + private Socket socket; + private DataInputStream in; + private DataOutputStream out; + + public PigpioSocketLock(String addr, int port) throws IOException { + this.addr = addr; + this.port = port; + reconnect(); + } + + public void reconnect() throws IOException { + socket = new Socket(addr, port); + out = new DataOutputStream(socket.getOutputStream()); + in = new DataInputStream(socket.getInputStream()); + } + + public void terminate() throws IOException { + in.close(); + in = null; + + out.flush(); + out.close(); + out = null; + + socket.close(); + socket = null; + } + + public synchronized int sendCmd(int cmd) throws IOException { + byte[] b = {}; + return sendCmd(cmd, 0, 0, 0, b); + } + + public synchronized int sendCmd(int cmd, int p1) throws IOException { + byte[] b = {}; + return sendCmd(cmd, p1, 0, 0, b); + } + + public synchronized int sendCmd(int cmd, int p1, int p2) throws IOException { + byte[] b = {}; + return sendCmd(cmd, p1, p2, 0, b); + } + + public synchronized int sendCmd(int cmd, int p1, int p2, int p3) throws IOException { + byte[] b = {}; + return sendCmd(cmd, p1, p2, p3, b); + } + + /** + * Send extended command to pigpiod and return result code + * + * @param cmd Command to send + * @param p1 Command parameter 1 + * @param p2 Command parameter 2 + * @param p3 Command parameter 3 (usually length of extended data - see paramater ext) + * @param ext Array of bytes containing extended data + * @return Command result code + * @throws IOException in case of network connection error + */ + @SuppressWarnings("UnusedAssignment") + public synchronized int sendCmd(int cmd, int p1, int p2, int p3, byte[] ext) throws IOException { + ByteBuffer bb = ByteBuffer.allocate(16 + ext.length); + + bb.putInt(Integer.reverseBytes(cmd)); + bb.putInt(Integer.reverseBytes(p1)); + bb.putInt(Integer.reverseBytes(p2)); + bb.putInt(Integer.reverseBytes(p3)); + + if (ext.length > 0) { + bb.put(ext); + } + + out.write(bb.array()); + out.flush(); + + int w = replyTimeoutMillis; + int a = in.available(); + + // if by any chance there is no response from pigpiod, then wait up to + // specified timeout + while (w > 0 && a < 16) { + w -= 10; + try { + Thread.sleep(10); + } catch (InterruptedException ignored) { + } + a = in.available(); + } + + // throw exception if response from pigpiod has not arrived yet + if (in.available() < 16) { + throw new IOException( + "Timeout: No response from pigpio daemon within " + replyTimeoutMillis + " ms."); + } + + int resp = Integer.reverseBytes(in.readInt()); // ignore response + resp = Integer.reverseBytes(in.readInt()); // ignore response + resp = Integer.reverseBytes(in.readInt()); // ignore response + resp = Integer.reverseBytes(in.readInt()); // contains error or response + return resp; + } + + /** + * Read all remaining bytes coming from pigpiod + * + * @param data Array to store read bytes. + * @throws IOException if unable to read from network + */ + public void readBytes(byte[] data) throws IOException { + in.readFully(data); + } +} diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java index 5ad2f87ef..c93e62d9e 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -22,9 +22,11 @@ import java.io.IOException; import org.photonvision.common.ProgramStatus; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.HardwareConfig; +import org.photonvision.common.configuration.HardwareSettings; 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.pi.PigpioSocket; import org.photonvision.common.hardware.VisionLED.VisionLEDMode; import org.photonvision.common.hardware.metrics.MetricsBase; import org.photonvision.common.logging.LogGroup; @@ -38,42 +40,55 @@ public class HardwareManager { private final Logger logger = new Logger(HardwareManager.class, LogGroup.General); private final HardwareConfig hardwareConfig; + private final HardwareSettings hardwareSettings; - @SuppressWarnings("FieldCanBeLocal") + @SuppressWarnings({"FieldCanBeLocal", "unused"}) private final StatusLED statusLED; + @SuppressWarnings("FieldCanBeLocal") private final NetworkTableEntry ledModeEntry; - @SuppressWarnings("FieldCanBeLocal") + @SuppressWarnings({"FieldCanBeLocal", "unused"}) private final NTDataChangeListener ledModeListener; public final VisionLED visionLED; // May be null if no LED is specified + private final PigpioSocket pigpioSocket; // will be null unless on Raspi + public static HardwareManager getInstance() { if (instance == null) { - instance = new HardwareManager(ConfigManager.getInstance().getConfig().getHardwareConfig()); + var conf = ConfigManager.getInstance().getConfig(); + instance = new HardwareManager(conf.getHardwareConfig(), conf.getHardwareSettings()); } return instance; } - private HardwareManager(HardwareConfig hardwareConfig) { + private HardwareManager(HardwareConfig hardwareConfig, HardwareSettings hardwareSettings) { this.hardwareConfig = hardwareConfig; + this.hardwareSettings = hardwareSettings; CustomGPIO.setConfig(hardwareConfig); MetricsBase.setConfig(hardwareConfig); + if (Platform.isRaspberryPi()) { + pigpioSocket = new PigpioSocket(); + } else { + pigpioSocket = null; + } + statusLED = hardwareConfig.statusRGBPins.size() == 3 ? new StatusLED(hardwareConfig.statusRGBPins) : null; + + var hasBrightnessRange = hardwareConfig.ledBrightnessRange.size() == 2; visionLED = hardwareConfig.ledPins.isEmpty() ? null : new VisionLED( hardwareConfig.ledPins, - hardwareConfig.ledPWMFrequency, - (hardwareConfig.ledPWMRange != null && hardwareConfig.ledPWMRange.size() == 2) - ? hardwareConfig.ledPWMRange.get(1) - : 0); + hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(0) : 0, + hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100, + pigpioSocket); ledModeEntry = NetworkTablesManager.getInstance().kRootTable.getEntry("ledMode"); ledModeEntry.setNumber(VisionLEDMode.VLM_DEFAULT.value); @@ -84,19 +99,22 @@ public class HardwareManager { Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit)); - if (visionLED != null) - visionLED.setBrightness( - ConfigManager.getInstance().getConfig().getHardwareSettings().ledBrightnessPercentage); + if (visionLED != null) { + visionLED.setBrightness(hardwareSettings.ledBrightnessPercentage); + visionLED.blink(85, 4); // bootup blink + } // Start hardware metrics thread (Disabled until implemented) // if (Platform.isLinux()) MetricsPublisher.getInstance().startTask(); } public void setBrightnessPercent(int percent) { - ConfigManager.getInstance().getConfig().getHardwareSettings().ledBrightnessPercentage = percent; - if (visionLED != null) visionLED.setBrightness(percent); - ConfigManager.getInstance().requestSave(); - logger.info("Setting led brightness to " + percent + "%"); + if (percent != hardwareSettings.ledBrightnessPercentage) { + hardwareSettings.ledBrightnessPercentage = percent; + if (visionLED != null) visionLED.setBrightness(percent); + ConfigManager.getInstance().requestSave(); + logger.info("Setting led brightness to " + percent + "%"); + } } private void onJvmExit() { diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/StatusLED.java b/photon-server/src/main/java/org/photonvision/common/hardware/StatusLED.java index 694df57a5..c9644732b 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/StatusLED.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/StatusLED.java @@ -20,7 +20,7 @@ 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; +import org.photonvision.common.hardware.GPIO.pi.PigpioPin; public class StatusLED { public final GPIOBase redLED; @@ -36,9 +36,9 @@ public class StatusLED { } if (Platform.isRaspberryPi()) { - redLED = new PiGPIO(statusLedPins.get(0)); - greenLED = new PiGPIO(statusLedPins.get(1)); - blueLED = new PiGPIO(statusLedPins.get(2)); + redLED = new PigpioPin(statusLedPins.get(0)); + greenLED = new PigpioPin(statusLedPins.get(1)); + blueLED = new PigpioPin(statusLedPins.get(2)); } else { redLED = new CustomGPIO(statusLedPins.get(0)); greenLED = new CustomGPIO(statusLedPins.get(1)); diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/VisionLED.java b/photon-server/src/main/java/org/photonvision/common/hardware/VisionLED.java index 0ff6a4e5b..3c7328d3f 100644 --- a/photon-server/src/main/java/org/photonvision/common/hardware/VisionLED.java +++ b/photon-server/src/main/java/org/photonvision/common/hardware/VisionLED.java @@ -23,25 +23,40 @@ 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.hardware.GPIO.pi.PigpioException; +import org.photonvision.common.hardware.GPIO.pi.PigpioPin; +import org.photonvision.common.hardware.GPIO.pi.PigpioSocket; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TimedTaskManager; +import org.photonvision.common.util.math.MathUtils; public class VisionLED { private static final Logger logger = new Logger(VisionLED.class, LogGroup.VisionModule); - public final List leds = new ArrayList<>(); + private final int[] ledPins; + private final List visionLEDs = new ArrayList<>(); + private final int brightnessMin; + private final int brightnessMax; + private final PigpioSocket pigpioSocket; private VisionLEDMode currentLedMode = VisionLEDMode.VLM_DEFAULT; private BooleanSupplier pipelineModeSupplier; - public VisionLED(List ledPins, int pwmFreq, int pwmRangeMax) { + private int mappedBrightnessPercentage; + + public VisionLED( + List ledPins, int brightnessMin, int brightnessMax, PigpioSocket pigpioSocket) { + this.brightnessMin = brightnessMin; + this.brightnessMax = brightnessMax; + this.pigpioSocket = pigpioSocket; + this.ledPins = ledPins.stream().mapToInt(i -> i).toArray(); ledPins.forEach( pin -> { if (Platform.isRaspberryPi()) { - leds.add(new PiGPIO(pin, pwmFreq, pwmRangeMax)); + visionLEDs.add(new PigpioPin(pin)); } else { - leds.add(new CustomGPIO(pin)); + visionLEDs.add(new CustomGPIO(pin)); } }); pipelineModeSupplier = () -> false; @@ -52,15 +67,47 @@ public class VisionLED { } public void setBrightness(int percentage) { - leds.forEach((led) -> led.setBrightness(percentage)); + mappedBrightnessPercentage = MathUtils.map(percentage, 0, 100, brightnessMin, brightnessMax); + setInternal(currentLedMode, false); + } + + public void blink(int pulseLengthMillis, int blinkCount) { + blinkImpl(pulseLengthMillis, blinkCount); + int blinkDuration = pulseLengthMillis * blinkCount * 2; + TimedTaskManager.getInstance() + .addOneShotTask(() -> setInternal(this.currentLedMode, false), blinkDuration + 150); } private void blinkImpl(int pulseLengthMillis, int blinkCount) { - leds.forEach((led) -> led.blink(pulseLengthMillis, blinkCount)); + if (Platform.isRaspberryPi()) { + try { + setStateImpl(false); // hack to ensure hardware PWM has stopped before trying to blink + pigpioSocket.generateAndSendWaveform(pulseLengthMillis, blinkCount, ledPins); + } catch (PigpioException e) { + logger.error("Failed to blink!", e); + } + } else { + for (GPIOBase led : visionLEDs) { + led.blink(pulseLengthMillis, blinkCount); + } + } } private void setStateImpl(boolean state) { - leds.forEach((led) -> led.setState(state)); + if (Platform.isRaspberryPi()) { + try { + // stop any active blink + pigpioSocket.waveTxStop(); + } catch (PigpioException e) { + logger.error("Failed to stop blink!", e); + } + } + // if the user has set an LED brightness other than 100%, use that instead + if (mappedBrightnessPercentage == 100 || !state) { + visionLEDs.forEach((led) -> led.setState(state)); + } else { + visionLEDs.forEach((led) -> led.setBrightness(mappedBrightnessPercentage)); + } } public void setState(boolean on) { @@ -108,7 +155,7 @@ public class VisionLED { setStateImpl(true); break; case VLM_BLINK: - blinkImpl(175, -1); + blinkImpl(85, -1); break; } currentLedMode = newLedMode; @@ -121,6 +168,9 @@ public class VisionLED { } else { if (currentLedMode == VisionLEDMode.VLM_DEFAULT) { switch (newLedMode) { + case VLM_DEFAULT: + setStateImpl(pipelineModeSupplier.getAsBoolean()); + break; case VLM_OFF: setStateImpl(false); break; diff --git a/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java index bc3417a21..63b595699 100644 --- a/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -22,19 +22,6 @@ import org.apache.commons.math3.util.FastMath; public class MathUtils { MathUtils() {} - public static double sigmoid(Number x) { - double bias = 0; - double a = 5; - double b = -0.05; - double k = 200; - - if (x.doubleValue() < 50) { - bias = -1.338; - } - - return ((k / (1 + Math.pow(Math.E, (a + (b * x.doubleValue()))))) + bias); - } - public static double toSlope(Number angle) { return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90)); } diff --git a/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java b/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java index fca3d3baf..fbe1274eb 100644 --- a/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java +++ b/photon-server/src/test/java/org/photonvision/hardware/HardwareConfigTest.java @@ -40,10 +40,6 @@ public class HardwareConfigTest { assertEquals(config.supportURL, "https://support.photonvision.com"); Assertions.assertArrayEquals( config.ledPins.stream().mapToInt(i -> i).toArray(), new int[] {2, 13}); - Assertions.assertArrayEquals( - config.ledPWMRange.stream().mapToInt(i -> i).toArray(), new int[] {0, 100}); - Assertions.assertEquals(config.ledPWMFrequency, 800); - CustomGPIO.setConfig(config); } catch (IOException e) { diff --git a/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java b/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java index 46d6a88d5..1dc8a626d 100644 --- a/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java +++ b/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java @@ -17,30 +17,34 @@ package org.photonvision.hardware; -import java.io.IOException; +import org.junit.jupiter.api.Assumptions; import org.junit.jupiter.api.Test; +import org.photonvision.common.hardware.GPIO.pi.PigpioException; +import org.photonvision.common.hardware.GPIO.pi.PigpioSocket; +import org.photonvision.common.hardware.HardwareManager; +import org.photonvision.common.hardware.Platform; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; public class HardwareManagerTest { + public static final Logger logger = new Logger(HardwareManager.class, LogGroup.General); + @Test - public void managementTest() throws IOException { - // var config = - // new ObjectMapper().readValue(TestUtils.getHardwareConfigJson(), - // HardwareConfig.class); - // - // var instance = HardwareManager.getInstance(); - // TODO: fix - - // instance.getGPIO(13).setPwmRange(List.of(0, 100)); - // Assertions.assertEquals(instance.getGPIO(13).getPwmRange().get(0), 0); - // Assertions.assertEquals(instance.getGPIO(13).getPwmRange().get(1), 100); - // instance.getGPIO(13).blink(250, 5); - // for (int i = 0; i < 101; i++) { - // instance.getGPIO(13).setBrightness(i); - // } - - // Assertions.assertEquals(config.statusRGBPins.get(0), -1); - // Assertions.assertEquals(config.statusRGBPins.get(1), -1); - // Assertions.assertEquals(config.statusRGBPins.get(2), -1); + public void managementTest() throws InterruptedException { + Assumptions.assumeTrue(Platform.isRaspberryPi()); + var socket = new PigpioSocket(); + try { + socket.gpioWrite(18, false); + socket.gpioWrite(13, false); + Thread.sleep(500); + for (int i = 0; i < 1000000; i++) { + int duty = 1000000 - i; + socket.hardwarePWM(18, 1000000, duty); + Thread.sleep(0, 25); + } + } catch (PigpioException e) { + logger.error("error", e); + } } } diff --git a/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java b/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java index 272e27fe9..b1b5840db 100644 --- a/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java +++ b/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java @@ -22,7 +22,7 @@ import static org.junit.jupiter.api.Assertions.*; import org.junit.jupiter.api.Test; 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.GPIO.pi.PigpioPin; import org.photonvision.common.hardware.Platform; import org.photonvision.common.hardware.metrics.CPUMetrics; import org.photonvision.common.hardware.metrics.GPUMetrics; @@ -56,7 +56,7 @@ public class HardwareTest { public void testGPIO() { GPIOBase gpio; if (Platform.isRaspberryPi()) { - gpio = new PiGPIO(18, 0, 100); + gpio = new PigpioPin(18); } else { gpio = new CustomGPIO(18); } @@ -86,7 +86,7 @@ public class HardwareTest { @Test public void testBlink() { if (!Platform.isRaspberryPi()) return; - GPIOBase pwm = new PiGPIO(18, 0, 100); + GPIOBase pwm = new PigpioPin(18); pwm.blink(125, 3); var startms = System.currentTimeMillis(); while (true) {