Conceptual help using timers to read motor encoders

[techy101]

I am doing a project with the STM32F407VGT that has six small DC motors, each with a quadrature encoder built in. The plan is to use these signals for closed loop position and speed control; I was hoping to get some basic conceptual guidance on how to best do this. The signal coming out of the encoders will max out at ~1kHz.

Direction of movement is needed, but does not need to be known at all times and I'd been planning on hooking the second channel of the encoder up to a GPIO and simply poll the state of the two lines once in a while to verify that direction matches what has been set.

As for the other encoder channel, I need to know both period and the number of rising edge events to determine position. (I don't need the precision of rising and falling due to some pretty aggressive gearing on the motors)


I’ve spent a while working my way through the reference manual, cross-series timer overview, and general purpose timer cookbook documents. Thus far I’ve been able to come up with a few different ways to achieve this, but I’m not sure which is the most appropriate and will have the least overall performance impact.

1. Set one timer for each encoder in up counter mode; sample the count value at a known rate using another timer interrupt and calculate the average period over the sample time. This would be set up so all six counters were polled and calculated at the same time. My understanding is that this has the benefit of being pretty simple, but may lack in accuracy and the interrupt handler could take a significant amount of time. Also, this seems like it will fall apart when the motor is moving at very slow speed and it would require the use of seven total timers.

2. Use the dedicated encoder mode, but my understanding of this is that only two motors can be used in this way. If this is correct then the method will not work.

3. Use input capture mode. This seems like the most appropriate option from the reading I’ve done, but is also the method I have the most confusion about. It seems like the simple approach to this is to use four channels (or two? I’ve think I’ve seen talk of only being able to use two channels of a four channel timer in this method). Each rising edge event on each channel triggers an interrupt handler which calculates the delta between the previous event and the current one, stores that value, and increments a counter variable. This seems concerning having up to ~6000 interrupts happening per second.

The other method with input capture seems to be using the same setup, but using a DMA request to store off the timer values. If I understand this correctly, which is questionable at best, these values would need to be polled by a flag set in a register whenever there was an event. The code executed when the flag is set would both read the current value, calculate the delta, and increment a counter.


Are these the only ways to approach this, and conceptually do I understand them correctly? Any guidance to which of these approaches is most appropriate, or if there is another that is better, would be greatly appreciated.

Thank you,

Matthew

[rstofer]

I've never done more than two motors...

I had the A and B signals from each encoder come into the uC on an interrupt pin (usually a port pin with Interrupt On Change).

Then I build a state machine where I capture the sequence of transitions and increment or decrement a counter at every transition.

With 6 motors, that is a lot of interrupts.  The ISR needs to be very short and very fast.  One thing that helps is when you have a way to set a pin to interrupt only on one edge.  If the signal is high, you want an interrupt when it goes low, when it is low, you want an interrupt when it goes high.  This also needs to be dealt with in the ISR since you will, at different times, be interrupt on both the leading and falling edge.  But when the interrupt comes along, I don't need to figure out whether the signal went high or low, I already know.

Sit down with a pencil and paper and assume both A & B are low.  Now draw the possible transitions:  A could go high (we'll call that forward) or B could go high.  If it's A, we increment the count, if it's B, we decrement the count.  Say we're in state A=1, B=0.  The next thing that can happen is A goes low (and we decrement the count) or B goes high and we increment the count.  Now both signals are high.  And so on...

After updating the count, change the interrupt edge for the signal that just transitioned and you are done with the interrupt.

Keep the code short!  I hesitate to suggest it but I might do it in assembly language so I could pare down the preamble and postamble getting in and out of the ISR.  I might check the compiler documentation and see which registers they leave alone.  I might then try to keep the counters in registers.

[AndyC_772]

IMHO the timers in STM32 are horrendously complicated, and that alone is a good enough reason to consider using something else.

Many of my designs end up with an FPGA on them somewhere, so I'd get the FPGA to count edges and work out where the motors are. It's much clearer to describe the meaning of the external signals and how they should be interpreted in VHDL, than to poke obscure timer registers in a way which you hope really means what you think it does.

Then, at regular intervals, have an ISR kick off an SPI transfer via DMA, which sucks the most up-to-date position info out of the FPGA. When the DMA transfer is complete, your CPU has the information it needs.

This solution does require extra silicon, of course, but it can run extremely fast, it's independent of CPU performance, and it scales to any number of motors.

[rstofer]

IMHO the timers in STM32 are horrendously complicated, and that alone is a good enough reason to consider using something else.

Many of my designs end up with an FPGA on them somewhere, so I'd get the FPGA to count edges and work out where the motors are. It's much clearer to describe the meaning of the external signals and how they should be interpreted in VHDL, than to poke obscure timer registers in a way which you hope really means what you think it does.

Then, at regular intervals, have an ISR kick off an SPI transfer via DMA, which sucks the most up-to-date position info out of the FPGA. When the DMA transfer is complete, your CPU has the information it needs.

This solution does require extra silicon, of course, but it can run extremely fast, it's independent of CPU performance, and it scales to any number of motors.

It is pretty easy to get SPI working at about 10 MHz and if the counters in the FPGA are 32 bit, 6 channels will require 192 bits and it will take about 20 us to transfer all registers to the uC.

Whether 20 us of lag is important to the control loop, I don't know.

I might even use a CPLD for this application.  There isn't much going on for the counters.  Write HDL for 1 channel, copy and paste for 6 channels.

OR...

Put the entire motor control in the FPGA and let the uC provide the high level functions.

The thing is, there's a heck of a learning curve to move from a uC to an FPGA!

[CM800]

IMHO the timers in STM32 are horrendously complicated, and that alone is a good enough reason to consider using something else.

Many of my designs end up with an FPGA on them somewhere, so I'd get the FPGA to count edges and work out where the motors are. It's much clearer to describe the meaning of the external signals and how they should be interpreted in VHDL, than to poke obscure timer registers in a way which you hope really means what you think it does.

Then, at regular intervals, have an ISR kick off an SPI transfer via DMA, which sucks the most up-to-date position info out of the FPGA. When the DMA transfer is complete, your CPU has the information it needs.

This solution does require extra silicon, of course, but it can run extremely fast, it's independent of CPU performance, and it scales to any number of motors.

It is pretty easy to get SPI working at about 10 MHz and if the counters in the FPGA are 32 bit, 6 channels will require 192 bits and it will take about 20 us to transfer all registers to the uC.

Whether 20 us of lag is important to the control loop, I don't know.

I might even use a CPLD for this application.  There isn't much going on for the counters.  Write HDL for 1 channel, copy and paste for 6 channels.

OR...

Put the entire motor control in the FPGA and let the uC provide the high level functions.

The thing is, there's a heck of a learning curve to move from a uC to an FPGA!

I would strongly suggest to go with the CPLD, MAX II or MAX V are great, I've already used them for encoder position feedback before.

[techy101]

Thank you for the suggestions. Moving to an FPGA or CPLD would be a stretch for the project, and the hope was to really stick with a single chip solution.

For the sake of argument, assuming this constraint do you have thoughts on the best approach?

Thank you,
Matthew

[nctnico]

I'd use a (high priority) 10kHz timer interrupt and handle the quadrature decoders in software (inside the interrupt routine!).

[rstofer]

Thank you for the suggestions. Moving to an FPGA or CPLD would be a stretch for the project, and the hope was to really stick with a single chip solution.

For the sake of argument, assuming this constraint do you have thoughts on the best approach?

Thank you,
Matthew

I would use the state machine concept I posted above.  It handled two motors with a mere AVR <something or other>, a high end STM32F4 should be able to handle 6 motors.

I don't imagine the code for an AVR will be much help but try to see what happens to the state and count for various sequences of A-B transitions.  You can see that each ISR is very short.

Code: [Select]

#include <avr/io.h>
#include <avr/interrupt.h>

#include <stdio.h>
#include <ctype.h>

#include "defines.h"

volatile int8_t  RightWheelState = 0;
volatile int32_t RightWheelCount = 0;
volatile int8_t  LeftWheelState  = 0;
volatile int32_t LeftWheelCount  = 0;

volatile uint8_t urx;
volatile uint8_t urx_recv;

/* Quadrature Encoder Scheme
 *
 * There are two pairs of quadrature signals, one for each wheel.
 *  The right wheel is on EXINT4 (PE4) and EXINT5 (PE5)
 *  The left  wheel is on EXINT6 (PE6) and EXINT7 (PE7)
 *  Each encoder has two outputs A & B.  For convenience, A is considered the MSB
 * while B is considered the LSB.
 */

const int32_t EncoderB[] = {+1,-1,-1,+1};
const int32_t EncoderA[] = {-1,+1,+1,-1};

SIGNAL(SIG_INTERRUPT4) // encoder B for right wheel
{
EICRB           ^= 0x01; // toggle the edge bit
RightWheelState ^= 0x01;
RightWheelCount += EncoderB[RightWheelState];
}

SIGNAL(SIG_INTERRUPT5) // encoder A for right wheel
{
EICRB         ^= 0x04;
RightWheelState ^= 0x02;
RightWheelCount += EncoderA[RightWheelState];
}

SIGNAL(SIG_INTERRUPT6) // encoder B for left wheel
{
EICRB          ^= 0x10;
LeftWheelState ^= 0x01;
LeftWheelCount += EncoderB[LeftWheelState];
}

SIGNAL(SIG_INTERRUPT7) // encoder A for left wheel
{
EICRB          ^= 0x40;
LeftWheelState ^= 0x02;
LeftWheelCount += EncoderA[LeftWheelState];
}

void IOInit(void)
{
UBRR0H = 0;
UBRR0L = BAUD;
UCSR0B = _BV(RXEN) | _BV(TXEN) | _BV(RXCIE);
}

static int uart_putchar(char c, FILE * stream)
{
if (c == '\n')
uart_putchar('\r', stream);
while (! (UCSR0A & _BV(UDRE)))
;
UDR0 = c;
return c;
}

SIGNAL(SIG_UART0_RECV)
{
uint8_t s;

s = UCSR0A;
urx = UDR0;
if (bit_is_clear(s, FE))
urx_recv = 1;
}

void QuadInit(void)
{
LeftWheelState  = (PORTE & 0xC0) >> 6;
RightWheelState = (PORTE & 0x30) >> 4;
EICRB = 0xff; // enable rising edge triggering on INT4..7
EICRB ^= (LeftWheelState  & 0x02) << 5; // if a signal is already set
EICRB ^= (LeftWheelState  & 0x01) << 4; // set the interrupt to falling edge
EICRB ^= (RightWheelState & 0x02) << 1;
EICRB ^= (RightWheelState & 0x01);
EIMSK = 0xf0; // enable these 4 interrupts
}

int main(void)
{
IOInit();
QuadInit();
sei();

fdevopen(uart_putchar, NULL);
printf("Hello World\n");

while (1) {
printf("LWS =%2d LWC =%6ld RWS =%2d RWC =%6ld\n",
LeftWheelState, LeftWheelCount, RightWheelState, RightWheelCount);
}
}


[jesuscf]

Quote from: techy101 on Yesterday at 11:17:59 PM

I am doing a project with the STM32F407VGT that has six small DC motors, each with a quadrature encoder built in. The plan is to use these signals for closed loop position and speed control; I was hoping to get some basic conceptual guidance on how to best do this. The signal coming out of the encoders will max out at ~1kHz.
.
.
.
Thank you,

Matthew

Remember  that a quadrature encoder gives you two signals out of phase by 90 degrees.  For 6 encoders, that would be 12 signals.  There is a nice article in Circuit Cellar Issue 152, from March 2003 titled "Using Rotary Encoders as Input Devices" by Brian Millier that can be helpful.  Long ago I used four quadrature encoders connected to a slow 8051 without problem, so a STM32 should handle 6 without trouble.  As rstofer suggested above, a fixed timer interrupt with a state machine also worked fine for me.

[nctnico]

Rstofer uses interrupts for each encoder input. This method will end in chaos when (not if) one or more of the encoder signals fails. Also having may different interrupts means you'll also have the overhead of saving & recalling the registers from stack for each interrupt. That is not very efficient.

[tatus1969]

ST provides a motor control library that does this job and many others. Take a look here:

http://www.st.com/en/embedded-software/stsw-stm32100.html

I am using it and have the source code of it available. It is quite efficient, using integer (fix point) arithmetic all over the place.

It is designed for brushless motors, but you should be able to use the encoder part as well. If you don't want this: they also explain in very much detail how they implement its various features. The STM32's timers have been specifically designed to support quadrature encoders.

[rstofer]

Rstofer uses interrupts for each encoder input. This method will end in chaos when (not if) one or more of the encoder signals fails. Also having may different interrupts means you'll also have the overhead of saving & recalling the registers from stack for each interrupt. That is not very efficient.

The alternative is to have even more timer interrupts (some multiple of the fastest edge) and look for edges.  It's true that the timer interrupt is just one interrupt but there's a lot going on looking at encoder edges for 6 motors and 4 edges per encoder.  You may also be looking for edges of encoders that aren't even in use.  With the interrupt code, if there is no edge, there is no interrupt.

Yes, if the encoder fails, the code can't fix it.  But there's nothing to fix in any event.  The count remains unchanged and if the programmer is clever, he can check for that in code somewhere else.  With the timer approach, say handling all 6 motors, how is a failed encoder handled?  Same story!  There are no edges and the count doesn't change.  It looks like the motor is stopped or stalled.

Either way will work.  Try them both and pick the one that works best.

There is a 'gotcha' with my sample code in that fetching the multi-byte counters is not an atomic operation on an 8 bit CPU.  The value can change while various bytes are being read.  Using a 32 bit CPU will prevent this or interrupts can be disabled while the value is fetched.

[dannyf]

"I was hoping to get some basic conceptual guidance on how to best do this. The signal coming out of the encoders will max out at ~1kHz. "

Use interrupts.

The timer polling approach wastes processor cycles unnecessarily. You are talking about two channels per encoder and 6 encoders. With an interrupt, you can do easily 10khz per encoder on 1mips. So 1khz per encoder is about 0.1mps and for 6 encoders you use less than 1mips. That's nothing for your target chip.

[nctnico]

@rstofer: a broken encoder may start firing interrupts at an uncontrolled rate = the entire device crashes.
@dannyf: a single timer interrupt is more efficient under high load (less overhead) and the cpu load is way more predictable. Also you need to be able to deal with the worst case situation so you need to reserve processing power anyway.

[Pack34]

You can make this a bit easy on yourself by limiting your encoder pulses as much as possible. Consider how much resolution you actually need.

A 20 CPR encoder attached to the rear shaft of a 1:71 gear ratio motor would give you an effective CPR of 1,420. Assuming a 6 inch diameter wheel will give you about 75 counts per inch. This is way more than you will likely need. You do not need a high resolution encoder for accurate positioning. It can actually be a great detriment due to interrupt timing issues destroying your control COM and "slop" in the wheel assembly.

Attempting to monitor six encoders moving at a decent speed with a single STM32 is a recipe for disaster. If you want to only use ARM chips, you will be best suited to having a single chip act as an encoder monitor for each wheel, and then request updated counts as appropriate from your primary processor.  This will be the most reliable solution other than using an FPGA.

[rstofer]

nctnico's point about the overhead of entering and leaving interrupts is absolutely something to think about.  I mentioned the preamble and postamble code emitted by GCC in an earlier reply.  What to do...

1) Ignore it and see how it works out.
2) Write the code in assembly language and save only the registers that are actually used.

