I can reproduce the issue by simply calling lots of DriverStation.ReportError (no CAN devices whatsoever).
The cause of the delays appear to be the large amount of strings passed into standard error stream (stderr).
A similar issue can be reproduced in C++ due to the large amount of strings pass into standard output stream (cout).
After killing these two in WPILIB, I have reran your example and had good-rapid-control of the robot, even though the DS racked up lots of error messages.
I wrote up a tracker for this issue, but my recommendation is to not design your robot to produce tons of error information in its typical use. So if disconnecting a camera, i2c device, spi device, CAN device, etc. has the result of creating frequent DS messages (and therefore stderr/stdout streams) turn off the object in your code.
Doug I put together a quick class that will find any CTRE CAN devices on the bus so you can better switch between your test bed and robot. Create an object of this class type and call Find() at the beginning of your code. Use it’s output to determine if you should even bother polling out data from you CAN devices.
You could try using the getters you have now, but typically if a device “disappears” the getters will return the last good value and report an error to the DS, so they may not be best to serve your purposes.
package org.usfirst.frc.team3620.robot;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import edu.wpi.first.wpilibj.can.CANJNI;
public class DeviceFinder {
private ByteBuffer targetID = ByteBuffer.allocateDirect(4);
private ByteBuffer timeStamp = ByteBuffer.allocateDirect(4);
/** helper routine to get last received message for a given ID */
private long checkMessage(int fullId, int deviceID) {
try {
targetID.clear();
targetID.order(ByteOrder.LITTLE_ENDIAN);
targetID.asIntBuffer().put(0,fullId|deviceID);
timeStamp.clear();
timeStamp.order(ByteOrder.LITTLE_ENDIAN);
timeStamp.asIntBuffer().put(0,0x00000000);
CANJNI.FRCNetworkCommunicationCANSessionMuxReceiveMessage(targetID.asIntBuffer(), 0x1fffffff, timeStamp);
long retval = timeStamp.getInt();
retval &= 0xFFFFFFFF; /* undo sign-extension */
return retval;
} catch (Exception e) {
return -1;
}
}
/** polls for received framing to determine if a device is present.
* This is meant to be used once initially (and not periodically) since
* this steals cached messages from the robot API.
* @return ArrayList of strings holding the names of devices we've found.
*/
public ArrayList<String> Find() {
ArrayList<String> retval = new ArrayList<String>();
/* get timestamp0 for each device */
long pdp0_timeStamp0; // only look for PDP at '0'
long ]pcm_timeStamp0 = new long[63];
long ]srx_timeStamp0 = new long[63];
pdp0_timeStamp0 = checkMessage(0x08041400,0);
for(int i=0;i<63;++i) {
pcm_timeStamp0* = checkMessage(0x09041400, i);
srx_timeStamp0* = checkMessage(0x02041400, i);
}
/* wait 200ms */
try {
Thread.sleep(200);
} catch (InterruptedException e) {
e.printStackTrace();
}
/* get timestamp1 for each device */
long pdp0_timeStamp1; // only look for PDP at '0'
long ]pcm_timeStamp1 = new long[63];
long ]srx_timeStamp1 = new long[63];
pdp0_timeStamp1 = checkMessage(0x08041400,0);
for(int i=0;i<63;++i) {
pcm_timeStamp1* = checkMessage(0x09041400, i);
srx_timeStamp1* = checkMessage(0x02041400, i);
}
/* compare, if timestamp0 is good and timestamp1 is good, and they are different, device is healthy */
if( pdp0_timeStamp0>=0 && pdp0_timeStamp1>=0 && pdp0_timeStamp0!=pdp0_timeStamp1)
retval.add("PDP 0");
for(int i=0;i<63;++i) {
if( pcm_timeStamp0*>=0 && pcm_timeStamp1*>=0 && pcm_timeStamp0*!=pcm_timeStamp1*)
retval.add("PCM " + i);
if( srx_timeStamp0*>=0 && srx_timeStamp1*>=0 && srx_timeStamp0*!=srx_timeStamp1*)
retval.add("SRX " + i);
}
return retval;
}
}