Tag Archives: dual motor controller v2

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}

Why two motor controllers?

Someone at work asked me why I put two separate motor controller boards on the robot. Well, it would have been very easy to do so.  And in my original prototype, I had done exactly that, using an Arduino UNO and the fantastic Adafruit Motor Shield.  My motor controller boards use the same H-bridge chip as Adafruit’s motor shield, but that’s where the similarities end.

The core of the Dual Motor Controller is the ATMEGA328P microcontroller.  Among this chip’s many talents: six of the pins can output a PWM signal.  But two of them are special.  Two of them can go ultrasonic without messing up the other timers!

eagle

It’s those two, in the middle, highlighted in bright red.

These two can run a PWM frequency around 32kHz, which is well within spec for the TB6612FNG motor driver, but it’s conveniently outside the hearing range for most humans.  You know that “hum” you sometimes get when electric motors aren’t going at full speed?  This gets rid of that!  It’ll annoy all the neighbourhood dogs, though.

But there’s more!

Having a separate board for the front and rear motors made it possible to mount the optical encoders directly to the same board as the controller.  A slotted disc on each wheel passes through an infrared break-beam detector, and results in a number of pulses that the microcontroller can count, which correlates to the speed of the wheel.  (I can only measure speed and distance with these sensors.  I’m only able to infer the direction.)

And finally:

Building two separate boards also saved me a little bit of money on the PCB.  OSH Park does a great job of manufacturing PCBs for me, but custom PCBs are certainly not the cheapest things in the world.  At $5.00 US per square inch, they’re about the only thing more expensive than Vancouver real estate.  But you get three boards for that price.   So having a need for more than one board certainly makes sense to me!

Some parts of the Office Robot

A few quick snaps of the Office Robot.  The motor control boards are essentially complete (working out the kinks in the firmware, still).  The connection between the motor control boards and the web interface (courtesy of a Raspberry Pi) is obviously still pretty much crap.  But it does work, and I’ve transferred the schematic to Eagle.  All those jumper wires, and pretty much everything that’s plugged into that breadboard will be replaced with a single PCB!

DSC_9236

The motor control boards are a little prettier.  There’s one for each set of wheels, and an interface board in the middle.

DSC_9222