use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class CalibratedFloat3DDevice method read.
public Tuple3f read() throws IOException {
Tuple3f value = device.read();
value.add(centerOffsets);
value.multiply(rangeMultipliers);
return value;
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class AccelerometerLSM303Device method read.
/**
* @return current acceleration, m/s^2
* @throws IOException
* exception
*/
public synchronized Tuple3f read() throws IOException {
Tuple3f rawData = new Tuple3f();
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;
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class CalibratedGyro method calibrate.
/**
* Does calibration.
*
* @throws IOException
* exception
*/
public void calibrate() throws IOException {
setCalibration(new Tuple3f(), Tuple3f.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++) {
Tuple3f tmp = read();
xvalues[i] = tmp.x;
yvalues[i] = tmp.y;
zvalues[i] = tmp.z;
sleep(20);
}
Tuple3f calibration = new Tuple3f();
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);
}
use of com.robo4j.math.geometry.Tuple3f in project robo4j by Robo4J.
the class AccelerometerLSM303Test method readValues.
private static Stats readValues(ReadableDevice<Tuple3f> device) throws IOException, InterruptedException {
Stats stats = new Stats();
for (int i = 0; i < 250; i++) {
Tuple3f fl = device.read();
stats.addValue(fl);
Thread.sleep(20);
if (i % 25 == 0) {
System.out.print(".");
}
}
System.out.println("");
return stats;
}
Aggregations