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.warn("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..3918911b 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,18 +16,11 @@
*/
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;
import com.robo4j.configuration.Configuration;
import com.robo4j.hw.rpi.i2c.lidar.LidarLiteDevice;
-import com.robo4j.logging.SimpleLoggingUtil;
import com.robo4j.math.geometry.Point2f;
import com.robo4j.math.geometry.ScanResult2D;
import com.robo4j.math.geometry.impl.ScanResultImpl;
@@ -35,325 +28,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.warn("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.error("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) {
+ LOGGER.error("Could not read servo input:{}", e.getMessage(), e);
+ return 0;
+ }
+ }
+
+ private RoboReference getReference(String unit) {
+ if (unit.equals("null")) {
+ return null;
+ }
+ return getContext().getReference(unit);
+ }
}
diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java
index 08620608..2a51e070 100644
--- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java
+++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java
@@ -27,7 +27,8 @@
import com.robo4j.hw.rpi.pad.LF710Pad;
import com.robo4j.hw.rpi.pad.PadInputResponseListener;
import com.robo4j.hw.rpi.pad.RoboControlPad;
-import com.robo4j.logging.SimpleLoggingUtil;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
import java.nio.file.Paths;
@@ -37,8 +38,8 @@
* @author Marcus Hirt (@hirt)
* @author Miro Wengner (@miragemiko)
*/
-public class LF710PadUnit extends RoboUnit