Actually, I could see a set of short interrupt handlers written in assembly code as the best way to handle this.

I could use the 'naked' attribute on the handler written in C as long as I saved and restored the registers I was using.  Maybe I would even write the assembly code as inline.

It might be worth looking at the code the compiler generates for the handlers and see how many instruction cycles could be saved by writing the handler in assembly code.  It might not be that many!

With so many interrupts (in interrupts per second, not the quantity of handlers), it might be important to worry about the preamble and postamble code.  OTOH, processors are pretty fast.  Maybe it just doesn't matter.

You can add code to the interrupt handler to set a pin when inside the handler.  Set it on entry and clear it on exit.  You can then look at the pin with a scope to get an idea of how much time is spend in the handler.  You won't see the preamble or postamble so the time will be short of what is actually required.  You can come up with a way to OR the 6 handlers.   

You could even use a counting semaphore and the handler that sees the count at zero on entry sets the pin before incrementing the count and the handler that see the count go to zero after decrementing the count can clear the LED.  The code to handle the semaphore (which will be removed) is an approximation of the preamble and postamble overhead.

Then, you could use a timer to set various pins (12 of them) in a maximum rate pattern of encoder signals for all 6 motors.  It should produce an interesting scope waveform on the LED pin. 

[rstofer]

@rstofer: a broken encoder may start firing interrupts at an uncontrolled rate = the entire device crashes.
@dannyf: a single timer interrupt is more efficient under high load (less overhead) and the cpu load is way more predictable. Also you need to be able to deal with the worst case situation so you need to reserve processing power anyway.

