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

Commit

Permalink
mentorSandbox -> master
Browse files Browse the repository at this point in the history 
Former-commit-id: 39a05aab3096ea9a2e1712d8c6d47953801b556b
Former-commit-id: 51909bb
  • Loading branch information
@rgatkinson
rgatkinson committed Jan 11, 2016
2 parents f48bf29 + a69a9b4 commit e8791cb
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 e8791cb

Please sign in to comment.