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));
    }
public void runOpMode()
    {
        initRobot();
        gyro.calibrate();
        while (!isStopRequested() &&
               gyro.isCalibrating())
        {
            sleep(50);
        }
        waitForStart();
       
       
        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;
       
       
        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;
           
           
           
        }
        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));
    }
public void runopmode() { initrobot(); gyro.calibrate(); while (!isstoprequested() && gyro.iscalibrating()) { sleep(50); } waitforstart(); 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; 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; } 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)); }