Measuring wheel speed

Each wheel on the robot is paired with a slotted disc, which passes through an optical sensor.  There are 20 slots on each disc, which translates to 20 “on” pulses per rotation.  However, it’s easier for me to count both the “on” and “off” pulse edges, so I’m dealing with 40 pulse edges per each wheel rotation.  I’m going to call these ticks as that’s a lot easier to say.
DSC_9248But ultimately, I don’t care about ticks.  I do care about speed, though.  So I’m going to start there, and figure out what I need in order to calculate that.

Simply stated:

speed=\frac{distance}{time}

or:

speed=\frac{1 tick}{timeSinceLastTick}

or, if I throw some averaging in there:

speed=\frac{numTicks}{elapsedTime}

I can already see that there’s going to be a problem when the speed approaches zero, as there are no ticks to measure the timings.  So I’m going to start by measuring both the number of ticks that have occurred, and the amount of time that’s passed.  If too much time has passed, I’ll consider the speed to be zero.

// Store up to 10 "tick" (or "no-tick") events
const uint8_t NUM_SAMPLES = 10;

// Keep track of how many of the "tick" or "no-tick" events have happened.
uint8_t _tickCount[NUM_SAMPLES];
volatile uint8_t _totalTicks = 0;

// Keep track of the event timestamps
uint32_t _timings[NUM_SAMPLES];

// Some pointers to the arrays above.
volatile uint8_t _oldestIndex = 0;
volatile uint8_t _newestIndex = NUM_SAMPLES - 1;

When a tick occurs, the system calls an interrupt:

ISR(PCINT1_vect) {
    uint8_t newState = PINC & (_BV(PINC2) | _BV(PINC3));
    uint64_t nowStamp = micros(); // timestamp in µs
    uint8_t changes = newState ^ _encoderState;
    _encoderState = newState;

    if (changes & (_BV(PINC2))) {
        _totalTicks = _totalTicks - _tickCount[_oldestIndex] + 1;
        _tickCount[_oldestIndex] = 1;
        _timings[_oldestIndex] = nowStamp;

        // Move the pointers along, overflowing back to zero if needed.
        _newestIndex = _oldestIndex;
        _oldestIndex++;
        if (_oldestIndex >= NUM_SAMPLES) {
            _oldestIndex = 0;
        }
    }
}

If a tick didn’t happen, we inject a zero into the mix:

// Called when... nothing happened!
void nothingHappened() {
    cli();
    // Overwrite the "oldest" item in the averaging loop, and adjust pointers.
    _totalTicks = _totalTicks - _tickCount[_oldestIndex] + 0;
    _tickCount[_oldestIndex] = 0; // nothing happened!
    _timings[_oldestIndex] = micros();

    _newestIndex = _oldestIndex;
    _oldestIndex++;
    if (_oldestIndexB >= NUM_SAMPLES) {
        _oldestIndexB = 0;
    }
    sei();
}

And finally, in order to calculate the current speed:

double getSpeed() {
    cli(); // Make sure the interrupt doesn't fire while we're in here.
    uint8_t totalTicks = _totalTicks;
    uint64_t oldestTime = _timings[_oldestIndex];
    uint64_t newestTime = _timings[_newestIndex];
    sei(); // Set the interrupts free!

    // To get the time difference, we can't simply subtract,
    // because micros() overflows every 70 minutes or so.
    // Implementation of getTimeDiff is left as an exercise
    // to the reader.
    uint64_t timeDiff = getTimeDiff(oldestTime, newestTime);

    if (totalTicks == 0 || timeDiff == 0 || timeDiff > 10000000) {
        return 0.0;
    } else {
        return (float)((totalTicks - 1) * 1000000) / (float)(timeDiff);
    }
}

There’s a little more to it, but you’ve got the idea.  It’s just a bunch of code that watches how far we’ve gone, another bunch that watches the clock, and at the end of the day, it’s just simple physics:

speed=\frac{distance}{time}

Leave a Reply

Your email address will not be published. Required fields are marked *