And a shorted MOSFET blows the entire H-bridge melting the Li-Po battery and burning the house down.  Nothing is perfect! 

The possibility of losing a photoelectric encoder are too remote to consider.  Unless it is on a bot playing war games in the mud.  Even then, it won't go batshit, it just won't send any transitions at all.  It's true that ragged edges may generate excessive interrupts due to 'bounce' but a low pass filter ought to cover that.  In any event, the interrupt code deals with it by transitioning between two states.  Increment the count, decrement the count, increment the count, decrement the count.  Eventually it will stabilize.  And I don't see how timer interrupts overcome the failed encoder.  There still won't be any transitions.

I don't see how you can oversample the 24 edges on every interrupt and call it more efficient than just handling the edges that actually occur.

[rstofer]

You can make this a bit easy on yourself by limiting your encoder pulses as much as possible. Consider how much resolution you actually need.

A 20 CPR encoder attached to the rear shaft of a 1:71 gear ratio motor would give you an effective CPR of 1,420. Assuming a 6 inch diameter wheel will give you about 75 counts per inch. This is way more than you will likely need. You do not need a high resolution encoder for accurate positioning. It can actually be a great detriment due to interrupt timing issues destroying your control COM and "slop" in the wheel assembly.

Attempting to monitor six encoders moving at a decent speed with a single STM32 is a recipe for disaster. If you want to only use ARM chips, you will be best suited to having a single chip act as an encoder monitor for each wheel, and then request updated counts as appropriate from your primary processor.  This will be the most reliable solution other than using an FPGA.

