diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java index 9117cb99..35ad0bf3 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java @@ -16,15 +16,6 @@ */ package com.robo4j.units.rpi.accelerometer; -import java.io.IOException; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collection; -import java.util.Collections; -import java.util.List; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -36,157 +27,165 @@ import com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.DataRate; import com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.FullScale; import com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.PowerMode; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.hw.rpi.i2c.accelerometer.CalibratedAccelerometer; import com.robo4j.math.geometry.Tuple3f; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; /** * Accelerometer unit. - * + *

* Use configuration settings to compensate for inverted axes, and for * calibration. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AccelerometerLSM303Unit extends I2CRoboUnit { - /** - * This key controls the {@link PowerMode}. Use the enum name of the power - * mode you want to use. The default is NORMAL. - */ - public static final String PROPERTY_KEY_POWER_MODE = "powerMode"; - - /** - * This key controls the {@link DataRate} to use. Use the enum name of the - * data rate you want to use. The default is HZ_10. - */ - public static final String PROPERTY_KEY_RATE = "rate"; - - /** - * This key controls which axes to enable. See - * {@link AccelerometerLSM303Device}. - * - * Default is 7. See {@link AccelerometerLSM303Device#AXIS_ENABLE_ALL}. - */ - private static final String PROPERTY_KEY_AXIS_ENABLE = "axisEnable"; - - /** - * This key controls the full scale (and thereby the sensitivity) of the - * accelerometer. See {@link FullScale}. Default is G_2. - */ - private static final String PROPERTY_KEY_FULL_SCALE = "fullScale"; - - /** - * This key controls the enabling the high res mode. Default is false. - */ - private static final String PROPERTY_KEY_ENABLE_HIGH_RES = "enableHighRes"; - - /** - * This key controls how often to read the accelerometer, in ms. Default is - * 200. - */ - private static final String PROPERTY_KEY_PERIOD = "period"; - - /** - * This attribute will provide the state of the accelerometer as a - * {@link Tuple3f}. - */ - public static final String ATTRIBUTE_NAME_STATE = "state"; - - public static final Collection> KNOWN_ATTRIBUTES = List.of(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE)); - - private final Scanner scanner = new Scanner(); - - private CalibratedFloat3DDevice accelerometer; - private Integer period; - - private volatile ScheduledFuture scannerTask; - private final List requests = new ArrayList<>(); - - private class Scanner implements Runnable { - @Override - public void run() { - try { - Tuple3f value = accelerometer.read(); - for (AccelerometerRequest request : requests) { - if (request.getPredicate().test(value)) { - notify(request.getTarget(), value); - } - } - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read accelerometer!", e); - } - } - - private void notify(RoboReference target, Tuple3f value) { - target.sendMessage(new AccelerometerEvent(value)); - } - } - - /** - * Constructor. - * - * @param context - * the robo context. - * @param id - * the robo unit id. - */ - public AccelerometerLSM303Unit(RoboContext context, String id) { - super(AccelerometerRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - PowerMode powerMode = PowerMode.valueOf(configuration.getString(PROPERTY_KEY_POWER_MODE, PowerMode.NORMAL.name())); - DataRate rate = DataRate.valueOf(configuration.getString(PROPERTY_KEY_RATE, DataRate.HZ_10.name())); - Integer axisEnable = configuration.getInteger(PROPERTY_KEY_AXIS_ENABLE, AccelerometerLSM303Device.AXIS_ENABLE_ALL); - FullScale fullScale = FullScale.valueOf(configuration.getString(PROPERTY_KEY_FULL_SCALE, FullScale.G_2.name())); - Boolean enableHighRes = configuration.getBoolean(PROPERTY_KEY_ENABLE_HIGH_RES, false); - Tuple3f offsets = readFloat3D(configuration.getChildConfiguration("offsets")); - Tuple3f multipliers = readFloat3D(configuration.getChildConfiguration("multipliers")); - period = configuration.getInteger(PROPERTY_KEY_PERIOD, 200); - - try { - AccelerometerLSM303Device device = new AccelerometerLSM303Device(getBus(), getAddress(), powerMode, rate, axisEnable, fullScale, - enableHighRes); - accelerometer = new CalibratedAccelerometer(device, offsets, multipliers); - } catch (IOException e) { - throw new ConfigurationException(String.format( - "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); - } - } - - private Tuple3f readFloat3D(Configuration config) { - return new Tuple3f(config.getFloat("x", 0f), config.getFloat("y", 0f), config.getFloat("z", 0f)); - } - - @Override - public void onMessage(AccelerometerRequest message) { - super.onMessage(message); - registerRequest(message); - } - - private void registerRequest(AccelerometerRequest message) { - synchronized (this) { - requests.add(message); - } - if (scannerTask == null) { - scannerTask = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); - } - } - - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { - try { - return (R) accelerometer.read(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read the accelerometer!", e); - } - } - return super.onGetAttribute(descriptor); - } + private static final Logger LOGGER = LoggerFactory.getLogger(AccelerometerLSM303Unit.class); + + /** + * This key controls the {@link PowerMode}. Use the enum name of the power + * mode you want to use. The default is NORMAL. + */ + public static final String PROPERTY_KEY_POWER_MODE = "powerMode"; + + /** + * This key controls the {@link DataRate} to use. Use the enum name of the + * data rate you want to use. The default is HZ_10. + */ + public static final String PROPERTY_KEY_RATE = "rate"; + + /** + * This key controls which axes to enable. See + * {@link AccelerometerLSM303Device}. + *

+ * Default is 7. See {@link AccelerometerLSM303Device#AXIS_ENABLE_ALL}. + */ + private static final String PROPERTY_KEY_AXIS_ENABLE = "axisEnable"; + + /** + * This key controls the full scale (and thereby the sensitivity) of the + * accelerometer. See {@link FullScale}. Default is G_2. + */ + private static final String PROPERTY_KEY_FULL_SCALE = "fullScale"; + + /** + * This key controls the enabling the high res mode. Default is false. + */ + private static final String PROPERTY_KEY_ENABLE_HIGH_RES = "enableHighRes"; + + /** + * This key controls how often to read the accelerometer, in ms. Default is + * 200. + */ + private static final String PROPERTY_KEY_PERIOD = "period"; + + /** + * This attribute will provide the state of the accelerometer as a + * {@link Tuple3f}. + */ + public static final String ATTRIBUTE_NAME_STATE = "state"; + + public static final Collection> KNOWN_ATTRIBUTES = List.of(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE)); + + private final Scanner scanner = new Scanner(); + + private CalibratedFloat3DDevice accelerometer; + private Integer period; + + private volatile ScheduledFuture scannerTask; + private final List requests = new ArrayList<>(); + + private class Scanner implements Runnable { + @Override + public void run() { + try { + Tuple3f value = accelerometer.read(); + for (AccelerometerRequest request : requests) { + if (request.getPredicate().test(value)) { + notify(request.getTarget(), value); + } + } + } catch (IOException e) { + LOGGER.error("Failed to read accelerometer!:{}", e.getMessage(), e); + } + } + + private void notify(RoboReference target, Tuple3f value) { + target.sendMessage(new AccelerometerEvent(value)); + } + } + + /** + * Constructor. + * + * @param context the robo context. + * @param id the robo unit id. + */ + public AccelerometerLSM303Unit(RoboContext context, String id) { + super(AccelerometerRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + PowerMode powerMode = PowerMode.valueOf(configuration.getString(PROPERTY_KEY_POWER_MODE, PowerMode.NORMAL.name())); + DataRate rate = DataRate.valueOf(configuration.getString(PROPERTY_KEY_RATE, DataRate.HZ_10.name())); + Integer axisEnable = configuration.getInteger(PROPERTY_KEY_AXIS_ENABLE, AccelerometerLSM303Device.AXIS_ENABLE_ALL); + FullScale fullScale = FullScale.valueOf(configuration.getString(PROPERTY_KEY_FULL_SCALE, FullScale.G_2.name())); + Boolean enableHighRes = configuration.getBoolean(PROPERTY_KEY_ENABLE_HIGH_RES, false); + Tuple3f offsets = readFloat3D(configuration.getChildConfiguration("offsets")); + Tuple3f multipliers = readFloat3D(configuration.getChildConfiguration("multipliers")); + period = configuration.getInteger(PROPERTY_KEY_PERIOD, 200); + + try { + AccelerometerLSM303Device device = new AccelerometerLSM303Device(getBus(), getAddress(), powerMode, rate, axisEnable, fullScale, + enableHighRes); + accelerometer = new CalibratedAccelerometer(device, offsets, multipliers); + } catch (IOException e) { + throw new ConfigurationException(String.format( + "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); + } + } + + private Tuple3f readFloat3D(Configuration config) { + return new Tuple3f(config.getFloat("x", 0f), config.getFloat("y", 0f), config.getFloat("z", 0f)); + } + + @Override + public void onMessage(AccelerometerRequest message) { + super.onMessage(message); + registerRequest(message); + } + + private void registerRequest(AccelerometerRequest message) { + synchronized (this) { + requests.add(message); + } + if (scannerTask == null) { + scannerTask = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); + } + } + + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { + try { + return (R) accelerometer.read(); + } catch (IOException e) { + LOGGER.error("Failed to read the accelerometer!:{}", e.getMessage(), e); + } + } + return super.onGetAttribute(descriptor); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java index 9be60258..3ecefeab 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java @@ -23,7 +23,8 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.camera.RaspiDevice; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.EnumSet; import java.util.concurrent.atomic.AtomicBoolean; @@ -31,83 +32,82 @@ /** * Unit generates image by using Rapsberry Pi raspistill utility * - * @see raspistill - * documentation - * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) + * @see raspistill + * documentation */ public class RaspistillUnit extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(RaspistillUnit.class); + private static final EnumSet acceptedStates = EnumSet.of(LifecycleState.STARTING, + LifecycleState.STARTED); + private static final String PROPERTY_TARGET = "target"; + private final AtomicBoolean continualMode = new AtomicBoolean(false); + private final AtomicBoolean cameraProgress = new AtomicBoolean(false); - private static final EnumSet acceptedStates = EnumSet.of(LifecycleState.STARTING, - LifecycleState.STARTED); - private static final String PROPERTY_TARGET = "target"; - private final AtomicBoolean continualMode = new AtomicBoolean(false); - private final AtomicBoolean cameraProgress = new AtomicBoolean(false); - - private final RaspiDevice device = new RaspiDevice(); - private String target; + private final RaspiDevice device = new RaspiDevice(); + private String target; - public RaspistillUnit(RoboContext context, String id) { - super(RaspistillRequest.class, context, id); - } + public RaspistillUnit(RoboContext context, String id) { + super(RaspistillRequest.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - target = configuration.getString(PROPERTY_TARGET, null); - if (target == null) { - throw ConfigurationException.createMissingConfigNameException(PROPERTY_TARGET); - } - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + target = configuration.getString(PROPERTY_TARGET, null); + if (target == null) { + throw ConfigurationException.createMissingConfigNameException(PROPERTY_TARGET); + } + } - @Override - public void onMessage(RaspistillRequest message) { - processMessage(message); - } + @Override + public void onMessage(RaspistillRequest message) { + processMessage(message); + } - private void processMessage(RaspistillRequest message) { - getContext().getScheduler().execute(() -> { - if (continualMode.get()) { - stopProgress(); - } - if (message.isActive()) { - startContinualMode(message); - } else if (cameraProgress.compareAndSet(false, true)) { - createImage(message); - } - }); - } + private void processMessage(RaspistillRequest message) { + getContext().getScheduler().execute(() -> { + if (continualMode.get()) { + stopProgress(); + } + if (message.isActive()) { + startContinualMode(message); + } else if (cameraProgress.compareAndSet(false, true)) { + createImage(message); + } + }); + } - private void stopProgress() { - continualMode.set(false); - while (cameraProgress.get()); - } + private void stopProgress() { + continualMode.set(false); + while (cameraProgress.get()) ; + } - private void startContinualMode(RaspistillRequest message) { - getContext().getScheduler().execute(() -> { - continualMode.set(true); - while (acceptedStates.contains(getState()) && continualMode.get()) { - if (cameraProgress.compareAndSet(false, true)) { - createImage(message); - } - } - }); + private void startContinualMode(RaspistillRequest message) { + getContext().getScheduler().execute(() -> { + continualMode.set(true); + while (acceptedStates.contains(getState()) && continualMode.get()) { + if (cameraProgress.compareAndSet(false, true)) { + createImage(message); + } + } + }); - } + } - private void createImage(RaspistillRequest message) { - try { - final byte[] image = device.executeCommandRaspistill(message.create()); - final RoboReference targetReference = getContext().getReference(target); - if (targetReference != null && image.length > 0) { - ImageDTO imageDTO = CameraUtil.createImageDTOBydMessageAndBytes(message, image); - targetReference.sendMessage(imageDTO); - } - cameraProgress.set(false); - } catch (Exception e) { - SimpleLoggingUtil.error(getClass(), "create image", e); - } - } + private void createImage(RaspistillRequest message) { + try { + final byte[] image = device.executeCommandRaspistill(message.create()); + final RoboReference targetReference = getContext().getReference(target); + if (targetReference != null && image.length > 0) { + ImageDTO imageDTO = CameraUtil.createImageDTOBydMessageAndBytes(message, image); + targetReference.sendMessage(imageDTO); + } + cameraProgress.set(false); + } catch (Exception e) { + LOGGER.error("create image:{}", e.getMessage(), e); + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java index 3eaa7a93..e85aa596 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java @@ -16,16 +16,6 @@ */ package com.robo4j.units.rpi.gps; -import java.io.IOException; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collection; -import java.util.Collections; -import java.util.List; -import java.util.concurrent.Future; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -41,160 +31,170 @@ import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.math.geometry.Tuple3f; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collection; +import java.util.Collections; +import java.util.List; +import java.util.concurrent.Future; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; + /** * Unit for getting GPS data. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class MtkGPSUnit extends RoboUnit { - /** - * This key configures the approximate update interval, or how often to - * schedule reads from the serial port. - */ - public static final String PROPERTY_KEY_READ_INTERVAL = "readInterval"; - - /** - * This key configures the scheduler to use for scheduling reads. Either - * PLATFORM or INTERNAL. Use INTERNAL if the reads take too long and start - * disrupting the platform scheduler too much. - */ - public static final String PROPERTY_KEY_SCHEDULER = "scheduler"; - - /** - * Value for the scheduler key for using the platform scheduler. - */ - public static final String PROPERTY_VALUE_PLATFORM_SCHEDULER = "platform"; - - /** - * Value for the scheduler key for using the internal scheduler. - */ - public static final String PROPERTY_VALUE_INTERNAL_SCHEDULER = "internal"; - - /** - * This is the default value for the read interval. - */ - public static final int DEFAULT_READ_INTERVAL = MTK3339GPS.DEFAULT_READ_INTERVAL; - - /** - * This is the default value for the serial port. - */ - public static final String DEFAULT_SERIAL_PORT = MTK3339GPS.DEFAULT_GPS_PORT; - - /** - * This attribute will provide the state of the read interval. - */ - public static final String ATTRIBUTE_NAME_READ_INTERVAL = "readInterval"; - - public static final Collection> KNOWN_ATTRIBUTES = Collections - .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_READ_INTERVAL))); - - private MTK3339GPS mtk3339gps; - private String serialPort; - private int readInterval = DEFAULT_READ_INTERVAL; - private List listeners = new ArrayList<>(); - - // The future, if scheduled with the platform scheduler - private volatile ScheduledFuture scheduledFuture; - - private static class GPSEventListener implements GPSListener { - private RoboReference target; - - GPSEventListener(RoboReference target) { - this.target = target; - } - - @Override - public void onPosition(PositionEvent event) { - target.sendMessage(event); - } - - @Override - public void onVelocity(VelocityEvent event) { - target.sendMessage(event); - } - } - - public MtkGPSUnit(RoboContext context, String id) { - super(GPSRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - serialPort = configuration.getString("serialPort", DEFAULT_SERIAL_PORT); - readInterval = configuration.getInteger("readInterval", DEFAULT_READ_INTERVAL); - String scheduler = configuration.getString("scheduler", PROPERTY_VALUE_PLATFORM_SCHEDULER); - boolean usePlatformScheduler = PROPERTY_VALUE_PLATFORM_SCHEDULER.equals(scheduler); - - try { - mtk3339gps = new MTK3339GPS(serialPort, readInterval); - } catch (IOException e) { - throw new ConfigurationException("Could not instantiate GPS!", e); - } - if (usePlatformScheduler) { - scheduledFuture = getContext().getScheduler().scheduleAtFixedRate(() -> mtk3339gps.update(), 10, readInterval, - TimeUnit.MILLISECONDS); - } else { - mtk3339gps.start(); - } - } - - @Override - public Future getAttribute(AttributeDescriptor attribute) { - return super.getAttribute(attribute); - } - - @Override - public void onMessage(GPSRequest message) { - super.onMessage(message); - RoboReference targetReference = message.getTarget(); - switch (message.getOperation()) { - case REGISTER: - register(targetReference); - break; - case UNREGISTER: - unregister(targetReference); - break; - default: - SimpleLoggingUtil.error(getClass(), "Unknown operation: " + message.getOperation()); - break; - } - } - - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Integer.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_READ_INTERVAL)) { - return (R) Integer.valueOf(readInterval); - } - return super.onGetAttribute(descriptor); - } - - @Override - public void shutdown() { - if (scheduledFuture != null) { - scheduledFuture.cancel(false); - } - mtk3339gps.shutdown(); - super.shutdown(); - } - - // Private Methods - private synchronized void unregister(RoboReference targetReference) { - List copy = new ArrayList<>(listeners); - for (GPSEventListener listener : copy) { - if (targetReference.equals(listener.target)) { - listeners.remove(listener); - mtk3339gps.removeListener(listener); - // I guess you could theoretically have several registered to - // the same target, so let's keep checking... - } - } - } - - private synchronized void register(RoboReference targetReference) { - GPSEventListener listener = new GPSEventListener(targetReference); - listeners.add(listener); - mtk3339gps.addListener(listener); - } + /** + * This key configures the approximate update interval, or how often to + * schedule reads from the serial port. + */ + public static final String PROPERTY_KEY_READ_INTERVAL = "readInterval"; + + /** + * This key configures the scheduler to use for scheduling reads. Either + * PLATFORM or INTERNAL. Use INTERNAL if the reads take too long and start + * disrupting the platform scheduler too much. + */ + public static final String PROPERTY_KEY_SCHEDULER = "scheduler"; + + /** + * Value for the scheduler key for using the platform scheduler. + */ + public static final String PROPERTY_VALUE_PLATFORM_SCHEDULER = "platform"; + + /** + * Value for the scheduler key for using the internal scheduler. + */ + public static final String PROPERTY_VALUE_INTERNAL_SCHEDULER = "internal"; + + /** + * This is the default value for the read interval. + */ + public static final int DEFAULT_READ_INTERVAL = MTK3339GPS.DEFAULT_READ_INTERVAL; + + /** + * This is the default value for the serial port. + */ + public static final String DEFAULT_SERIAL_PORT = MTK3339GPS.DEFAULT_GPS_PORT; + + /** + * This attribute will provide the state of the read interval. + */ + public static final String ATTRIBUTE_NAME_READ_INTERVAL = "readInterval"; + + public static final Collection> KNOWN_ATTRIBUTES = Collections + .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_READ_INTERVAL))); + + private MTK3339GPS mtk3339gps; + private String serialPort; + private int readInterval = DEFAULT_READ_INTERVAL; + private List listeners = new ArrayList<>(); + + // The future, if scheduled with the platform scheduler + private volatile ScheduledFuture scheduledFuture; + + private static class GPSEventListener implements GPSListener { + private RoboReference target; + + GPSEventListener(RoboReference target) { + this.target = target; + } + + @Override + public void onPosition(PositionEvent event) { + target.sendMessage(event); + } + + @Override + public void onVelocity(VelocityEvent event) { + target.sendMessage(event); + } + } + + public MtkGPSUnit(RoboContext context, String id) { + super(GPSRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + serialPort = configuration.getString("serialPort", DEFAULT_SERIAL_PORT); + readInterval = configuration.getInteger("readInterval", DEFAULT_READ_INTERVAL); + String scheduler = configuration.getString("scheduler", PROPERTY_VALUE_PLATFORM_SCHEDULER); + boolean usePlatformScheduler = PROPERTY_VALUE_PLATFORM_SCHEDULER.equals(scheduler); + + try { + mtk3339gps = new MTK3339GPS(serialPort, readInterval); + } catch (IOException e) { + throw new ConfigurationException("Could not instantiate GPS!", e); + } + if (usePlatformScheduler) { + scheduledFuture = getContext().getScheduler().scheduleAtFixedRate(() -> mtk3339gps.update(), 10, readInterval, + TimeUnit.MILLISECONDS); + } else { + mtk3339gps.start(); + } + } + + @Override + public Future getAttribute(AttributeDescriptor attribute) { + return super.getAttribute(attribute); + } + + @Override + public void onMessage(GPSRequest message) { + super.onMessage(message); + RoboReference targetReference = message.getTarget(); + switch (message.getOperation()) { + case REGISTER: + register(targetReference); + break; + case UNREGISTER: + unregister(targetReference); + break; + default: + SimpleLoggingUtil.error(getClass(), "Unknown operation: " + message.getOperation()); + break; + } + } + + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Integer.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_READ_INTERVAL)) { + return (R) Integer.valueOf(readInterval); + } + return super.onGetAttribute(descriptor); + } + + @Override + public void shutdown() { + if (scheduledFuture != null) { + scheduledFuture.cancel(false); + } + mtk3339gps.shutdown(); + super.shutdown(); + } + + // Private Methods + private synchronized void unregister(RoboReference targetReference) { + List copy = new ArrayList<>(listeners); + for (GPSEventListener listener : copy) { + if (targetReference.equals(listener.target)) { + listeners.remove(listener); + mtk3339gps.removeListener(listener); + // I guess you could theoretically have several registered to + // the same target, so let's keep checking... + } + } + } + + private synchronized void register(RoboReference targetReference) { + GPSEventListener listener = new GPSEventListener(targetReference); + listeners.add(listener); + mtk3339gps.addListener(listener); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java index e527f129..1ef2140f 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java @@ -16,12 +16,6 @@ */ package com.robo4j.units.rpi.gps; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.Future; -import java.util.concurrent.ScheduledFuture; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; @@ -33,106 +27,114 @@ import com.robo4j.hw.rpi.gps.PositionEvent; import com.robo4j.hw.rpi.gps.VelocityEvent; import com.robo4j.hw.rpi.i2c.gps.TitanX1GPS; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.Future; +import java.util.concurrent.ScheduledFuture; /** * Unit for getting GPS data. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class TitanGPSUnit extends RoboUnit { - private final List listeners = new ArrayList<>(); - private TitanX1GPS titangps; - - // The future, if scheduled with the platform scheduler - private volatile ScheduledFuture scheduledFuture; - - private static class GPSEventListener implements GPSListener { - private final RoboReference target; - - private GPSEventListener(RoboReference target) { - this.target = target; - } - - @Override - public void onPosition(PositionEvent event) { - target.sendMessage(event); - } - - @Override - public void onVelocity(VelocityEvent event) { - target.sendMessage(event); - } - } - - public TitanGPSUnit(RoboContext context, String id) { - super(GPSRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - // TODO: Configuration for bus, address etc - try { - titangps = new TitanX1GPS(); - } catch (IOException e) { - throw new ConfigurationException("Could not instantiate GPS!", e); - } - titangps.start(); - } - - @Override - public Future getAttribute(AttributeDescriptor attribute) { - return super.getAttribute(attribute); - } - - @Override - public void onMessage(GPSRequest message) { - super.onMessage(message); - RoboReference targetReference = message.getTarget(); - switch (message.getOperation()) { - case REGISTER: - register(targetReference); - break; - case UNREGISTER: - unregister(targetReference); - break; - default: - SimpleLoggingUtil.error(getClass(), "Unknown operation: " + message.getOperation()); - break; - } - } - - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - return super.onGetAttribute(descriptor); - } - - @Override - public void shutdown() { - if (scheduledFuture != null) { - scheduledFuture.cancel(false); - } - titangps.shutdown(); - super.shutdown(); - } - - // Private Methods - private synchronized void unregister(RoboReference targetReference) { - List copy = new ArrayList<>(listeners); - for (GPSEventListener listener : copy) { - if (targetReference.equals(listener.target)) { - listeners.remove(listener); - titangps.removeListener(listener); - // I guess you could theoretically have several registered to - // the same target, so let's keep checking... - } - } - } - - private synchronized void register(RoboReference targetReference) { - var listener = new GPSEventListener(targetReference); - listeners.add(listener); - titangps.addListener(listener); - } + private static final Logger LOGGER = LoggerFactory.getLogger(TitanGPSUnit.class); + private final List listeners = new ArrayList<>(); + private TitanX1GPS titangps; + + // The future, if scheduled with the platform scheduler + private volatile ScheduledFuture scheduledFuture; + + private static class GPSEventListener implements GPSListener { + private final RoboReference target; + + private GPSEventListener(RoboReference target) { + this.target = target; + } + + @Override + public void onPosition(PositionEvent event) { + target.sendMessage(event); + } + + @Override + public void onVelocity(VelocityEvent event) { + target.sendMessage(event); + } + } + + public TitanGPSUnit(RoboContext context, String id) { + super(GPSRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + // TODO: Configuration for bus, address etc + try { + titangps = new TitanX1GPS(); + } catch (IOException e) { + throw new ConfigurationException("Could not instantiate GPS!", e); + } + titangps.start(); + } + + @Override + public Future getAttribute(AttributeDescriptor attribute) { + return super.getAttribute(attribute); + } + + @Override + public void onMessage(GPSRequest message) { + super.onMessage(message); + RoboReference targetReference = message.getTarget(); + switch (message.getOperation()) { + case REGISTER: + register(targetReference); + break; + case UNREGISTER: + unregister(targetReference); + break; + default: + LOGGER.error("Unknown operation:{}", message.getOperation()); + break; + } + } + + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + return super.onGetAttribute(descriptor); + } + + @Override + public void shutdown() { + if (scheduledFuture != null) { + scheduledFuture.cancel(false); + } + titangps.shutdown(); + super.shutdown(); + } + + // Private Methods + private synchronized void unregister(RoboReference targetReference) { + List copy = new ArrayList<>(listeners); + for (GPSEventListener listener : copy) { + if (targetReference.equals(listener.target)) { + listeners.remove(listener); + titangps.removeListener(listener); + // I guess you could theoretically have several registered to + // the same target, so let's keep checking... + } + } + } + + private synchronized void register(RoboReference targetReference) { + var listener = new GPSEventListener(targetReference); + listeners.add(listener); + titangps.addListener(listener); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java index 35f4923e..bf62b3ab 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java @@ -16,15 +16,6 @@ */ package com.robo4j.units.rpi.gyro; -import java.io.IOException; -import java.util.Arrays; -import java.util.Collection; -import java.util.Collections; -import java.util.HashMap; -import java.util.Map; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -35,203 +26,212 @@ import com.robo4j.hw.rpi.i2c.gyro.CalibratedGyro; import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device; import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device.Sensitivity; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.math.geometry.Tuple3f; import com.robo4j.units.rpi.I2CRoboUnit; import com.robo4j.units.rpi.gyro.GyroRequest.GyroAction; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.Arrays; +import java.util.Collection; +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; /** * Gyro unit based on the {@link GyroL3GD20Device}. Note that there can only be * ONE active notification threshold per target. If a new one is registered * before it has triggered, the new one will replace the old one. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ @WorkTrait public class GyroL3GD20Unit extends I2CRoboUnit { - /** - * This key configures the sensitivity of the gyro. Use the name of the - * Sensitivity enum. Default is DPS_245. - * - * @see Sensitivity - */ - public static final String PROPERTY_KEY_SENSITIVITY = "sensitivity"; - - /** - * This key configures the high pass filter. Set to true to enable. Default - * is true. - * - * @see GyroL3GD20Device - */ - public static final String PROPERTY_KEY_HIGH_PASS_FILTER = "enableHighPass"; - - /** - * This key configures how often to read the gyro in ms. Default is 10. - */ - public static final String PROPERTY_KEY_PERIOD = "period"; - - /** - * This attribute will provide the state of the gyro as a {@link Tuple3f}. - */ - public static final String ATTRIBUTE_NAME_STATE = "state"; - - public static final Collection> KNOWN_ATTRIBUTES = Collections - .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE))); - - private final Map, GyroNotificationEntry> activeThresholds = new HashMap<>(); - - private final GyroScanner scanner = new GyroScanner(); - - // TODO review field purpose - private Sensitivity sensitivity; - private boolean highPassFilter; - private int period; - private CalibratedGyro gyro; - private volatile ScheduledFuture readings; - - private class GyroScanner implements Runnable { - private long lastReadingTime = System.currentTimeMillis(); - private Tuple3f lastReading = new Tuple3f(0f, 0f, 0f); - - @Override - public void run() { - Tuple3f data = read(); - long newTime = System.currentTimeMillis(); - - // Trapezoid - Tuple3f tmp = new Tuple3f(data); - long deltaTime = newTime - lastReadingTime; - data.add(lastReading); - data.multiplyScalar(deltaTime / 2000.0f); - - lastReading.set(tmp); - addToDeltas(data); - lastReadingTime = newTime; - } - - private void addToDeltas(Tuple3f data) { - synchronized (GyroL3GD20Unit.this) { - for (GyroNotificationEntry notificationEntry : activeThresholds.values()) { - notificationEntry.addDelta(data); - } - } - } - - private void reset() { - lastReadingTime = System.currentTimeMillis(); - lastReading = read(); - } - - private Tuple3f read() { - try { - return gyro.read(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not read gyro, aborting.", e); - return null; - } - } - } - - /** - * Constructor. - * - * @param context - * the robo context. - * @param id - * the robo unit id. - */ - public GyroL3GD20Unit(RoboContext context, String id) { - super(GyroRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - sensitivity = Sensitivity.valueOf(configuration.getString(PROPERTY_KEY_SENSITIVITY, "DPS_245")); - period = configuration.getInteger(PROPERTY_KEY_PERIOD, 10); - highPassFilter = configuration.getBoolean(PROPERTY_KEY_HIGH_PASS_FILTER, true); - try { - gyro = new CalibratedGyro(new GyroL3GD20Device(getBus(), getAddress(), sensitivity, highPassFilter)); - } catch (IOException e) { - throw new ConfigurationException(String.format( - "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); - } - } - - @Override - public Collection> getKnownAttributes() { - return KNOWN_ATTRIBUTES; - } - - @Override - public void onMessage(GyroRequest message) { - RoboReference notificationTarget = message.getTarget(); - switch (message.getAction()) { - case CALIBRATE: - try { - gyro.calibrate(); - scanner.reset(); - if (notificationTarget != null) { - notificationTarget.sendMessage(new GyroEvent(new Tuple3f(0, 0, 0))); - } - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to calibrate!", e); - } - break; - case STOP: - stopForNotificationTarget(notificationTarget); - break; - case ONCE: - case CONTINUOUS: - if (message.getNotificationThreshold() != null) { - setUpNotification(message); - } - break; - } - } - - private void stopForNotificationTarget(RoboReference notificationTarget) { - synchronized (this) { - if (notificationTarget == null) { - activeThresholds.clear(); - } else { - activeThresholds.remove(notificationTarget); - } - if (activeThresholds.isEmpty()) { - if (readings != null) { - readings.cancel(false); - readings = null; - } - } - } - } - - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { - try { - return (R) gyro.read(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read the gyro!", e); - } - } - return super.onGetAttribute(descriptor); - } - - private void setUpNotification(GyroRequest request) { - synchronized (this) { - if (request.getAction() == GyroAction.CONTINUOUS) { - activeThresholds.put(request.getTarget(), new ContinuousGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); - } else { - activeThresholds.put(request.getTarget(), new FixedGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); - } - } - if (readings == null) { - synchronized (this) { - readings = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); - } - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(GyroL3GD20Unit.class); + /** + * This key configures the sensitivity of the gyro. Use the name of the + * Sensitivity enum. Default is DPS_245. + * + * @see Sensitivity + */ + public static final String PROPERTY_KEY_SENSITIVITY = "sensitivity"; + + /** + * This key configures the high pass filter. Set to true to enable. Default + * is true. + * + * @see GyroL3GD20Device + */ + public static final String PROPERTY_KEY_HIGH_PASS_FILTER = "enableHighPass"; + + /** + * This key configures how often to read the gyro in ms. Default is 10. + */ + public static final String PROPERTY_KEY_PERIOD = "period"; + + /** + * This attribute will provide the state of the gyro as a {@link Tuple3f}. + */ + public static final String ATTRIBUTE_NAME_STATE = "state"; + + public static final Collection> KNOWN_ATTRIBUTES = Collections + .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE))); + + private final Map, GyroNotificationEntry> activeThresholds = new HashMap<>(); + + private final GyroScanner scanner = new GyroScanner(); + + // TODO review field purpose + private Sensitivity sensitivity; + private boolean highPassFilter; + private int period; + private CalibratedGyro gyro; + private volatile ScheduledFuture readings; + + private class GyroScanner implements Runnable { + private long lastReadingTime = System.currentTimeMillis(); + private Tuple3f lastReading = new Tuple3f(0f, 0f, 0f); + + @Override + public void run() { + Tuple3f data = read(); + long newTime = System.currentTimeMillis(); + + // Trapezoid + Tuple3f tmp = new Tuple3f(data); + long deltaTime = newTime - lastReadingTime; + data.add(lastReading); + data.multiplyScalar(deltaTime / 2000.0f); + + lastReading.set(tmp); + addToDeltas(data); + lastReadingTime = newTime; + } + + private void addToDeltas(Tuple3f data) { + synchronized (GyroL3GD20Unit.this) { + for (GyroNotificationEntry notificationEntry : activeThresholds.values()) { + notificationEntry.addDelta(data); + } + } + } + + private void reset() { + lastReadingTime = System.currentTimeMillis(); + lastReading = read(); + } + + private Tuple3f read() { + try { + return gyro.read(); + } catch (IOException e) { + LOGGER.error("Could not read gyro, aborting:", e.getMessage(), e); + return null; + } + } + } + + /** + * Constructor. + * + * @param context the robo context. + * @param id the robo unit id. + */ + public GyroL3GD20Unit(RoboContext context, String id) { + super(GyroRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + sensitivity = Sensitivity.valueOf(configuration.getString(PROPERTY_KEY_SENSITIVITY, "DPS_245")); + period = configuration.getInteger(PROPERTY_KEY_PERIOD, 10); + highPassFilter = configuration.getBoolean(PROPERTY_KEY_HIGH_PASS_FILTER, true); + try { + gyro = new CalibratedGyro(new GyroL3GD20Device(getBus(), getAddress(), sensitivity, highPassFilter)); + } catch (IOException e) { + throw new ConfigurationException(String.format( + "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); + } + } + + @Override + public Collection> getKnownAttributes() { + return KNOWN_ATTRIBUTES; + } + + @Override + public void onMessage(GyroRequest message) { + RoboReference notificationTarget = message.getTarget(); + switch (message.getAction()) { + case CALIBRATE: + try { + gyro.calibrate(); + scanner.reset(); + if (notificationTarget != null) { + notificationTarget.sendMessage(new GyroEvent(new Tuple3f(0, 0, 0))); + } + } catch (IOException e) { + LOGGER.error("Failed to calibrate!:{}", e.getMessage(), e); + } + break; + case STOP: + stopForNotificationTarget(notificationTarget); + break; + case ONCE: + case CONTINUOUS: + if (message.getNotificationThreshold() != null) { + setUpNotification(message); + } + break; + } + } + + private void stopForNotificationTarget(RoboReference notificationTarget) { + synchronized (this) { + if (notificationTarget == null) { + activeThresholds.clear(); + } else { + activeThresholds.remove(notificationTarget); + } + if (activeThresholds.isEmpty()) { + if (readings != null) { + readings.cancel(false); + readings = null; + } + } + } + } + + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { + try { + return (R) gyro.read(); + } catch (IOException e) { + LOGGER.error("Failed to read the gyro!:{}", e.getMessage(), e); + } + } + return super.onGetAttribute(descriptor); + } + + private void setUpNotification(GyroRequest request) { + synchronized (this) { + if (request.getAction() == GyroAction.CONTINUOUS) { + activeThresholds.put(request.getTarget(), new ContinuousGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); + } else { + activeThresholds.put(request.getTarget(), new FixedGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); + } + } + if (readings == null) { + synchronized (this) { + readings = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); + } + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java index 00d449fb..d69bfbf2 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java @@ -28,7 +28,8 @@ import com.robo4j.hw.rpi.imu.bno.DataListener; import com.robo4j.hw.rpi.imu.bno.impl.Bno080SPIDevice; import com.robo4j.hw.rpi.imu.bno.shtp.SensorReportId; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.ArrayList; @@ -42,92 +43,92 @@ * @author Miroslav Wengner (@miragemiko) */ public class Bno080Unit extends RoboUnit { - - public static final String PROPERTY_REPORT_TYPE = "reportType"; - public static final String PROPERTY_REPORT_DELAY = "reportDelay"; - - private static final class BnoListenerEvent implements DataListener { - private final RoboReference target; - - BnoListenerEvent(RoboReference target) { - this.target = target; - } - - @Override - public void onResponse(DataEvent3f event) { - target.sendMessage(event); - } - } - - private final List listeners = new ArrayList<>(); - private Bno080Device device; - - public Bno080Unit(RoboContext context, String id) { - super(BnoRequest.class, context, id); - } - - // TODO review field purpose - private int reportDelay; - private SensorReportId report; - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - - final String reportType = configuration.getString(PROPERTY_REPORT_TYPE, null); - report = SensorReportId.valueOf(reportType.toUpperCase()); - if (report.equals(SensorReportId.NONE)) { - throw new ConfigurationException(PROPERTY_REPORT_TYPE); - } - - final Integer delay = configuration.getInteger(PROPERTY_REPORT_DELAY, null); - if (delay == null || delay <= 0) { - throw new ConfigurationException(PROPERTY_REPORT_DELAY); - } - this.reportDelay = delay; - - try { - // TODO: make device configurable - device = Bno080Factory.createDefaultSPIDevice(); - } catch (IOException | InterruptedException e) { - throw new ConfigurationException("Could not initiate device", e); - } - device.start(report, reportDelay); - } - - @Override - public void onMessage(BnoRequest message) { - RoboReference target = message.getTarget(); - switch (message.getListenerAction()) { - case REGISTER: - register(target); - break; - case UNREGISTER: - unregister(target); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("Unknown operation: %s", message)); - } - - } - - @Override - public void shutdown() { - super.shutdown(); - device.shutdown(); - } - - private synchronized void register(RoboReference target) { - BnoListenerEvent event = new BnoListenerEvent(target); - listeners.add(event); - device.addListener(event); - } - - private synchronized void unregister(RoboReference target) { - for (BnoListenerEvent l : new ArrayList<>(listeners)) { - if (target.equals(l.target)) { - listeners.remove(l); - device.removeListener(l); - } - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(Bno080Unit.class); + public static final String PROPERTY_REPORT_TYPE = "reportType"; + public static final String PROPERTY_REPORT_DELAY = "reportDelay"; + + private static final class BnoListenerEvent implements DataListener { + private final RoboReference target; + + BnoListenerEvent(RoboReference target) { + this.target = target; + } + + @Override + public void onResponse(DataEvent3f event) { + target.sendMessage(event); + } + } + + private final List listeners = new ArrayList<>(); + private Bno080Device device; + + public Bno080Unit(RoboContext context, String id) { + super(BnoRequest.class, context, id); + } + + // TODO review field purpose + private int reportDelay; + private SensorReportId report; + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + + final String reportType = configuration.getString(PROPERTY_REPORT_TYPE, null); + report = SensorReportId.valueOf(reportType.toUpperCase()); + if (report.equals(SensorReportId.NONE)) { + throw new ConfigurationException(PROPERTY_REPORT_TYPE); + } + + final Integer delay = configuration.getInteger(PROPERTY_REPORT_DELAY, null); + if (delay == null || delay <= 0) { + throw new ConfigurationException(PROPERTY_REPORT_DELAY); + } + this.reportDelay = delay; + + try { + // TODO: make device configurable + device = Bno080Factory.createDefaultSPIDevice(); + } catch (IOException | InterruptedException e) { + throw new ConfigurationException("Could not initiate device", e); + } + device.start(report, reportDelay); + } + + @Override + public void onMessage(BnoRequest message) { + RoboReference target = message.getTarget(); + switch (message.getListenerAction()) { + case REGISTER: + register(target); + break; + case UNREGISTER: + unregister(target); + break; + default: + LOGGER.error("Unknown operation: {}", message); + } + + } + + @Override + public void shutdown() { + super.shutdown(); + device.shutdown(); + } + + private synchronized void register(RoboReference target) { + BnoListenerEvent event = new BnoListenerEvent(target); + listeners.add(event); + device.addListener(event); + } + + private synchronized void unregister(RoboReference target) { + for (BnoListenerEvent l : new ArrayList<>(listeners)) { + if (target.equals(l.target)) { + listeners.remove(l); + device.removeListener(l); + } + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java index 4389cfbf..4c6cd526 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java @@ -20,7 +20,8 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.hw.rpi.imu.bno.DataEvent3f; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * DataEventListener unit listens to {@link DataEvent3f} event types produced by @@ -30,13 +31,14 @@ * @author Miroslav Wengner (@miragemiko) */ public class DataEventListenerUnit extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(DataEventListenerUnit.class); - public DataEventListenerUnit(RoboContext context, String id) { - super(DataEvent3f.class, context, id); - } + public DataEventListenerUnit(RoboContext context, String id) { + super(DataEvent3f.class, context, id); + } - @Override - public void onMessage(DataEvent3f message) { - SimpleLoggingUtil.info(getClass(), "received:" + message); - } + @Override + public void onMessage(DataEvent3f message) { + LOGGER.info("received:{}", message); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java index 1fde88a2..63eb50cf 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java @@ -23,8 +23,9 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.imu.bno.VectorEvent; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.net.LookupServiceProvider; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * BNOVectorEventListenerUnit listen to VectorEvent events produced by {@link Bno080Unit} @@ -33,34 +34,35 @@ * @author Miroslav Wengner (@miragemiko) */ public class VectorEventListenerUnit extends RoboUnit { - - public static final String ATTR_TARGET_CONTEXT = "targetContext"; - public static final String ATTR_REMOTE_UNIT = "remoteUnit"; - public VectorEventListenerUnit(RoboContext context, String id) { - super(VectorEvent.class, context, id); - } + public static final String ATTR_TARGET_CONTEXT = "targetContext"; + public static final String ATTR_REMOTE_UNIT = "remoteUnit"; + private static final Logger LOGGER = LoggerFactory.getLogger(VectorEventListenerUnit.class); - private String targetContext; - private String remoteUnit; + public VectorEventListenerUnit(RoboContext context, String id) { + super(VectorEvent.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - targetContext = configuration.getString(ATTR_TARGET_CONTEXT, null); - remoteUnit = configuration.getString(ATTR_REMOTE_UNIT, null); - } + private String targetContext; + private String remoteUnit; - @Override - public void onMessage(VectorEvent message) { - SimpleLoggingUtil.info(getClass(), "received:" + message); - RoboContext remoteContext = LookupServiceProvider.getDefaultLookupService().getContext(targetContext); - if(remoteContext != null){ - RoboReference roboReference = remoteContext.getReference(remoteUnit); - if(roboReference != null){ - roboReference.sendMessage(message); - } - } else { - SimpleLoggingUtil.info(getClass(), String.format("context not found: %s", targetContext)); - } - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + targetContext = configuration.getString(ATTR_TARGET_CONTEXT, null); + remoteUnit = configuration.getString(ATTR_REMOTE_UNIT, null); + } + + @Override + public void onMessage(VectorEvent message) { + LOGGER.info("received:{}", message); + RoboContext remoteContext = LookupServiceProvider.getDefaultLookupService().getContext(targetContext); + if (remoteContext != null) { + RoboReference roboReference = remoteContext.getReference(remoteUnit); + if (roboReference != null) { + roboReference.sendMessage(message); + } + } else { + LOGGER.warn("context not found: {}", targetContext); + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java index c3b8f0d1..47d75dcf 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java @@ -16,8 +16,6 @@ */ package com.robo4j.units.rpi.lcd; -import java.io.IOException; - import com.robo4j.ConfigurationException; import com.robo4j.LifecycleState; import com.robo4j.RoboContext; @@ -28,101 +26,105 @@ import com.robo4j.hw.rpi.i2c.adafruitlcd.Button; import com.robo4j.hw.rpi.i2c.adafruitlcd.ButtonListener; import com.robo4j.hw.rpi.i2c.adafruitlcd.ButtonPressedObserver; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * A {@link RoboUnit} for the button part of the Adafruit 16x2 character LCD * shield. Having a separate unit allows the buttons to be used independently of * the LCD. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitButtonUnit extends I2CRoboUnit { - private AdafruitLcd lcd; - private ButtonPressedObserver observer; - private String target; - private ButtonListener buttonListener; + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitButtonUnit.class); + private AdafruitLcd lcd; + private ButtonPressedObserver observer; + private String target; + private ButtonListener buttonListener; - public AdafruitButtonUnit(RoboContext context, String id) { - super(Object.class, context, id); - } + public AdafruitButtonUnit(RoboContext context, String id) { + super(Object.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - target = configuration.getString("target", null); - if (target == null) { - throw ConfigurationException.createMissingConfigNameException("target"); - } - try { - lcd = AdafruitLcdUnit.getLCD(getBus(), getAddress()); - } catch (IOException e) { - throw new ConfigurationException("Could not initialize LCD shield", e); - } - setState(LifecycleState.INITIALIZED); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + target = configuration.getString("target", null); + if (target == null) { + throw ConfigurationException.createMissingConfigNameException("target"); + } + try { + lcd = AdafruitLcdUnit.getLCD(getBus(), getAddress()); + } catch (IOException e) { + throw new ConfigurationException("Could not initialize LCD shield", e); + } + setState(LifecycleState.INITIALIZED); + } - @Override - public void start() { - final RoboReference targetRef = getContext().getReference(target); - setState(LifecycleState.STARTING); - observer = new ButtonPressedObserver(lcd); - buttonListener = (Button button) -> { - if (getState() == LifecycleState.STARTED) { - try { - switch (button) { - case UP: - targetRef.sendMessage(AdafruitButtonEnum.UP); - break; - case DOWN: - targetRef.sendMessage(AdafruitButtonEnum.DOWN); - break; - case RIGHT: - targetRef.sendMessage(AdafruitButtonEnum.LEFT); - break; - case LEFT: - targetRef.sendMessage(AdafruitButtonEnum.RIGHT); - break; - case SELECT: - targetRef.sendMessage(AdafruitButtonEnum.SELECT); - break; - default: - lcd.clear(); - lcd.setText(String.format("Button %s\nis not in use...", button.toString())); - } - } catch (IOException e) { - handleException(e); - } - } - }; + @Override + public void start() { + final RoboReference targetRef = getContext().getReference(target); + setState(LifecycleState.STARTING); + observer = new ButtonPressedObserver(lcd); + buttonListener = (Button button) -> { + if (getState() == LifecycleState.STARTED) { + try { + switch (button) { + case UP: + targetRef.sendMessage(AdafruitButtonEnum.UP); + break; + case DOWN: + targetRef.sendMessage(AdafruitButtonEnum.DOWN); + break; + case RIGHT: + targetRef.sendMessage(AdafruitButtonEnum.LEFT); + break; + case LEFT: + targetRef.sendMessage(AdafruitButtonEnum.RIGHT); + break; + case SELECT: + targetRef.sendMessage(AdafruitButtonEnum.SELECT); + break; + default: + lcd.clear(); + lcd.setText(String.format("Button %s\nis not in use...", button.toString())); + } + } catch (IOException e) { + handleException(e); + } + } + }; - observer.addButtonListener(buttonListener); - setState(LifecycleState.STARTED); - } + observer.addButtonListener(buttonListener); + setState(LifecycleState.STARTED); + } - @Override - public void stop() { - observer.removeButtonListener(buttonListener); - observer = null; - buttonListener = null; - } + @Override + public void stop() { + observer.removeButtonListener(buttonListener); + observer = null; + buttonListener = null; + } - @Override - public void shutdown() { - try { - lcd.stop(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to disconnect LCD", e); - } - } + @Override + public void shutdown() { + try { + lcd.stop(); + } catch (IOException e) { + LOGGER.error("Failed to disconnect LCD:{}", e.getMessage(), e); + } + } - //Private Methods - private void handleException(IOException e) { - setState(LifecycleState.STOPPING); - shutdown(); - setState(LifecycleState.FAILED); - } + //Private Methods + private void handleException(IOException e) { + setState(LifecycleState.STOPPING); + shutdown(); + setState(LifecycleState.FAILED); + } } \ No newline at end of file diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java index bc64a605..4ce38290 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java @@ -28,11 +28,12 @@ import com.robo4j.hw.rpi.i2c.adafruitlcd.LcdFactory; import com.robo4j.hw.rpi.i2c.adafruitlcd.impl.AdafruitLcdImpl.Direction; import com.robo4j.hw.rpi.utils.I2cBus; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CEndPoint; import com.robo4j.units.rpi.I2CRegistry; import com.robo4j.units.rpi.I2CRoboUnit; import com.robo4j.util.StringConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.Arrays; @@ -42,176 +43,164 @@ /** * A {@link RoboUnit} for the Adafruit 16x2 character LCD shield. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitLcdUnit extends I2CRoboUnit { - private static final String ATTRIBUTE_NAME_COLOR = "color"; - private static final String ATTRIBUTE_NAME_TEXT = "text"; + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitLcdUnit.class); + private static final String ATTRIBUTE_NAME_COLOR = "color"; + private static final String ATTRIBUTE_NAME_TEXT = "text"; - public static final Collection> KNOWN_ATTRIBUTES = Collections - .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(String.class, ATTRIBUTE_NAME_TEXT), - DefaultAttributeDescriptor.create(Color.class, ATTRIBUTE_NAME_COLOR))); + public static final Collection> KNOWN_ATTRIBUTES = Collections + .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(String.class, ATTRIBUTE_NAME_TEXT), + DefaultAttributeDescriptor.create(Color.class, ATTRIBUTE_NAME_COLOR))); - private final AtomicReference stringMessage = new AtomicReference<>(StringConstants.EMPTY); - private AdafruitLcd lcd; + private final AtomicReference stringMessage = new AtomicReference<>(StringConstants.EMPTY); + private AdafruitLcd lcd; - public AdafruitLcdUnit(RoboContext context, String id) { - super(LcdMessage.class, context, id); - } + public AdafruitLcdUnit(RoboContext context, String id) { + super(LcdMessage.class, context, id); + } - /** - * - * @param bus - * used bus - * @param address - * desired address + /** + * @param bus used bus + * @param address desired address */ - static AdafruitLcd getLCD(I2cBus bus, int address) throws IOException { - Object lcd = I2CRegistry.getI2CDeviceByEndPoint(new I2CEndPoint(bus, address)); - if (lcd == null) { - try { - lcd = LcdFactory.createLCD(bus, address); - // Note that we cannot catch hardware specific exceptions here, - // since they will be loaded when we run as mocked. - } catch (Exception e) { - throw new IOException(e); - } - I2CRegistry.registerI2CDevice(lcd, new I2CEndPoint(bus, address)); - } - return (AdafruitLcd) lcd; - } + static AdafruitLcd getLCD(I2cBus bus, int address) throws IOException { + Object lcd = I2CRegistry.getI2CDeviceByEndPoint(new I2CEndPoint(bus, address)); + if (lcd == null) { + try { + lcd = LcdFactory.createLCD(bus, address); + // Note that we cannot catch hardware specific exceptions here, + // since they will be loaded when we run as mocked. + } catch (Exception e) { + throw new IOException(e); + } + I2CRegistry.registerI2CDevice(lcd, new I2CEndPoint(bus, address)); + } + return (AdafruitLcd) lcd; + } - /** - * - * @param message - * the message received by this unit. - * - */ - @Override - public void onMessage(LcdMessage message) { - try { - processLcdMessage(message); - } catch (Exception e) { - SimpleLoggingUtil.debug(getClass(), "Could not accept message" + message.toString(), e); - } - } + /** + * @param message the message received by this unit. + */ + @Override + public void onMessage(LcdMessage message) { + try { + processLcdMessage(message); + } catch (Exception e) { + LOGGER.error("Could not accept message:{}", message.toString(), e); + } + } - /** - * - * @param configuration - * - unit configuration - * @throws ConfigurationException - * exception - */ - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - try { - lcd = getLCD(getBus(), getAddress()); - } catch (IOException e) { - throw new ConfigurationException("Could not initialize LCD", e); - } - } + /** + * @param configuration - unit configuration + * @throws ConfigurationException exception + */ + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + try { + lcd = getLCD(getBus(), getAddress()); + } catch (IOException e) { + throw new ConfigurationException("Could not initialize LCD", e); + } + } - @Override - public void stop() { - setState(LifecycleState.STOPPING); - try { - lcd.clear(); - lcd.setDisplayEnabled(false); - lcd.stop(); - } catch (IOException e) { - throw new AdafruitException("Could not disconnect LCD", e); - } - setState(LifecycleState.STOPPED); - } + @Override + public void stop() { + setState(LifecycleState.STOPPING); + try { + lcd.clear(); + lcd.setDisplayEnabled(false); + lcd.stop(); + } catch (IOException e) { + throw new AdafruitException("Could not disconnect LCD", e); + } + setState(LifecycleState.STOPPED); + } - /** - * @param message - * accepted message type - * @throws IOException - */ - private void processLcdMessage(LcdMessage message) throws IOException { - switch (message.getType()) { - case CLEAR: - lcd.clear(); - break; - case DISPLAY_ENABLE: - final boolean disen = Boolean.valueOf(message.getText().trim()); - lcd.setDisplayEnabled(disen); - break; - case SCROLL: - // TODO: consider enum as the constant - switch (message.getText().trim()) { - case "left": - lcd.scrollDisplay(Direction.LEFT); - break; - case "right": - lcd.scrollDisplay(Direction.RIGHT); - break; - default: - SimpleLoggingUtil.error(getClass(), "Scroll direction " + message.getText() + " is unknown"); - break; - } - break; - case SET_TEXT: - if (message.getColor() != null) { - lcd.setBacklight(message.getColor()); - } - if (message.getText() != null) { - String text = message.getText(); - lcd.setText(text); - stringMessage.set(text); - } - break; - case STOP: - lcd.stop(); - break; - default: - SimpleLoggingUtil.error(getClass(), message.getType() + "demo not supported!"); - break; - } - } + /** + * @param message accepted message type + * @throws IOException + */ + private void processLcdMessage(LcdMessage message) throws IOException { + switch (message.getType()) { + case CLEAR: + lcd.clear(); + break; + case DISPLAY_ENABLE: + final boolean disen = Boolean.valueOf(message.getText().trim()); + lcd.setDisplayEnabled(disen); + break; + case SCROLL: + // TODO: consider enum as the constant + switch (message.getText().trim()) { + case "left": + lcd.scrollDisplay(Direction.LEFT); + break; + case "right": + lcd.scrollDisplay(Direction.RIGHT); + break; + default: + LOGGER.error("Scroll direction {} is unknown", message.getText()); + break; + } + break; + case SET_TEXT: + if (message.getColor() != null) { + lcd.setBacklight(message.getColor()); + } + if (message.getText() != null) { + String text = message.getText(); + lcd.setText(text); + stringMessage.set(text); + } + break; + case STOP: + lcd.stop(); + break; + default: + LOGGER.error("demo not supported!:{}", message.getType()); + break; + } + } - @SuppressWarnings("unchecked") - @Override - public R onGetAttribute(AttributeDescriptor attribute) { - if (ATTRIBUTE_NAME_TEXT.equals(attribute.getAttributeName())) { - return (R) stringMessage.get(); - } else if (ATTRIBUTE_NAME_COLOR.equals(attribute.getAttributeName())) { - try { - return (R) lcd.getBacklight(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read the color", e); - } - } - return null; - } + @SuppressWarnings("unchecked") + @Override + public R onGetAttribute(AttributeDescriptor attribute) { + if (ATTRIBUTE_NAME_TEXT.equals(attribute.getAttributeName())) { + return (R) stringMessage.get(); + } else if (ATTRIBUTE_NAME_COLOR.equals(attribute.getAttributeName())) { + try { + return (R) lcd.getBacklight(); + } catch (IOException e) { + LOGGER.error("Failed to read the color:{}", e.getMessage(), e); + } + } + return null; + } - @Override - public Collection> getKnownAttributes() { - return KNOWN_ATTRIBUTES; - } + @Override + public Collection> getKnownAttributes() { + return KNOWN_ATTRIBUTES; + } - /** - * Fill one of the first 8 CGRAM locations with custom characters. The location - * parameter should be between 0 and 7 and pattern should provide an array of 8 - * bytes containing the pattern. e.g. you can design your custom character at - * <a - * href=http://www.quinapalus.com/hd44780udg.html> http://www.quinapalus.com/hd44780udg.html<a/> . - * To show your custom character obtain the string representation for the - * location e.g. String.format("custom char=%c", 0). - * - * @param location - * storage location for this character, between 0 and 7 - * @param pattern - * array of 8 bytes containing the character's pattern - * @throws IOException - * exception - */ - public void createChar(final int location, final byte[] pattern) throws IOException { - lcd.createChar(location, pattern); - } + /** + * Fill one of the first 8 CGRAM locations with custom characters. The location + * parameter should be between 0 and 7 and pattern should provide an array of 8 + * bytes containing the pattern. e.g. you can design your custom character at + * <a + * href=http://www.quinapalus.com/hd44780udg.html> http://www.quinapalus.com/hd44780udg.html<a/> . + * To show your custom character obtain the string representation for the + * location e.g. String.format("custom char=%c", 0). + * + * @param location storage location for this character, between 0 and 7 + * @param pattern array of 8 bytes containing the character's pattern + * @throws IOException exception + */ + public void createChar(final int location, final byte[] pattern) throws IOException { + lcd.createChar(location, pattern); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java index de075033..e9ca6774 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java @@ -19,44 +19,42 @@ import com.robo4j.RoboContext; import com.robo4j.hw.rpi.i2c.adafruitbackpack.AbstractBackpack; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * AbstractI2CBackpackUnit abstract I2C LedBackpack unit * - * @param - * backpack unit type - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ abstract class AbstractI2CBackpackUnit extends I2CRoboUnit { + static final String ATTRIBUTE_ADDRESS = "address"; + static final String ATTRIBUTE_BUS = "bus"; + static final String ATTRIBUTE_BRIGHTNESS = "brightness"; + private static final Logger LOGGER = LoggerFactory.getLogger(AbstractI2CBackpackUnit.class); - static final String ATTRIBUTE_ADDRESS = "address"; - static final String ATTRIBUTE_BUS = "bus"; - static final String ATTRIBUTE_BRIGHTNESS = "brightness"; - - AbstractI2CBackpackUnit(Class messageType, RoboContext context, String id) { - super(messageType, context, id); - } + AbstractI2CBackpackUnit(Class messageType, RoboContext context, String id) { + super(messageType, context, id); + } - void processMessage(AbstractBackpack device, DrawMessage message) { - switch (message.getType()) { - case CLEAR: - device.clear(); - break; - case PAINT: - paint(message); - break; - case DISPLAY: - paint(message); - device.display(); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("Illegal message: %s", message)); - } - } + void processMessage(AbstractBackpack device, DrawMessage message) { + switch (message.getType()) { + case CLEAR: + device.clear(); + break; + case PAINT: + paint(message); + break; + case DISPLAY: + paint(message); + device.display(); + break; + default: + LOGGER.error("Illegal message: {}", message); + } + } - abstract void paint(DrawMessage message); + abstract void paint(DrawMessage message); } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java index 487bcb40..76ba7cf0 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java @@ -17,19 +17,20 @@ package com.robo4j.units.rpi.led; -import java.io.IOException; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.i2c.adafruitbackpack.AbstractBackpack; import com.robo4j.hw.rpi.i2c.adafruitbackpack.AlphanumericDevice; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * AdafruitAlphanumericUnit - * + *

* This version of the LED backpack is designed for two dual 14-segment * "Alphanumeric" displays. These 14-segment displays normally require 18 pins * (4 'characters' and 14 total segments each) This backpack solves the @@ -38,60 +39,61 @@ * controller chip takes care of everything, drawing all the LEDs in the * background. All you have to do is write data to it using the 2-pin I2C * interface - * + *

* see https://learn.adafruit.com/adafruit-led-backpack/0-54-alphanumeric * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitAlphanumericUnit extends I2CRoboUnit { - private AlphanumericDevice device; + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitAlphanumericUnit.class); + private AlphanumericDevice device; - public AdafruitAlphanumericUnit(RoboContext context, String id) { - super(AlphaNumericMessage.class, context, id); - } + public AdafruitAlphanumericUnit(RoboContext context, String id) { + super(AlphaNumericMessage.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - int brightness = configuration.getInteger(AbstractI2CBackpackUnit.ATTRIBUTE_BRIGHTNESS, AbstractBackpack.DEFAULT_BRIGHTNESS); + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + int brightness = configuration.getInteger(AbstractI2CBackpackUnit.ATTRIBUTE_BRIGHTNESS, AbstractBackpack.DEFAULT_BRIGHTNESS); - try { - device = new AlphanumericDevice(getBus(), getAddress(), brightness); - } catch (IOException e) { - throw new ConfigurationException("Failed to instantiate device", e); - } - } + try { + device = new AlphanumericDevice(getBus(), getAddress(), brightness); + } catch (IOException e) { + throw new ConfigurationException("Failed to instantiate device", e); + } + } - @Override - public void onMessage(AlphaNumericMessage message) { - switch (message.getCommand()) { - case CLEAR: - device.clear(); - break; - case PAINT: - render(message); - break; - case DISPLAY: - render(message); - device.display(); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("Illegal message: %s", message)); - } - } + @Override + public void onMessage(AlphaNumericMessage message) { + switch (message.getCommand()) { + case CLEAR: + device.clear(); + break; + case PAINT: + render(message); + break; + case DISPLAY: + render(message); + device.display(); + break; + default: + LOGGER.error("Illegal message: {}", message); + } + } - private void render(AlphaNumericMessage message) { - if (message.getStartPosition() == -1) { - for (int i = 0; i < message.getCharacters().length; i++) { - device.addCharacter((char) message.getCharacters()[i], message.getDots()[i]); - } - } else { - for (int i = 0; i < message.getCharacters().length; i++) { - device.setCharacter((message.getStartPosition() + i) % device.getNumberOfCharacters(), (char) message.getCharacters()[i], - message.getDots()[i]); - } - } + private void render(AlphaNumericMessage message) { + if (message.getStartPosition() == -1) { + for (int i = 0; i < message.getCharacters().length; i++) { + device.addCharacter((char) message.getCharacters()[i], message.getDots()[i]); + } + } else { + for (int i = 0; i < message.getCharacters().length; i++) { + device.setCharacter((message.getStartPosition() + i) % device.getNumberOfCharacters(), (char) message.getCharacters()[i], + message.getDots()[i]); + } + } - } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java index ba7cc4b9..6cdc6286 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java @@ -16,12 +16,6 @@ */ package com.robo4j.units.rpi.lidarlite; -import java.io.IOException; -import java.util.concurrent.ExecutionException; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicInteger; -import java.util.function.Predicate; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.RoboReference; @@ -35,325 +29,331 @@ import com.robo4j.math.jfr.ScanEvent; import com.robo4j.units.rpi.I2CRoboUnit; import com.robo4j.units.rpi.pwm.PCA9685ServoUnit; -import jdk.jfr.Event; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.concurrent.ExecutionException; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.function.Predicate; /** * This unit controls a servo to do laser range sweep. - * + *

* <p> Configuration: </p> <li> <ul> pan: the servo to * use for panning the laser. Defaults to "laserscanner.servo". Set to "null" to * not use a servo for panning. </ul> <ul> servoRange: the range in * degrees that send 1.0 (full) to a servo will result in. This must be measured * by testing your hardware. Changing configuration on your servo will change * this too. </ul> </li> - * + *

* FIXME(Marcus/Mar 10, 2017): Only supports the pan servo for now. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class LaserScanner extends I2CRoboUnit { - /** - * Since the lidar lite will sometimes give some abnormal readings with very - * short range, we need to be able to filter out readings. Set this - * depending on your hardware and needs. - */ - private final static float DEFAULT_FILTER_MIN_RANGE = 0.08f; - private Predicate pointFilter; - private String pan; - private LidarLiteDevice lidar; - private float servoRange; - private float angularSpeed; - private float minimumAcquisitionTime; - private float trim; - - /** - * Filter for filtering out mis-reads. Anything closer than minRange will be - * filtered out. - */ - private static class MinRangePointFilter implements Predicate { - private final float minRange; - - private MinRangePointFilter(float minRange) { - this.minRange = minRange; - } - - @Override - public boolean test(Point2f t) { - return t.getRange() >= minRange; - } - } - - private final static class ScanJob implements Runnable { - private final AtomicInteger invokeCount = new AtomicInteger(0); - private final ScanResultImpl scanResult; - private final ScanRequest request; - private final RoboReference recipient; - private final RoboReference servo; - private final boolean lowToHigh; - private final int numberOfScans; - private long delayMicros; - private final float trim; - private final float servoRange; - private final LidarLiteDevice lidar; - private volatile float currentAngle; - private volatile boolean finished = false; - private final ScanEvent scanEvent; - - /** - * - * @param lowToHigh - * @param minimumServoMovementTime - * the minimum time for the servo to complete the movement - * over the range, in seconds - * @param minimumAcquisitionTime - * @param trim - * @param request - * @param servo - * @param servoRange - * @param lidar - * @param recipient - */ - public ScanJob(boolean lowToHigh, float minimumServoMovementTime, float minimumAcquisitionTime, float trim, ScanRequest request, - RoboReference servo, float servoRange, LidarLiteDevice lidar, RoboReference recipient, - Predicate pointFilter) { - this.lowToHigh = lowToHigh; - this.trim = trim; - this.request = request; - this.servo = servo; - this.servoRange = servoRange; - this.lidar = lidar; - this.recipient = recipient; - // one move, one first acquisition - this.numberOfScans = calculateNumberOfScans() + 1; - this.delayMicros = calculateDelay(minimumAcquisitionTime, minimumServoMovementTime); - this.currentAngle = lowToHigh ? request.getStartAngle() : request.getStartAngle() + request.getRange(); - this.scanResult = new ScanResultImpl(100, request.getStep(), pointFilter); - scanEvent = new ScanEvent(scanResult.getScanID(), getScanInfo()); - scanEvent.setScanLeftRight(lowToHigh); - JfrUtils.begin(scanEvent); - } - - @Override - public void run() { - int currentRun = invokeCount.incrementAndGet(); - if (currentRun == 1) { - // On first step, only move servo to start position - float normalizedServoTarget = getNormalizedAngle(); - servo.sendMessage(normalizedServoTarget); - return; - } else if (currentRun == 2) { - // On second, just start acquisition (no point to read yet) - startAcquisition(); - } else if (currentRun > numberOfScans) { - doScan(); - finish(); - } else { - doScan(); - } - // FIXME(Marcus/Mar 10, 2017): May want to synchronize this... - updateTargetAngle(); - } - - private void startAcquisition() { - try { - lidar.acquireRange(); - servo.sendMessage(getNormalizedAngle()); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not read laser!", e); - } - } - - private float getNormalizedAngle() { - return this.currentAngle / this.servoRange; - } - - private void doScan() { - // Read previous acquisition - try { - float readDistance = lidar.readDistance(); - // Laser was actually shooting at the previous angle, before - // moving - recalculate for that angle - float lastAngle = currentAngle + (lowToHigh ? -request.getStep() - trim : request.getStep() + trim); - scanResult.addPoint(Point2f.fromPolar(readDistance, (float) Math.toRadians(lastAngle))); - servo.sendMessage(getNormalizedAngle()); - lidar.acquireRange(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not read laser!", e); - } - } - - private void finish() { - if (!finished) { - recipient.sendMessage(scanResult); - finished = true; - JfrUtils.end(scanEvent); - JfrUtils.commit(scanEvent); - } else { - SimpleLoggingUtil.error(getClass(), "Tried to scan more laser points after being finished!"); - } - } - - private void updateTargetAngle() { - if (lowToHigh) { - currentAngle += request.getStep(); - } else { - currentAngle -= request.getStep(); - } - } - - private int calculateNumberOfScans() { - return Math.round(request.getRange() / request.getStep()); - } - - // FIXME(Marcus/Mar 10, 2017): Calculate the required delay later from - // physical model. - private long calculateDelay(float minimumAcquisitionTime, float minimumServoMovementTime) { - float delayPerStep = minimumServoMovementTime * 1000 / calculateNumberOfScans(); - // If we have a slow servo, we will need to wait for the servo to - // move. If we have a slow acquisition, we will need to wait for the - // laser before continuing - float actualDelay = Math.max(delayPerStep, minimumAcquisitionTime); - return Math.round(actualDelay * 1000.0d); - } - - private String getScanInfo() { - return String.format("start: %2.1f, end: %2.1f, step:%2.1f", request.getStartAngle(), - request.getStartAngle() + request.getRange(), request.getStep()); - } - } - - private final static class ScanFixedAngleJob implements Runnable { - private final RoboReference recipient; - private final LidarLiteDevice lidar; - private final float angle; - private final Predicate filter; - private volatile boolean firstRun = true; - - public ScanFixedAngleJob(RoboReference recipient, LidarLiteDevice lidar, float angle, Predicate filter) { - this.recipient = recipient; - this.lidar = lidar; - this.angle = angle; - this.filter = filter; - } - - @Override - public void run() { - try { - if (firstRun) { - firstRun = false; - lidar.acquireRange(); - } else { - ScanResultImpl scan = new ScanResultImpl(1, 0f, filter); - scan.addPoint(lidar.readDistance(), (float) Math.toRadians(angle)); - recipient.sendMessage(scan); - } - } catch (IOException e) { - SimpleLoggingUtil.debug(getClass(), "Failed to read lidar", e); - } - } - } - - public LaserScanner(RoboContext context, String id) { - super(ScanRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - pan = configuration.getString("servo", "laserscanner.servo"); - - // Using degrees for convenience - servoRange = (float) configuration.getFloat("servoRange", 45.0f); - - // Using angular degrees per second. - angularSpeed = configuration.getFloat("angularSpeed", 90.0f); - - // Minimum acquisition time, in ms - minimumAcquisitionTime = configuration.getFloat("minAquisitionTime", 2.0f); - - // Trim to align left to right and right to left scans (in degrees) - trim = configuration.getFloat("trim", 0.0f); - - // Set the default minimal range to filter out - float minFilterRange = configuration.getFloat("minFilterRange", DEFAULT_FILTER_MIN_RANGE); - pointFilter = new MinRangePointFilter(minFilterRange); - - try { - lidar = new LidarLiteDevice(getBus(), getAddress()); - } catch (IOException e) { - throw new ConfigurationException(String.format( - "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); - } - } - - @Override - public void onMessage(ScanRequest message) { - RoboReference servo = getPanServo(); - if (message.getRange() == 0 || message.getStep() == 0) { - scheduleFixPanScan(message, servo); - } else { - scheduleScan(message, servo); - } - } - - private void scheduleFixPanScan(ScanRequest message, RoboReference servo) { - servo.sendMessage(message.getStartAngle() / servoRange); - ScanFixedAngleJob fixedAngleJob = new ScanFixedAngleJob(message.getReceiver(), lidar, message.getStartAngle(), pointFilter); - // FIXME(Marcus/Sep 5, 2017): We should try to calculate this - we are - // now assuming that it will take little time to move the servo to the - // "new" position, however, the fact is that the new position will - // usually be the old position. - getContext().getScheduler().schedule(fixedAngleJob, 31, TimeUnit.MILLISECONDS); - getContext().getScheduler().schedule(fixedAngleJob, (long) (31 + minimumAcquisitionTime), TimeUnit.MILLISECONDS); - } - - private RoboReference getPanServo() { - RoboReference servo = getReference(pan); - return servo; - } - - private void scheduleScan(ScanRequest message, RoboReference servo) { - float currentInput = getCurrentInput(servo); - float midPoint = message.getStartAngle() + message.getRange() / 2; - boolean lowToHigh = false; - if (inputToAngle(currentInput) <= midPoint) { - lowToHigh = true; - } - float minimumServoMovementTime = message.getRange() / angularSpeed; - ScanJob job = new ScanJob(lowToHigh, minimumServoMovementTime, minimumAcquisitionTime, trim, message, servo, servoRange, lidar, - message.getReceiver(), pointFilter); - schedule(job); - } - - private float inputToAngle(float currentInput) { - return servoRange * currentInput; - } - - private void schedule(ScanJob job) { - long actualDelayMicros = job.delayMicros; - // One extra for first servo move. - for (int i = 0; i < job.numberOfScans + 1; i++) { - // FIXME(Marcus/Apr 4, 2017): Simplified - need to take angular - // speed of the servo into account. - getContext().getScheduler().schedule(job, actualDelayMicros, TimeUnit.MICROSECONDS); - actualDelayMicros += job.delayMicros; - } - } - - private float getCurrentInput(RoboReference servo) { - try { - return servo.getAttribute(PCA9685ServoUnit.ATTRIBUTE_SERVO_INPUT).get(); - } catch (InterruptedException | ExecutionException e) { - SimpleLoggingUtil.error(getClass(), "Could not read servo input!", e); - return 0; - } - } - - private RoboReference getReference(String unit) { - if (unit.equals("null")) { - return null; - } - return getContext().getReference(unit); - } + private static final Logger LOGGER = LoggerFactory.getLogger(LaserScanner.class); + /** + * Since the lidar lite will sometimes give some abnormal readings with very + * short range, we need to be able to filter out readings. Set this + * depending on your hardware and needs. + */ + private final static float DEFAULT_FILTER_MIN_RANGE = 0.08f; + private Predicate pointFilter; + private String pan; + private LidarLiteDevice lidar; + private float servoRange; + private float angularSpeed; + private float minimumAcquisitionTime; + private float trim; + + /** + * Filter for filtering out mis-reads. Anything closer than minRange will be + * filtered out. + */ + private static class MinRangePointFilter implements Predicate { + private final float minRange; + + private MinRangePointFilter(float minRange) { + this.minRange = minRange; + } + + @Override + public boolean test(Point2f t) { + return t.getRange() >= minRange; + } + } + + private final static class ScanJob implements Runnable { + private final AtomicInteger invokeCount = new AtomicInteger(0); + private final ScanResultImpl scanResult; + private final ScanRequest request; + private final RoboReference recipient; + private final RoboReference servo; + private final boolean lowToHigh; + private final int numberOfScans; + private long delayMicros; + private final float trim; + private final float servoRange; + private final LidarLiteDevice lidar; + private volatile float currentAngle; + private volatile boolean finished = false; + private final ScanEvent scanEvent; + + /** + * @param lowToHigh + * @param minimumServoMovementTime the minimum time for the servo to complete the movement + * over the range, in seconds + * @param minimumAcquisitionTime + * @param trim + * @param request + * @param servo + * @param servoRange + * @param lidar + * @param recipient + */ + public ScanJob(boolean lowToHigh, float minimumServoMovementTime, float minimumAcquisitionTime, float trim, ScanRequest request, + RoboReference servo, float servoRange, LidarLiteDevice lidar, RoboReference recipient, + Predicate pointFilter) { + this.lowToHigh = lowToHigh; + this.trim = trim; + this.request = request; + this.servo = servo; + this.servoRange = servoRange; + this.lidar = lidar; + this.recipient = recipient; + // one move, one first acquisition + this.numberOfScans = calculateNumberOfScans() + 1; + this.delayMicros = calculateDelay(minimumAcquisitionTime, minimumServoMovementTime); + this.currentAngle = lowToHigh ? request.getStartAngle() : request.getStartAngle() + request.getRange(); + this.scanResult = new ScanResultImpl(100, request.getStep(), pointFilter); + scanEvent = new ScanEvent(scanResult.getScanID(), getScanInfo()); + scanEvent.setScanLeftRight(lowToHigh); + JfrUtils.begin(scanEvent); + } + + @Override + public void run() { + int currentRun = invokeCount.incrementAndGet(); + if (currentRun == 1) { + // On first step, only move servo to start position + float normalizedServoTarget = getNormalizedAngle(); + servo.sendMessage(normalizedServoTarget); + return; + } else if (currentRun == 2) { + // On second, just start acquisition (no point to read yet) + startAcquisition(); + } else if (currentRun > numberOfScans) { + doScan(); + finish(); + } else { + doScan(); + } + // FIXME(Marcus/Mar 10, 2017): May want to synchronize this... + updateTargetAngle(); + } + + private void startAcquisition() { + try { + lidar.acquireRange(); + servo.sendMessage(getNormalizedAngle()); + } catch (IOException e) { + LOGGER.error("Could not read laser!:{}", e.getMessage(), e); + } + } + + private float getNormalizedAngle() { + return this.currentAngle / this.servoRange; + } + + private void doScan() { + // Read previous acquisition + try { + float readDistance = lidar.readDistance(); + // Laser was actually shooting at the previous angle, before + // moving - recalculate for that angle + float lastAngle = currentAngle + (lowToHigh ? -request.getStep() - trim : request.getStep() + trim); + scanResult.addPoint(Point2f.fromPolar(readDistance, (float) Math.toRadians(lastAngle))); + servo.sendMessage(getNormalizedAngle()); + lidar.acquireRange(); + } catch (IOException e) { + LOGGER.error("Could not read laser!:{}", e.getMessage(), e); + } + } + + private void finish() { + if (!finished) { + recipient.sendMessage(scanResult); + finished = true; + JfrUtils.end(scanEvent); + JfrUtils.commit(scanEvent); + } else { + LOGGER.error("Tried to scan more laser points after being finished!"); + } + } + + private void updateTargetAngle() { + if (lowToHigh) { + currentAngle += request.getStep(); + } else { + currentAngle -= request.getStep(); + } + } + + private int calculateNumberOfScans() { + return Math.round(request.getRange() / request.getStep()); + } + + // FIXME(Marcus/Mar 10, 2017): Calculate the required delay later from + // physical model. + private long calculateDelay(float minimumAcquisitionTime, float minimumServoMovementTime) { + float delayPerStep = minimumServoMovementTime * 1000 / calculateNumberOfScans(); + // If we have a slow servo, we will need to wait for the servo to + // move. If we have a slow acquisition, we will need to wait for the + // laser before continuing + float actualDelay = Math.max(delayPerStep, minimumAcquisitionTime); + return Math.round(actualDelay * 1000.0d); + } + + private String getScanInfo() { + return String.format("start: %2.1f, end: %2.1f, step:%2.1f", request.getStartAngle(), + request.getStartAngle() + request.getRange(), request.getStep()); + } + } + + private final static class ScanFixedAngleJob implements Runnable { + private final RoboReference recipient; + private final LidarLiteDevice lidar; + private final float angle; + private final Predicate filter; + private volatile boolean firstRun = true; + + public ScanFixedAngleJob(RoboReference recipient, LidarLiteDevice lidar, float angle, Predicate filter) { + this.recipient = recipient; + this.lidar = lidar; + this.angle = angle; + this.filter = filter; + } + + @Override + public void run() { + try { + if (firstRun) { + firstRun = false; + lidar.acquireRange(); + } else { + ScanResultImpl scan = new ScanResultImpl(1, 0f, filter); + scan.addPoint(lidar.readDistance(), (float) Math.toRadians(angle)); + recipient.sendMessage(scan); + } + } catch (IOException e) { + LOGGER.debug("Failed to read lidar:{}", e.getMessage(), e); + } + } + } + + public LaserScanner(RoboContext context, String id) { + super(ScanRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + pan = configuration.getString("servo", "laserscanner.servo"); + + // Using degrees for convenience + servoRange = (float) configuration.getFloat("servoRange", 45.0f); + + // Using angular degrees per second. + angularSpeed = configuration.getFloat("angularSpeed", 90.0f); + + // Minimum acquisition time, in ms + minimumAcquisitionTime = configuration.getFloat("minAquisitionTime", 2.0f); + + // Trim to align left to right and right to left scans (in degrees) + trim = configuration.getFloat("trim", 0.0f); + + // Set the default minimal range to filter out + float minFilterRange = configuration.getFloat("minFilterRange", DEFAULT_FILTER_MIN_RANGE); + pointFilter = new MinRangePointFilter(minFilterRange); + + try { + lidar = new LidarLiteDevice(getBus(), getAddress()); + } catch (IOException e) { + throw new ConfigurationException(String.format( + "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); + } + } + + @Override + public void onMessage(ScanRequest message) { + RoboReference servo = getPanServo(); + if (message.getRange() == 0 || message.getStep() == 0) { + scheduleFixPanScan(message, servo); + } else { + scheduleScan(message, servo); + } + } + + private void scheduleFixPanScan(ScanRequest message, RoboReference servo) { + servo.sendMessage(message.getStartAngle() / servoRange); + ScanFixedAngleJob fixedAngleJob = new ScanFixedAngleJob(message.getReceiver(), lidar, message.getStartAngle(), pointFilter); + // FIXME(Marcus/Sep 5, 2017): We should try to calculate this - we are + // now assuming that it will take little time to move the servo to the + // "new" position, however, the fact is that the new position will + // usually be the old position. + getContext().getScheduler().schedule(fixedAngleJob, 31, TimeUnit.MILLISECONDS); + getContext().getScheduler().schedule(fixedAngleJob, (long) (31 + minimumAcquisitionTime), TimeUnit.MILLISECONDS); + } + + private RoboReference getPanServo() { + RoboReference servo = getReference(pan); + return servo; + } + + private void scheduleScan(ScanRequest message, RoboReference servo) { + float currentInput = getCurrentInput(servo); + float midPoint = message.getStartAngle() + message.getRange() / 2; + boolean lowToHigh = false; + if (inputToAngle(currentInput) <= midPoint) { + lowToHigh = true; + } + float minimumServoMovementTime = message.getRange() / angularSpeed; + ScanJob job = new ScanJob(lowToHigh, minimumServoMovementTime, minimumAcquisitionTime, trim, message, servo, servoRange, lidar, + message.getReceiver(), pointFilter); + schedule(job); + } + + private float inputToAngle(float currentInput) { + return servoRange * currentInput; + } + + private void schedule(ScanJob job) { + long actualDelayMicros = job.delayMicros; + // One extra for first servo move. + for (int i = 0; i < job.numberOfScans + 1; i++) { + // FIXME(Marcus/Apr 4, 2017): Simplified - need to take angular + // speed of the servo into account. + getContext().getScheduler().schedule(job, actualDelayMicros, TimeUnit.MICROSECONDS); + actualDelayMicros += job.delayMicros; + } + } + + private float getCurrentInput(RoboReference servo) { + try { + return servo.getAttribute(PCA9685ServoUnit.ATTRIBUTE_SERVO_INPUT).get(); + } catch (InterruptedException | ExecutionException e) { + SimpleLoggingUtil.error(getClass(), "Could not read servo input!", e); + return 0; + } + } + + private RoboReference getReference(String unit) { + if (unit.equals("null")) { + return null; + } + return getContext().getReference(unit); + } }