private void waitForWritesToChannelToDrain(int channel) {
try {
SynchronousOpMode.synchronousThreadWaitForLoopCycleEmptyOfActionKey(
this.getChannelWriteKey(channel));
} catch (InterruptedException e) {
throw SwerveRuntimeException.wrap(e);
}
}
/**
* Instantiate an I2cDeviceClient instance in the indicated device with the indicated initial
* window of registers being read.
*
* @param i2cDevice the device we are to be a client of
* @param i2cAddr8Bit its 8 bit i2cAddress
* @param autoClose if true, the device client will automatically close when the associated
* SynchronousOpMode stops
*/
public I2cDeviceClient(
II2cDevice i2cDevice, int i2cAddr8Bit, boolean autoClose, IStopActionRegistrar registrar) {
i2cDevice.setI2cAddr(i2cAddr8Bit);
this.i2cDevice = i2cDevice;
this.callback = new Callback();
this.callbackThread = null;
this.callbackThreadOriginalPriority = 0; // not known
this.callbackThreadPriorityBoost = 0; // no boost
this.hardwareCycleCount = 0;
this.loggingEnabled = false;
this.loggingTag = String.format("I2cDeviceClient(%s)", i2cDevice.getDeviceName());
;
this.timeSinceLastHeartbeat = new ElapsedTime();
this.timeSinceLastHeartbeat.reset();
this.msHeartbeatInterval = 0;
this.heartbeatAction = null;
this.readCache = this.i2cDevice.getI2cReadCache();
this.readCacheLock = this.i2cDevice.getI2cReadCacheLock();
this.writeCache = this.i2cDevice.getI2cWriteCache();
this.writeCacheLock = this.i2cDevice.getI2cWriteCacheLock();
this.readWindow = null;
this.readWindowActuallyRead = null;
this.readWindowSentToController = null;
this.readWindowChanged = false;
this.readWindowSentToControllerInitialized = false;
this.nanoTimeReadCacheValid = 0;
this.readCacheStatus = READ_CACHE_STATUS.IDLE;
this.writeCacheStatus = WRITE_CACHE_STATUS.IDLE;
this.modeCacheStatus = MODE_CACHE_STATUS.IDLE;
if (autoClose) {
if (registrar == null) registrar = SynchronousOpMode.getStopActionRegistrar();
if (registrar != null) {
registrar.registerActionOnStop(
new IAction() {
@Override
public void doAction() {
I2cDeviceClient.this.close();
}
});
}
}
this.i2cDevice.registerForI2cPortReadyCallback(this.callback);
}
/**
* Switch the controller to either read-only or write-only device mode.
*
* <p>We assume that the only spontaneous transitions are SWITCHING_TO_READ_MODE -> READ_MODE and
* SWITCHING_TO_WRITE_MODE -> WRITE_MODE. All other transitions only happen because we (or someone
* else) requests them.
*/
private synchronized void switchToMode(DeviceMode newMode) throws InterruptedException {
// Note: remember that in general the user code may choose to spawn worker threads.
// Thus, we may have multiple, concurrent threads simultaneously trying to switch modes.
// We deal with that by using synchronized public methods, allowing only one client in at
// a time; this gives us a sequential sequence of modes we need the controller to be in.
// This method, too, is synchronized, but that's paranoia.
// If we don't currently know his mode, we need to ask the controller where we stand
if (null == this.controllerMode) {
this.controllerMode = this.getMotorControllerDeviceMode();
}
// If the controller is being stupid in returning a non-actual mode (the mock one was)
// then to heck with trying to keep him happy
if (null == this.controllerMode) return;
// We might have caught this guy mid transition. Wait until he settles down
if (this.controllerMode == DeviceMode.SWITCHING_TO_READ_MODE
|| this.controllerMode == DeviceMode.SWITCHING_TO_WRITE_MODE) {
for (; ; ) {
this.controllerMode = this.getMotorControllerDeviceMode();
//
if (this.controllerMode == DeviceMode.SWITCHING_TO_READ_MODE
|| this.controllerMode == DeviceMode.SWITCHING_TO_WRITE_MODE) {
SynchronousOpMode.synchronousThreadIdle();
} else break;
}
}
// If he's read-write, then that's just dandy
if (this.controllerMode == DeviceMode.READ_WRITE) return;
// If he's not what we want, then ask him to switch him to what we want and
// spin until he gets there.
if (this.controllerMode != newMode) {
// We need to complete any existing thunks (we could be more refined, but that suffices)
// as, to quote Johnathan Berling:
//
// http://ftcforum.usfirst.org/showthread.php?4352-Legacy-Motor-controller-write-to-read-mode-amount-of-time/page3
/*
When the loop call finishes, all commands are sent simultaneously to the device. So, it
simultaneously gets put into read mode and told to change the channel mode. In this case
it can't comply with the command to switch the channel mode since the port is in read mode.
Code:
motorLeft.setTargetPosition(firstTarget);
motorRight.setTargetPosition(-firstTarget);
motorLeft.setPower(1.0);
motorRight.setPower(1.0);
wheelController.setMotorControllerDeviceMode(DcMot orController.DeviceMode.READ_ONLY);
In this case, all of the lines above the READ_ONLY line won't take effect until the
device is placed back into write mode.
*/
// What this says is that if you're switching to a new mode then there better not
// be any existing commands still in the queue for that device. In effect, mode switches
// should (conservatively) happen at the TOP of a loop() call so that they are compatible
// with anything else that is issued to that controller in that call.
//
// We accomplish this by waiting until any incompatible operations were executed on
// previous loop() calls. *New* incompatible operations are prevented from starting
// by the fact that this controller object uses synchronized methods.
int oppositeKey =
newMode == DeviceMode.READ_ONLY
? this.controllerReadThunkKey
: this.controllerWriteThunkKey;
SynchronousOpMode.synchronousThreadWaitForLoopCycleEmptyOfActionKey(oppositeKey);
// Tell him to switch
this.setMotorControllerDeviceMode(newMode);
// Wait until he gets there
do {
SynchronousOpMode.synchronousThreadIdle();
this.controllerMode = this.getMotorControllerDeviceMode();
} while (this.controllerMode != newMode);
}
}
