use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class Bno080SPIDevice method createVectorEvent.
private DataEvent3f createVectorEvent(DataEventType type, long timeStamp, int... data) {
if (data == null || data.length < 6) {
return EMPTY_EVENT;
}
final int status = data[0] & 0xFFFF;
final int x = data[1] & 0xFFFF;
final int y = data[2] & 0xFFFF;
final int z = data[3] & 0xFFFF;
final int qReal = data[4] & 0xFFFF;
// Only available on
final int qRadianAccuracy = data[5] & 0xFFFF;
// rotation vector, not
// game rot vector
final Tuple3f tuple3f = ShtpUtils.createTupleFromFixed(type.getQ(), x, y, z);
return new VectorEvent(type, status, tuple3f, timeStamp, intToFloat(qReal, type.getQ()), intToFloat(qRadianAccuracy, type.getQ()));
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class GyroL3GD20Device method read.
public Tuple3f read() throws IOException {
byte[] xyz = new byte[6];
Tuple3f data = new Tuple3f();
i2cDevice.write((byte) (REGISTER_OUT_X_L | 0x80));
i2cDevice.read(REGISTER_OUT_X_H | 0x80, xyz, 0, 6);
int x = (xyz[1] & 0xFF | (xyz[0] << 8));
int y = (xyz[3] & 0xFF | (xyz[2] << 8));
int z = (xyz[5] & 0xFF | (xyz[4] << 8));
data.x = x * sensitivity.getSensitivityFactor();
data.y = y * sensitivity.getSensitivityFactor();
data.z = z * sensitivity.getSensitivityFactor();
return data;
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class AccelerometerLSM303Unit method onInitialization.
@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);
}
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class Stats method getAvg.
public Tuple3f getAvg() {
Tuple3f avg = new Tuple3f();
avg.x = sum.x / count;
avg.y = sum.y / count;
avg.z = sum.z / count;
return avg;
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class CalibratedMagnetometerExample method main.
public static void main(String[] args) throws IOException, InterruptedException {
// NOTE(Marcus/Jul 23, 2017): Change the actual bias vector and
// transform matrix to match your calibration values.
// See the MagViz tool for more information.
Tuple3f bias = new Tuple3f(-44.689f, -2.0665f, -15.240f);
Matrix3f transform = new Matrix3f(1.887f, 5.987f, -5.709f, 5.987f, 1.528f, -2.960f, -5.709f, -2.960f, 9.761f);
MagnetometerLSM303Device magnetometer = new MagnetometerLSM303Device(I2CBus.BUS_1, 0x1e, Mode.CONTINUOUS_CONVERSION, Rate.RATE_7_5, false, bias, transform);
System.out.println("Starting to read and print the heading. Make sure the magnetometer is flat in the XY-plane (this example does not do tilt compensated heading).");
System.out.println("Press <Enter> to quit...");
EXECUTOR.scheduleAtFixedRate(new MagnetometerPoller(magnetometer), 100, 500, TimeUnit.MILLISECONDS);
System.in.read();
EXECUTOR.shutdown();
EXECUTOR.awaitTermination(1, TimeUnit.SECONDS);
System.out.println("Goodbye!");
}
Aggregations