Using a separate ARM chip for the encoders is probably better than using an FPGA.  At least conceptually...

So, the peripheral ARM deals with the encoders and produces 6 each 32 bit counters.  It transfers these to the host via SPI which is VERY fast.  It might not matter which CPU is the master but the host ('boss') probably wants to use DMA to receive the data and put it in 6 specific locations (circular buffer).  These values will be continuously updated and no code intervention is required.  The counter values just magically appear!  I like magic!

I think it would be easier to just send all 6 counters than to send just one.  It makes the DMA setup a lot easier on the host.  Just keep filling the same 6 word circular buffer.

The only reason this is better than using an FPGA is the learning curve to get the FPGA to provide similar performance.  The CPLD or FPGA could handle many more encoder edges and not get lost.   SPI is pretty easy on an FPGA, especially for the master end of the bus.

ETA:  I don't think the STM32F4 handles 32 bit transfers via SPI.  This needs to be considered.  The peripheral device will need to fully update the count before sending it via SPI and the DMA on the host needs to understand that the 6 counters require 12 shorts or even 24 bytes.

http://www.st.com/content/ccc/resource/training/technical/product_training/d9/cf/69/ae/71/43/4f/23/STM32L4_Peripheral_SPI.pdf/files/STM32L4_Peripheral_SPI.pdf/jcr:content/translations/en.STM32L4_Peripheral_SPI.pdf

