Hi
this is my first project with the bot board and I’d like some help getting started. My bot has 6 motors, 4 with quadrature encoders. I’m going to use HSERVO to drive the motor controllers but how do I connect and monitor the encoder inputs?
I’m sure this must be pretty routine biut I can’t seem to find much in the forum. Do I use interrupts? Do I off load it to another board? Is there any sample code around?
Any help would be appreciated.
Thanks in advance
Hi Cheeter,
Not sure if there is enough information here to give you a complete answer, like what type of encoder you are using… But from your description (HSERVO), I assume you have an Atom Pro 28 plugged into your bot board. I know that some of this is covered in different threads…
My Rover has 4 motors, two with encoders. These are the ones sold by Lynxmotion (lynxmotion.com/Product.aspx?productID=598&CategoryID=11).
Depending on your encoder which I will assume is similiar (if not the same), you will need to build a set of connectors to connect it to your bot board. On mine, I had it split to two different 3 pin connectors. The first one had Channel A, plus power, plus ground on the appropriate pin. The second one just had Channel B on it.
I then plugged the Channel A connector into one of the IO pins on the Bot board that can gnererate an interrupt. The obvious choices are WKP0-6 or IRQ1-3. On my bot I am using IRQ2 (and soon IRQ3) as I am using WKP0-6 to monitor signals coming from the Laser 6 receiver.
Currently my code does not get the absolutely best precission available from a quadrature encoder, but it is probably good enough for what I need. What I do is:
For the interrupt I am handling I setup to first interrupt on the leading edge. When the interrupt happens, I check the state of Channel B. If it is high at this point I know that I am going forward(may be reverse depending on the orientation of the motor) and I increment my counter, else I decrement it. I then set the interrupt source for the trailing edge of signal and when it happens, you do the same thing, except the test for Channel B is in opposit…
You could also implement it by having interrupts for both channels , which would increase your precession, but that is processing a lot of interrupts. You might also be able to implement this by polling the channells, but you will most likely miss some…
Your code can also remember the direction the motor is going. Also it could keep timings and as such could calculate speed. I will prbably add some of this later.
As for code, I will be glad to share what I have. However I should warn you that I am using an assembly language interrupt handler. There is more information about how to use these interrupts up on this thread:
lynxmotion.net/viewtopic.php?t=3496.
There are probably better code samples around, but here is some extracts from my Rover code. So far I simply display counts from the encoders on an LCD and it appears to work. Later I will integrate into some autonomous roving code which will better test it. So your millage may differ… Here is the code for processing one encoder on IRQ2…
[code]ENCODER1B con p16 'encoder 1 Channel B
ENCODER2B con P17 'encoder 2 Channel B
ENCODER1A con P18 'encoder 1 Channel A - IRQ2
ENCODER1B con P19 'encoder 2 Channel A - IRQ3
lEncoder1Cnt var slong
;=============================- Interrupt definitions =======================================
ONASMINTERRUPT IRQ2INT, HANDLE_IRQ2
;============================== Start of CODE ================================================
;----------------------------- Init System components ---------------------------------------
PMR1.bit6 = 1 ; enable pin to IRQ2 interrupt instead of normal I/O
IEGR1.bit2 = 1 ; Interrupt IRQ2 on risigin edge
input ENCODER1B
input ENCODER2B
lTimerWOverflowCnt = 0; // initialize timer overflow count…
ENABLE IRQ2INT
;================================ Main Loop =================================================
main_loop:
; … do whatever your code wants.
; The veriable lEncoder1Cnt has the current encoder count.
; you could also modify the interrupt handler to remember direction.
; Also you could add timings code to calculate speed…
goto main_loop
;---------------------------------------------------------------------------------------
; Handle IRQ2 interrupt - We will have the encoder Channel A on this interrupt.
; - We are handling both rising and falling edge of the encoder…
BEGINASMSUB
HANDLE_IRQ2
push.l er1 ; first save away ER1 as we will mess with it.
bclr #2,@IRR1:8 ; clear the IRQ2 bit in the interrupt pending mask
andc #0x7f,ccr ; allow other interrupts to happen
mov.l @LENCODER1CNT:32,er1 ; increment our count of encoders we have seen
btst.b #2,@IEGR1:8
beq _IQ2_FALL_EDGE:8
bclr #2,@IEGR1:8 ; Set to falling edge
; ok now lets check the state of ChannelB - Pin P16 with on H8 is P11
btst #1,@PDR1:8
beq _IQ2_RE_NB:8
dec.l #1,er1 ; Channel B was high first so going in reverse
bra _IQ2_EXIT:8
_IQ2_RE_NB:
inc.l #1,er1 ; Channel B was low so we are going forward
bra _IQ2_EXIT:8
; Ok process falling edge of Channel A - sortof reverse for leading edge for state of channel B
_IQ2_FALL_EDGE:
bset #2,@IEGR1:8
; ok now lets check the state of ChannelB - Pin P16 with on H8 is P11
btst #1,@PDR1:8
beq _IQ2_FE_NB:8
inc.l #1,er1 ; Channel B was high so we are going forward
bra _IQ2_EXIT:8
_IQ2_FE_NB:
dec.l #1,er1 ; Channel B was low first so going in reverse
; save away the updated counter
_IQ2_EXIT:
mov.l er1, @LENCODER1CNT:32
pop.l er1
rte
ENDASMSUB
[/code]
Thanks for the reply Kurte.
I’m building a tractor type device with drive motors on both tracks. The motors have built in encoders providing 5 pulses per rev. I need to drive it around autonomously in a preprogrammed path. I have a scanner which will provide pretty accurate positionoing but I have to stop my tractor to get an update. I figure I can do a pretty good job of dead reckonning with the encoder inputs. The better the dead reckonning the fewer times I’ll have to stop to update position.
I had hoped that the wiring would be as you described (that’s good) and I thought I could use the interrupt function to simply index a variable that would be available to my main logic. I hadn’t really considered directionality but I guess I’d be wise to inclide that in the interrupt routine.
If I have 4 motors generating a total of 40 pulses per rev and about 100 rev per second that’s about 4000 interrupts per second. Do you think the pro board can handle that load and still allow me room for my mian logic or do you think I’ll need to off load the process?
Thanks,
Cheeter
You are welcome. I think AcidTech could better answer some of this, but if all you wish to do in your interrupt handler is to increment a variable, my quick calculations for an assembly language interrupt handler for IRQ2 is something like:
14 - Interrupt latency
10 - push.l er1
08 - bclr #2,@IRR1:8
10 - mov.l @MYVAR, er1
02 - inc.l #1, er1
10 - mov.ler1, MYVAR
10 - pop.l
10 - rte
Which if I added this up came out to 74 clock cycles per interrupt. At 2000 interrupts per second you would use up: 296000 cycles, which I believe is less than 2% of the CPU. Again Nathan would be a better expert on this and obvlously the more complicated you make your handler the more CPU it will take up…
P.S. - You don’t necessarily have to write this handler in assembly language, it simply makes the processing a lot quicker. In the code above, you can access the variable MYVAR (in upper or lower case) as a long variable…
Kurt