Skip to content
This repository was archived by the owner on Sep 6, 2024. It is now read-only.

Commit

Permalink
1. Add accel, gryo, and mag range, bw, etc parameterization. 2. get r…
Browse files Browse the repository at this point in the history 
…id of II2cDeviceClient.executeFunctionWhileLocked 3. I2cDeviceClient.read() preserves read window 4. Fix naming convention in IMU parameter block

Former-commit-id: 420905a2d18cd169f2b42b823105a473ed0715e4
Former-commit-id: 20ae056
  • Loading branch information
@rgatkinson
rgatkinson committed Jan 11, 2016
1 parent 4763b07 commit a69a9b4
Show file tree
Hide file tree
Showing 5 changed files with 113 additions and 127 deletions.
10 changes: 5 additions & 5 deletions SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/examples/SynchIMUDemo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,11 @@ public class SynchIMUDemo extends SynchronousOpMode

@Override public void main() throws InterruptedException
{
// We are expecting the IMU to be attached to an I2C port on a core device interface
// We are expecting the IMU to be attached to an I2C port on a core device interface
// module and named "imu". Retrieve that raw I2cDevice and then wrap it in an object that
// semantically understands this particular kind of sensor.
parameters.angleunit = IBNO055IMU.ANGLEUNIT.DEGREES;
parameters.accelunit = IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC;
parameters.angleUnit = IBNO055IMU.ANGLEUNIT.DEGREES;
parameters.accelUnit = IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
parameters.loggingTag = "BNO055";
imu = ClassFactory.createAdaFruitBNO055IMU(hardwareMap.i2cDevice.get("imu"), parameters);
Expand Down Expand Up @@ -186,7 +186,7 @@ public Object value()

String formatAngle(double angle)
{
return parameters.angleunit==IBNO055IMU.ANGLEUNIT.DEGREES ? formatDegrees(angle) : formatRadians(angle);
return parameters.angleUnit ==IBNO055IMU.ANGLEUNIT.DEGREES ? formatDegrees(angle) : formatRadians(angle);
}
String formatRadians(double radians)
{
Expand All @@ -202,7 +202,7 @@ String formatRate(double cyclesPerSecond)
}
String formatPosition(double coordinate)
{
String unit = parameters.accelunit== IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC
String unit = parameters.accelUnit == IBNO055IMU.ACCELUNIT.METERS_PERSEC_PERSEC
? "m" : "??";
return String.format("%.2f%s", coordinate, unit);
}
Expand Down
49 changes: 45 additions & 4 deletions SwerveRoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/IBNO055IMU.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,11 +48,32 @@ class Parameters
/** units in which temperature are measured. See Section 3.6.1 (p31) of the BNO055 specification */
public TEMPUNIT temperatureUnit = TEMPUNIT.CELSIUS;
/** units in which angles and angular rates are measured. See Section 3.6.1 (p31) of the BNO055 specification */
public ANGLEUNIT angleunit = ANGLEUNIT.RADIANS;
public ANGLEUNIT angleUnit = ANGLEUNIT.RADIANS;
/** units in which accelerations are measured. See Section 3.6.1 (p31) of the BNO055 specification */
public ACCELUNIT accelunit = ACCELUNIT.METERS_PERSEC_PERSEC;
public ACCELUNIT accelUnit = ACCELUNIT.METERS_PERSEC_PERSEC;
/** directional convention for measureing pitch angles. See Section 3.6.1 (p31) of the BNO055 specification */
public PITCHMODE pitchmode = PITCHMODE.ANDROID; // Section 3.6.2
public PITCHMODE pitchMode = PITCHMODE.ANDROID; // Section 3.6.2

/** accelerometer range. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */
public ACCELRANGE accelRange = ACCELRANGE.G4;
/** accelerometer bandwidth. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */
public ACCELBANDWIDTH accelBandwidth = ACCELBANDWIDTH.HZ62_5;
/** accelerometer power mode. See Section 3.5.2 (p27) and Section 4.2.2 (p77) of the BNO055 specification */
public ACCELPOWERMODE accelPowerMode = ACCELPOWERMODE.NORMAL;

/** gyroscope range. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */
public GYRORANGE gyroRange = GYRORANGE.DPS2000;
/** gyroscope bandwidth. See Section 3.5.2 (p27) and Table 3-4 (p21) of the BNO055 specification */
public GYROBANDWIDTH gyroBandwidth = GYROBANDWIDTH.HZ32;
/** gyroscope power mode. See Section 3.5.2 (p27) and Section 4.4.4 (p78) of the BNO055 specification */
public GYROPOWERMODE gyroPowerMode = GYROPOWERMODE.NORMAL;

/** magnetometer data rate. See Section 3.5.3 (p27) and Section 4.4.3 (p77) of the BNO055 specification */
public MAGRATE magRate = MAGRATE.HZ10;
/** magnetometer op mode. See Section 3.5.3 (p27) and Section 4.4.3 (p77) of the BNO055 specification */
public MAGOPMODE magOpMode = MAGOPMODE.REGULAR;
/** magnetometer power mode. See Section 3.5.3 (p27) and Section 4.4.3 (p77) of the BNO055 specification */
public MAGPOWERMODE magPowerMode = MAGPOWERMODE.NORMAL;

/** calibration data with which the BNO055 should be initialized */
public byte[] calibrationData = null;
Expand Down Expand Up @@ -307,6 +328,17 @@ enum ANGLEUNIT { DEGREES(0), RADIANS(1); public fi
enum ACCELUNIT { METERS_PERSEC_PERSEC(0), MILLIGALS(1); public final byte bVal; ACCELUNIT(int i) { bVal =(byte)i; }}
enum PITCHMODE { WINDOWS(0), ANDROID(1); public final byte bVal; PITCHMODE(int i) { bVal =(byte)i; }}

enum GYRORANGE { DPS2000(0), DPS1000(1), DPS500(2), DPS250(3), DPS125(4); public final byte bVal; GYRORANGE(int i) { bVal =(byte)(i<<0);}}
enum GYROBANDWIDTH { HZ523(0), HZ230(1), HZ116(2), HZ47(3), HZ23(4), HZ12(5), HZ64(6), HZ32(7); public final byte bVal; GYROBANDWIDTH(int i) { bVal =(byte)(i<<3);}}
enum GYROPOWERMODE { NORMAL(0), FAST(1), DEEP(2), SUSPEND(3), ADVANCED(4) ; public final byte bVal; GYROPOWERMODE(int i) { bVal =(byte)(i<<0);}}
enum ACCELRANGE { G2(0), G4(1), G8(2), G16(3); public final byte bVal; ACCELRANGE(int i) { bVal =(byte)(i<<0);}}
enum ACCELBANDWIDTH { HZ7_81(0), HZ15_63(1), HZ31_25(2), HZ62_5(3), HZ125(4), HZ250(5), HZ500(6), HZ1000(7); public final byte bVal; ACCELBANDWIDTH(int i) { bVal =(byte)(i<<2);}}
enum ACCELPOWERMODE { NORMAL(0), SUSPEND(1), LOW1(2), STANDBY(3), LOW2(4), DEEP(5); public final byte bVal; ACCELPOWERMODE(int i) { bVal =(byte)(i<<5);}}

enum MAGRATE { HZ2(0), HZ6(1), HZ8(2), HZ10(3), HZ15(4), HZ20(5), HZ25(6), HZ30(7); public final byte bVal; MAGRATE(int i) { bVal =(byte)(i<<0);}}
enum MAGOPMODE { LOW(0), REGULAR(1), ENHANCED(2), HIGH(3); public final byte bVal; MAGOPMODE(int i) { bVal =(byte)(i<<3);}}
enum MAGPOWERMODE { NORMAL(0), SLEEP(1), SUSPEND(2), FORCE(3); public final byte bVal; MAGPOWERMODE(int i) { bVal =(byte)(i<<5);}}

/**
* Sensor modes are described in Table 3-5 (p21) of the BNO055 specification,
* where they are termed "operation modes".
Expand Down Expand Up @@ -472,7 +504,16 @@ enum REGISTER
ACCEL_RADIUS_LSB(0X67),
ACCEL_RADIUS_MSB(0X68),
MAG_RADIUS_LSB(0X69),
MAG_RADIUS_MSB(0X6A);
MAG_RADIUS_MSB(0X6A),

/** Selected Page 1 registers */
ACC_CONFIG(0x08),
MAG_CONFIG(0x09),
GYR_CONFIG_0(0x0A),
GYR_CONFIG_1(0x0B),
ACC_SLEEP_CONFIG(0x0C),
GYR_SLEEP_CONFIG(0x0D);

//------------------------------------------------------------------------------------------
public final byte bVal;
private REGISTER(int i)
Expand Down
47 changes: 3 additions & 44 deletions ...RoboticsLibrary/src/main/java/org/swerverobotics/library/interfaces/II2cDeviceClient.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,6 @@ public interface II2cDeviceClient extends HardwareDevice
*
* @see #setReadWindow(ReadWindow)
* @see #read8(int)
* @see #executeFunctionWhileLocked(IFunc)
*/
void ensureReadWindow(ReadWindow windowNeeded, ReadWindow windowToSet);

Expand Down Expand Up @@ -130,22 +129,13 @@ public interface II2cDeviceClient extends HardwareDevice
* a new read fresh window will be created with the same set of registers. Otherwise, a
* window that exactly covers the requested set of registers will be created.</p>
*
* <p>If one is trying to optimize the the register window by calling
* {@link #ensureReadWindow(ReadWindow, ReadWindow) ensureReadWindow()}, this auto-window
* creation can cause difficulties if any concurrent access is present. In such situations,
* {@link #executeFunctionWhileLocked(IFunc)} can be used to allow you to atomically both
* set the read window and execute a read without the possibility of the read window being
* re-adjusted in the middle.
* </p>
*
* @param ireg the register number of the first byte register to read
* @param creg the number of bytes / registers to read
* @return the data which was read, together with the timestamp
*
* @see #read(int, int)
* @see #read8(int)
* @see #ensureReadWindow(ReadWindow, ReadWindow)
* @see #executeFunctionWhileLocked(IFunc)
*/
TimestampedData readTimeStamped(int ireg, int creg);

Expand All @@ -172,7 +162,6 @@ class TimestampedData
*
* @see #ensureReadWindow(ReadWindow, ReadWindow)
* @see #readTimeStamped(int, int)
* @see #executeFunctionWhileLocked(IFunc)
*/
TimestampedData readTimeStamped(int ireg, int creg, ReadWindow readWindowNeeded, ReadWindow readWindowSet);

Expand Down Expand Up @@ -237,30 +226,6 @@ class TimestampedData
*/
void waitForWriteCompletions();

//----------------------------------------------------------------------------------------------
// Concurrency management
//----------------------------------------------------------------------------------------------

/**
* Executes the indicated action while holding the concurrency lock on the object
* so as to prevent other threads from interleaving.
*
* @param action the action to execute
* @see #executeFunctionWhileLocked(IFunc)
*/
void executeActionWhileLocked(Runnable action);

/**
* Executes the indicated function while holding the concurrency lock on the object
* so as to prevent other threads from interleaving. Returns the value of the function.
*
* @param function the function to execute
* @param <T> the type of the data returned from the function
* @return the datum value returned from the function
* @see #executeActionWhileLocked(Runnable)
*/
<T> T executeFunctionWhileLocked(IFunc<T> function);

//----------------------------------------------------------------------------------------------
// Heartbeats
//----------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -317,13 +282,7 @@ class HeartbeatAction
/** Priority #2: re-issue the last I2C write operation, if possible. */
public boolean rewriteLastWritten = false;

/** Priority #3: explicitly read a given register window. Note that using
* this form of heartbeat may cause the I2C device to experience concurrency it
* otherwise might not support for this heartbeat form may make use of
* worker threads.
*
* @see #executeFunctionWhileLocked(IFunc)
*/
/** Priority #3: explicitly read a given register window */
public ReadWindow heartbeatReadWindow = null;
}

Expand Down Expand Up @@ -567,9 +526,9 @@ public ReadWindow(int iregFirst, int creg, READ_MODE readMode)

/**
* Returns a copy of this window but with the {@link #readIssued} flag clear
* @return a fresh readable copy of the window
* @return a fresh copy of the window into which data can actually be read.
*/
public ReadWindow freshCopy()
public ReadWindow readableCopy()
{
return new ReadWindow(this.iregFirst, this.creg, this.readMode);
}
Expand Down
80 changes: 34 additions & 46 deletions ...eRoboticsLibrary/src/main/java/org/swerverobotics/library/internal/AdaFruitBNO055IMU.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,18 +166,30 @@ public void initialize(Parameters parameters)
write8(REGISTER.PAGE_ID, 0);

// Set the output units. Section 3.6, p31
int unitsel = (parameters.pitchmode.bVal << 7) | // pitch angle convention
int unitsel = (parameters.pitchMode.bVal << 7) | // pitch angle convention
(parameters.temperatureUnit.bVal << 4) | // temperature
(parameters.angleunit.bVal << 2) | // euler angle units
(parameters.angleunit.bVal << 1) | // gyro units, per second
(parameters.accelunit.bVal /*<< 0*/); // accelerometer units
(parameters.angleUnit.bVal << 2) | // euler angle units
(parameters.angleUnit.bVal << 1) | // gyro units, per second
(parameters.accelUnit.bVal /*<< 0*/); // accelerometer units
write8(REGISTER.UNIT_SEL, unitsel);

// Use or don't use the external crystal
// See Section 5.5 (p100) of the BNO055 specification.
write8(REGISTER.SYS_TRIGGER, parameters.useExternalCrystal ? 0x80 : 0x00);
delayLoreExtra(10);

// Switch to page 1 so we can write some more registers
write8(REGISTER.PAGE_ID, 1);

// Configure selected page 1 registers
write8(REGISTER.ACC_CONFIG, parameters.accelPowerMode.bVal | parameters.accelBandwidth.bVal | parameters.accelRange.bVal);
write8(REGISTER.MAG_CONFIG, parameters.magPowerMode.bVal | parameters.magOpMode.bVal | parameters.magRate.bVal);
write8(REGISTER.GYR_CONFIG_0, parameters.gyroBandwidth.bVal | parameters.gyroRange.bVal);
write8(REGISTER.GYR_CONFIG_1, parameters.gyroPowerMode.bVal);

// Switch back
write8(REGISTER.PAGE_ID, 0);

// Run a self test. This appears to be a necessary step in order for the
// sensor to be able to actually be used.
write8(REGISTER.SYS_TRIGGER, read8(REGISTER.SYS_TRIGGER) | 0x01); // SYS_TRIGGER=0x3F
Expand Down Expand Up @@ -341,20 +353,14 @@ public synchronized EulerAngles getAngularOrientation()

public synchronized Quaternion getQuaternionOrientation()
{
return this.deviceClient.executeFunctionWhileLocked(new IFunc<Quaternion>()
{
@Override public Quaternion value()
{
// Ensure we can see the registers we need
deviceClient.ensureReadWindow(
new II2cDeviceClient.ReadWindow(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8, readMode),
upperWindow);

// Section 3.6.5.5 of BNO055 specification
II2cDeviceClient.TimestampedData ts = deviceClient.readTimeStamped(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8);
return new Quaternion(ts, (1 << 14));
}
});
// Ensure we can see the registers we need
deviceClient.ensureReadWindow(
new II2cDeviceClient.ReadWindow(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8, readMode),
upperWindow);

// Section 3.6.5.5 of BNO055 specification
II2cDeviceClient.TimestampedData ts = deviceClient.readTimeStamped(REGISTER.QUATERNION_DATA_W_LSB.bVal, 8);
return new Quaternion(ts, (1 << 14));
}

/**
Expand All @@ -363,7 +369,7 @@ public synchronized Quaternion getQuaternionOrientation()
*/
private double getAngularScale()
{
return this.parameters.angleunit == ANGLEUNIT.DEGREES ? 16.0 : 900.0;
return this.parameters.angleUnit == ANGLEUNIT.DEGREES ? 16.0 : 900.0;
}

/**
Expand All @@ -372,7 +378,7 @@ private double getAngularScale()
*/
private double getAccelerationScale()
{
return this.parameters.accelunit == ACCELUNIT.METERS_PERSEC_PERSEC ? 100.0 : 1.0;
return this.parameters.accelUnit == ACCELUNIT.METERS_PERSEC_PERSEC ? 100.0 : 1.0;
}

/**
Expand All @@ -387,17 +393,11 @@ private double getFluxScale()

private II2cDeviceClient.TimestampedData getVector(final VECTOR vector)
{
return this.deviceClient.executeFunctionWhileLocked(new IFunc<II2cDeviceClient.TimestampedData>()
{
@Override public II2cDeviceClient.TimestampedData value()
{
// Ensure that the 6 bytes for this vector are visible in the register window.
ensureReadWindow(new II2cDeviceClient.ReadWindow(vector.getValue(), 6, readMode));
// Ensure that the 6 bytes for this vector are visible in the register window.
ensureReadWindow(new II2cDeviceClient.ReadWindow(vector.getValue(), 6, readMode));

// Read the data
return deviceClient.readTimeStamped(vector.getValue(), 6);
}
});
// Read the data
return deviceClient.readTimeStamped(vector.getValue(), 6);
}

//------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -588,26 +588,14 @@ class AccelerationManager implements IHandshakeable

@Override public synchronized byte read8(final REGISTER reg)
{
return this.deviceClient.executeFunctionWhileLocked(new IFunc<Byte>()
{
@Override public Byte value()
{
ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, 1, readMode));
return deviceClient.read8(reg.bVal);
}
});
ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, 1, readMode));
return deviceClient.read8(reg.bVal);
}

@Override public synchronized byte[] read(final REGISTER reg, final int cb)
{
return this.deviceClient.executeFunctionWhileLocked(new IFunc<byte[]>()
{
@Override public byte[] value()
{
ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, cb, readMode));
return deviceClient.read(reg.bVal, cb);
}
});
ensureReadWindow(new II2cDeviceClient.ReadWindow(reg.bVal, cb, readMode));
return deviceClient.read(reg.bVal, cb);
}

@Override public void write8(REGISTER reg, int data)
Expand Down
Loading

0 comments on commit a69a9b4

Please sign in to comment.