Search in sources :

Example 6 with Tuple3f

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()));
}
Also used : Tuple3f(com.robo4j.math.geometry.Tuple3f) VectorEvent(com.robo4j.hw.rpi.imu.bno.VectorEvent)

Example 7 with Tuple3f

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;
}
Also used : Tuple3f(com.robo4j.math.geometry.Tuple3f)

Example 8 with Tuple3f

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);
    }
}
Also used : CalibratedAccelerometer(com.robo4j.hw.rpi.i2c.accelerometer.CalibratedAccelerometer) Tuple3f(com.robo4j.math.geometry.Tuple3f) ConfigurationException(com.robo4j.ConfigurationException) DataRate(com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.DataRate) PowerMode(com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.PowerMode) FullScale(com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.FullScale) IOException(java.io.IOException) AccelerometerLSM303Device(com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device)

Example 9 with Tuple3f

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;
}
Also used : Tuple3f(com.robo4j.math.geometry.Tuple3f)

Example 10 with Tuple3f

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!");
}
Also used : Tuple3f(com.robo4j.math.geometry.Tuple3f) Matrix3f(com.robo4j.math.geometry.Matrix3f)

Aggregations

Tuple3f (com.robo4j.math.geometry.Tuple3f)14 ConfigurationException (com.robo4j.ConfigurationException)1 RoboBuilder (com.robo4j.RoboBuilder)1 RoboContext (com.robo4j.RoboContext)1 AccelerometerLSM303Device (com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device)1 DataRate (com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.DataRate)1 FullScale (com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.FullScale)1 PowerMode (com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.PowerMode)1 CalibratedAccelerometer (com.robo4j.hw.rpi.i2c.accelerometer.CalibratedAccelerometer)1 DataEvent3f (com.robo4j.hw.rpi.imu.bno.DataEvent3f)1 VectorEvent (com.robo4j.hw.rpi.imu.bno.VectorEvent)1 Matrix3f (com.robo4j.math.geometry.Matrix3f)1 Tuple3i (com.robo4j.math.geometry.Tuple3i)1 IOException (java.io.IOException)1 InputStream (java.io.InputStream)1