Skip to content

Commit

Permalink
[69] Simple logging partial commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@mirage22
mirage22 committed Oct 9, 2024
1 parent d019469 commit 50d1661
Show file tree
Hide file tree
Showing 13 changed files with 1,420 additions and 1,423 deletions.
299 changes: 149 additions & 150 deletions ...j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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.
*
* <p>
* Use configuration settings to compensate for inverted axes, and for
* calibration.
*
*
* @author Marcus Hirt (@hirt)
* @author Miroslav Wengner (@miragemiko)
*/
public class AccelerometerLSM303Unit extends I2CRoboUnit<AccelerometerRequest> {
/**
* 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<AttributeDescriptor<?>> 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<AccelerometerRequest> 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<AccelerometerEvent> 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> R onGetAttribute(AttributeDescriptor<R> 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}.
* <p>
* 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<AttributeDescriptor<?>> 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<AccelerometerRequest> 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<AccelerometerEvent> 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> R onGetAttribute(AttributeDescriptor<R> 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);
}
}
Loading

0 comments on commit 50d1661

Please sign in to comment.