[rstofer]

Here's an experiment:  Write a timer interrupt handler that turns an LED on upon entry, does a little bit of nonsense work and turns the LED off upon exit.  Now set the timer to interrupt at some rate like 24 kHz which would correspond to every edge occurring 1000 times per second.

Look at the LED output on a scope and see how much time is being spent in the handler and how much is left for processing.

I like experiments!

Realize that the preamble and postamble code are not accounted for in the LED duration but you can add enough nonsense code to emulate those plus doing the real encoder work.

[dannyf]

"he overhead of entering and leaving interrupts is absolutely something to think about"

Not sure what overhead there is in leaving an isr.

With that said, a little math will help. Isr latency is 20 ticks (usually less and can be made less). 20 ticks to process the isr itself is reasonable but let's budget for 80 ticks. So a total of 40 to 100 ticks per isr invocation.

1khz per pulse train, 2 pulse trains per encoder, 6 encoders total means 1k x 2 x 6 x 100 or 1.2 mips on the high side and 0.5mips on the low side.

A lowly 8bit runs 10mips plus and a 32bit runs 36mips plus.

So do you still want to worry about the overhead?

[dannyf]

It is not hard to try similar math on the pilling approach.

But fundamentally, why would you run the risk of missing a pulse? It just seems highly counter intuitive to poll here.

