Examples with Float3D com.robo4j.math.geometry.Float3D used on opensource projects

Example 6 with Float3D

use of com.robo4j.math.geometry.Float3D in project robo4j by Robo4J.

the class AccelerometerLSM303Device method read.

/**
	 * @return current acceleration, m/s^2
	 * @throws IOException
	 */
public synchronized Float3D read() throws IOException {
    Float3D rawData = new Float3D();
    byte[] data = new byte[6];
    int n = i2cDevice.read(OUT_X_L_A | 0x80, data, 0, 6);
    if (n != 6) {
        getLogger().warning("Failed to read all data from accelerometer. Should have read 6, could only read " + n);
    }
    float k = scale.getSensitivity() / 1000.0f;
    rawData.x = read12bitSigned(data, 0) * k;
    rawData.y = read12bitSigned(data, 2) * k;
    rawData.z = read12bitSigned(data, 4) * k;
    return rawData;
}

Example 7 with Float3D

use of com.robo4j.math.geometry.Float3D in project robo4j by Robo4J.

the class MagnetometerLSM303Test method main.

// FIXME(Marcus/Dec 5, 2016): Verify that this one works.
public static void main(String[] args) throws IOException, InterruptedException {
    if (args.length != 2) {
        System.out.println("Usage: MagnetometerLSM303Test <delay between reads (ms)> <print every Nth read>");
        System.exit(1);
    }
    int delay = Integer.parseInt(args[0]);
    int modulo = Integer.parseInt(args[1]);
    MagnetometerLSM303Device device = new MagnetometerLSM303Device();
    int count = 0;
    while (true) {
        Float3D fl = device.read();
        if (count++ % modulo == 0) {
            System.out.println(String.format("Value %d = %s", count, fl.toString()));
        }
        Thread.sleep(delay);
    }
}

Example 8 with Float3D

use of com.robo4j.math.geometry.Float3D in project robo4j by Robo4J.

the class Stats method getAvg.

public Float3D getAvg() {
    Float3D avg = new Float3D();
    avg.x = sum.x / count;
    avg.y = sum.y / count;
    avg.z = sum.z / count;
    return avg;
}

Example 9 with Float3D

use of com.robo4j.math.geometry.Float3D 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);
    Float3D offsets = readFloat3D(configuration.getChildConfiguration("offsets"));
    Float3D 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);
    }
}

Example 10 with Float3D

use of com.robo4j.math.geometry.Float3D in project robo4j by Robo4J.

the class CalibratedGyro method calibrate.

/**
	 * Does calibration.
	 * 
	 * @throws IOException
	 */
public void calibrate() throws IOException {
    setCalibration(new Float3D(), Float3D.createIdentity());
    float[] xvalues = new float[NUMBER_OF_CALIBRATION_READINGS];
    float[] yvalues = new float[NUMBER_OF_CALIBRATION_READINGS];
    float[] zvalues = new float[NUMBER_OF_CALIBRATION_READINGS];
    for (int i = 0; i < NUMBER_OF_CALIBRATION_READINGS; i++) {
        Float3D tmp = read();
        xvalues[i] = tmp.x;
        yvalues[i] = tmp.y;
        zvalues[i] = tmp.z;
        sleep(20);
    }
    Float3D calibration = new Float3D();
    calibration.x = calibrate(xvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
    calibration.y = calibrate(yvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
    calibration.z = calibrate(zvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
    System.out.println(RANGE_MULTIPLIERS);
    setCalibration(calibration, RANGE_MULTIPLIERS);
}