//TODO: SBH - Figure out how to register/deregister if timing shows its needed
//colorSensor.getI2cController()
//           .registerForI2cPortReadyCallback(colorSensor, colorSensor.getPort());
public void runOpMode()
    {
        initRobot();
        gyro.calibrate();
        while (!isStopRequested() &&
               gyro.isCalibrating())
        {
            sleep(50);
        }
        waitForStart();
        //startRobot();
        //sleep(1000);  //Give motors time to ramp up to speed
        long minLoopInterval = Long.MAX_VALUE;
        long maxLoopInterval = Long.MIN_VALUE;
        long loopCount = 0;
        long prevLoopTime;
        long minSampleInterval = Long.MAX_VALUE;
        long maxSampleInterval = Long.MIN_VALUE;
        long sampleCount = 0;
        long prevSampleTime;
        long totalSampleTime = 0;
        long startTime = System.nanoTime();
        prevSampleTime = startTime;
        prevLoopTime = startTime;
        int prevSample = getSensorValue();
        ElapsedTime spdTimer = new ElapsedTime();
        double spdTimout = 2.0;
        double curSpd = 0.3;
        long stoppedSleepTime = 0;
        //TODO: SBH - Figure out how to register/deregister if timing shows its needed
        //colorSensor.getI2cController().deregisterForPortReadyCallback(colorSensor.getPort());
        while (opModeIsActive() && curSpd <= 0.35)
        {
            Log.i(TAG, String.format(Locale.US, "TESTING AT SPEED = %5.2f", curSpd));
            startRobot(curSpd);
            while (spdTimer.seconds() < spdTimout)
            {
                long currTime = System.nanoTime();
                int currSample = getSensorValue();
                long loopInterval = currTime - prevLoopTime;
                long sampleTime = 0;
                boolean sampleIsNew = false;
                if (currSample != prevSample)
                {
                    sampleIsNew = true;
                    sampleTime = currTime - prevSampleTime;
                    sampleCount++;
                    prevSample = currSample;
                    totalSampleTime += sampleTime;
                    prevSampleTime = currTime;
                    if (sampleTime < minSampleInterval)
                        minSampleInterval = sampleTime;
                    else if (sampleTime > maxSampleInterval)
                        maxSampleInterval = sampleTime;
                }
                if (loopInterval < minLoopInterval)
                {
                    minLoopInterval = loopInterval;
                } else if (loopInterval > maxLoopInterval)
                {
                    maxLoopInterval = loopInterval;
                }
                if (sampleIsNew)
                {
                    logRobot(String.format(Locale.US, "NEW SAMPLE - sampleTime %7.3f",
                            sampleTime / MS2NS));
                }
                logRobot(String.format(Locale.US, "[%4d:%7.3f] LoopInterval=%7.3f, ",
                        loopCount, (currTime - startTime) / MS2NS, loopInterval / MS2NS));
                prevLoopTime = currTime;
                if (useSleep)
                {
                    long startSleepTime = System.nanoTime();
                    waitForTick(sleepMs);
                    long endSleepTime = System.nanoTime();
                    totalSampleTime -= (endSleepTime - startSleepTime);
                }
                loopCount++;
            }
            stopRobot();
            long startStopSleepTime = System.nanoTime();
            waitForTick(500);
            stoppedSleepTime += (System.nanoTime() - startStopSleepTime);
            spdTimer.reset();
            if(curSpd < 0.15) curSpd += 0.01;
            else curSpd += 0.05;
            //TODO: SBH - Figure out how to register/deregister if timing shows its needed
            //colorSensor.getI2cController()
            //           .registerForI2cPortReadyCallback(colorSensor, colorSensor.getPort());
        }
        stopRobot();
        long endTime = System.nanoTime() - stoppedSleepTime;
        Log.i(TAG, String.format(
                "Loop: MinInterval=%7.3f, MaxInterval=%7.3f, AvgInterval=%7.3f",
                minLoopInterval/MS2NS, maxLoopInterval/MS2NS,
                (endTime - startTime)/MS2NS/loopCount));
        Log.i(TAG, String.format(
                "Sensor: MinSampleInterval=%7.3f, MaxSampleInterval=%7.3f, AvgSampleInterval=%7.3f %7.3f",
                minSampleInterval/MS2NS, maxSampleInterval/MS2NS,
                (endTime - startTime)/MS2NS/sampleCount,
                (double)totalSampleTime/MS2NS/sampleCount));
    }
double curSpd = 0.3;
long stoppedSleepTime = 0;
//TODO: SBH - Figure out how to register/deregister if timing shows its needed
//colorSensor.getI2cController().deregisterForPortReadyCallback(colorSensor.getPort());
while (opModeIsActive() && curSpd <= 0.35)
{
Log.i(TAG, String.format(Locale.US, "TESTING AT SPEED = %5.2f", curSpd));
long prevSampleTime;
long totalSampleTime = 0;
long startTime = System.nanoTime();
prevSampleTime = startTime;
prevLoopTime = startTime;
int prevSample = getSensorValue();
ElapsedTime spdTimer = new ElapsedTime();
double spdTimout = 2.0;
double curSpd = 0.3;
long stoppedSleepTime = 0;
//TODO: SBH - Figure out how to register/deregister if timing shows its needed
//colorSensor.getI2cController().deregisterForPortReadyCallback(colorSensor.getPort());
while (opModeIsActive() && curSpd <= 0.35)
{
Log.i(TAG, String.format(Locale.US, "TESTING AT SPEED = %5.2f", curSpd));
startRobot(curSpd);
while (spdTimer.seconds() < spdTimout)
{
long currTime = System.nanoTime();
int currSample = getSensorValue();
long loopInterval = currTime - prevLoopTime;
long sampleTime = 0;
boolean sampleIsNew = false;
waitForStart();
//startRobot();
//sleep(1000); //Give motors time to ramp up to speed
long minLoopInterval = Long.MAX_VALUE;
long maxLoopInterval = Long.MIN_VALUE;
long loopCount = 0;
long prevLoopTime;
long minSampleInterval = Long.MAX_VALUE;
long maxSampleInterval = Long.MIN_VALUE;
long sampleCount = 0;
long prevSampleTime;
long totalSampleTime = 0;
long startTime = System.nanoTime();
prevSampleTime = startTime;
prevLoopTime = startTime;
int prevSample = getSensorValue();
ElapsedTime spdTimer = new ElapsedTime();
double spdTimout = 2.0;
double curSpd = 0.3;
long stoppedSleepTime = 0;
//TODO: SBH - Figure out how to register/deregister if timing shows its needed
//colorSensor.getI2cController().deregisterForPortReadyCallback(colorSensor.getPort());
while (opModeIsActive() && curSpd <= 0.35)
{
Log.i(TAG, String.format(Locale.US, "TESTING AT SPEED = %5.2f", curSpd));
startRobot(curSpd);
while (spdTimer.seconds() < spdTimout)
{
long currTime = System.nanoTime();
int currSample = getSensorValue();
long loopInterval = currTime - prevLoopTime;
long sampleTime = 0;
boolean sampleIsNew = false;
if (currSample != prevSample)
{
sampleIsNew = true;
sampleTime = currTime - prevSampleTime;
sampleCount++;
prevSample = currSample;
totalSampleTime += sampleTime;
prevSampleTime = currTime;
if (sampleTime < minSampleInterval)
minSampleInterval = sampleTime;