[Sal Ammoniac]

2) Write the code in assembly language and save only the registers that are actually used.

Actually, I could see a set of short interrupt handlers written in assembly code as the best way to handle this.

I could use the 'naked' attribute on the handler written in C as long as I saved and restored the registers I was using.  Maybe I would even write the assembly code as inline.

An ARM Cortex will automatically save R0, R1, R2, R3, R12, LR, PC, and PSR on the stack when handling an interrupt. You have no control over this. You can therefore write an interrupt handler completely in C without worrying about saving registers as long as you only use the registers that get saved automatically.

[rstofer]

"he overhead of entering and leaving interrupts is absolutely something to think about"

Not sure what overhead there is in leaving an isr.

Restoring registers saved on entry

With that said, a little math will help. Isr latency is 20 ticks (usually less and can be made less). 20 ticks to process the isr itself is reasonable but let's budget for 80 ticks. So a total of 40 to 100 ticks per isr invocation.

1khz per pulse train, 2 pulse trains per encoder, 6 encoders total means 1k x 2 x 6 x 100 or 1.2 mips on the high side and 0.5mips on the low side.

A lowly 8bit runs 10mips plus and a 32bit runs 36mips plus.

So do you still want to worry about the overhead?

Might need to double it because there are two edge interrupts on each channel.

You're right, there is no real problem with interrupts.  I MIGHT do the experiments just because I like experiments although  I'm pretty sure I would just write the code and be done with it!  Because I am lazy...

[rstofer]

2) Write the code in assembly language and save only the registers that are actually used.

Actually, I could see a set of short interrupt handlers written in assembly code as the best way to handle this.

I could use the 'naked' attribute on the handler written in C as long as I saved and restored the registers I was using.  Maybe I would even write the assembly code as inline.

An ARM Cortex will automatically save R0, R1, R2, R3, R12, LR, PC, and PSR on the stack when handling an interrupt. You have no control over this. You can therefore write an interrupt handler completely in C without worrying about saving registers as long as you only use the registers that get saved automatically.

And this is the big difference between the early ARM7s and the more modern Cortex versions.  In theory, you don't even need to worry about which registers you will use because the compiler handles all that stuff.  Just write the code and move on. The compiler knows which registers are already saved.

Life is really sweet when you can write handlers as ordinary code and not have to spend a lot of time on the minute details.

[dannyf]

"MIGHT do the experiments just because I like experiments"

Try it. What I typically do is to flip s pin in the main loop. By comparing the frequency of that pin, you know how much processing power is lost to the isr.