Need Help ASM problem with Quadrature Encoder with speed

Julian800 · 2010-07-19 05:39

Hi Guys,
·I need some help with my code. I add speed code into Jeff's Quadrature Encoder v1.0 . what I want is getting speed of encoder (encoder count/ms). I only need·to use it on one encoder. I add few line of assembly , so I can start encoder as· Encoder.Start(19, 1, 1, @Pos)
the speed is write to Pos[noparse][[/noparse]2].

my code just accumulate CNT difference between each sample loop,·then compare with delay 1 ms CNT( I should use 80_000, but from testig 76_000 give me result close to actual speed.·I use 80Mhz system clock )

the result of speed output from my code about 95% is right, but get 1 count difference from time to time (following·are results of reading speed 20)
I am new to embedded controller and assembly. I just can't figure out where is the problem.· Please help !!

Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 19
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 19
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 21
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 21
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20
Speed: 20
Speed: 19
Speed: 20
Speed: 19
Speed: 20
Speed: 19
Speed: 20
Speed: 20
Speed: 20

(VAR
  byte          Cog             'Cog (ID+1) that is running Update
  byte          TotDelta        'Number of encoders needing deta value support.
  long          Pos             'Address of position buffer

PUB Start(StartPin, NumEnc, NumDelta, PosAddr): Pass
''Record configuration, clear all encoder positions and launch a continuous encoder-reading cog.
''PARAMETERS: StartPin = (0..63) 1st pin of encoder 1.  2nd pin of encoder 1 is StartPin+1.
''                       Additional pins for other encoders are contiguous starting with StartPin+2 but MUST NOT cross port boundry (31).
''            NumEnc   = Number of encoders (1..16) to monitor.
''            NumDelta = Number of encoders (0..16) needing delta value support (can be less than NumEnc).
''            PosAddr  = Address of a buffer of longs where each encoder's position (and deta position, if any) is to be stored.
''RETURNS:    True if successful, False otherwise.
  Pin := StartPin                                                                                     
  TotEnc := NumEnc                                                                                    
  TotDelta := NumDelta
  Pos := PosAddr
  Stop
  longfill(Pos, 0, TotEnc+TotDelta)
  Pass := (Cog := cognew(@Update, Pos) + 1) > 0

PUB Stop
''Stop the encoder-reading cog, if there is one.
  if Cog > 0
    cogstop(Cog-1)

PUB ReadDelta(EncID): DeltaPos
''Read delta position (relative position value since last time read) of EncID.
  DeltaPos := 0 + -(EncID < TotDelta) * -long[noparse][[/noparse]Pos][noparse][[/noparse]TotEnc+EncID] + (long[noparse][[/noparse]Pos][noparse][[/noparse]TotEnc+EncID] := long[noparse][[/noparse]Pos][noparse][[/noparse]EncID])
  
    
 
'************************************
'* Encoder Reading Assembly Routine *
'************************************
DAT
'Read all encoders and update encoder positions in main memory.
'See "Theory of Operation," below, for operational explanation.
'Cycle Calculation Equation:
'  Terms:     SU = :Sample to :Update.  UTI = :UpdatePos through :IPos.  MMW = Main Memory Write.
'             AMMN = After MMW to :Next.  NU = :Next to :UpdatePos.  SH = Resync to Hub.  NS = :Next to :Sample.
'  Equation:  SU + UTI + MMW + (AMMN + NU + UTI + SH + MMW) * (TotEnc-1) + AMMN + NS
'             = 92 + 16  +  8  + ( 16  + 4  + 16  + 6  +  8 ) * (TotEnc-1) +  16  + 12
'             = 144 + 50*(TotEnc-1)
                        org     0
                                                                                                
Update                  test    Pin, #$20               wc      'Test for upper or lower port
                        muxc    :PinSrc, #%1                    'Adjust :PinSrc instruction for proper port
                        mov     IPosAddr, #IntPos               'Clear all internal encoder position values
                        movd    :IClear, IPosAddr               '  set starting internal pointer
                        mov     Idx, TotEnc                     '  for all encoders...  
        :IClear         mov     0, #0                           '  clear internal memory
                        add     IPosAddr, #1                    '  increment pointer
                        movd    :IClear, IPosAddr               
                        djnz    Idx, #:IClear                   '  loop for each encoder
                                                                
                        mov     St2, ina                        'Take first sample of encoder pins
                        shr     St2, Pin                
:Sample                 mov     IPosAddr, #IntPos               'Reset encoder position buffer addresses
                        movd    :IPos+0, IPosAddr                               
                        movd    :IPos+1, IPosAddr
                        mov     MPosAddr, PAR
                        Mov     MSpeedAddr,MPosAddr
                        Add     MSpeedAddr,#8                          
                        mov     St1, St2                        'Calc 2-bit signed offsets (St1 = B1:A1)
                        mov     T1,  St2                        '                           T1  = B1:A1 
                        shl     T1, #1                          '                           T1  = A1:x 
        :PinSrc         mov     St2, inb                        '  Sample encoders         (St2 = B2:A2 left shifted by first encoder offset)
                        shr     St2, Pin                        '  Adj for first encoder   (St2 = B2:A2)
                        xor     St1, St2                        '          St1  =              B1^B2:A1^A2
                        xor     T1, St2                         '          T1   =              A1^B2:x
                        and     T1, BMask                       '          T1   =              A1^B2:0
                        or      T1, AMask                       '          T1   =              A1^B2:1
                        mov     T2, St1                         '          T2   =              B1^B2:A1^A2
                        and     T2, AMask                       '          T2   =                  0:A1^A2
                        and     St1, BMask                      '          St1  =              B1^B2:0
                        shr     St1, #1                         '          St1  =                  0:B1^B2
                        xor     T2, St1                         '          T2   =                  0:A1^A2^B1^B2
                        mov     St1, T2                         '          St1  =                  0:A1^B2^B1^A2
                        shl     St1, #1                         '          St1  =        A1^B2^B1^A2:0
                        or      St1, T2                         '          St1  =        A1^B2^B1^A2:A1^B2^B1^A2
                        and     St1, T1                         '          St1  =  A1^B2^B1^A2&A1^B2:A1^B2^B1^A2
                        mov     Idx, TotEnc                     'For all encoders...
:UpdatePos              ror     St1, #2                         'Rotate current bit pair into 31:30
                        mov     Diff, St1                       'Convert 2-bit signed to 32-bit signed Diff
                        sar     Diff, #30
        :IPos           add     0, Diff                         'Add to encoder position value
                        wrlong  0, MPosAddr                     'Write new position to main memory
                        add     IPosAddr, #1                    'Increment encoder position addresses
                        movd    :IPos+0, IPosAddr
                        movd    :IPos+1, IPosAddr
                        add     MPosAddr, #4                            
:Next                   djnz    Idx, #:UpdatePos                ' my code from next line -------------------------------
                        mov     after, cnt                      ' load curent CNT into after
                        sub    after, before                    ' calculate CNT difference between sample 
                        mov     before , cnt                    ' load CNT for next calculate
                        add     time,after                      ' add CNT difference to 
                        Cmp     Delay, time    Wc               ' compare with  Delay to 
                       if_NC jmp     #:Sample                   ' if   time not reach delay do another sample
                       Mov     Restlt,IntPos                    ' load current position counter to Result
                       Sub     Restlt, PreSped                  '  calculate position counter different between delay
                       wrlong  Restlt, MSpeedAddr               ' Write Result to hub address
                       Mov      time, #0                        ' rest time back to 0
                       Mov     PreSped,IntPos                   ' load current position counter for next calculate 
                        jmp     #:Sample                        'Loop forever
'Define Encoder Reading Cog's constants/variables
AMask                   long    $55555555                       'A bit mask
BMask                   long    $AAAAAAAA                       'B bit mask
MSB                     long    $80000000                       'MSB mask for current bit pair
Delay                   long     76_000
Pin                     long    0                               'First pin connected to first encoder
TotEnc                  long    0                               'Total number of encoders
before                  res     1                               ' 
after                   res     1
PreSped                 res     1
Restlt                res     1 
time                    res     1  
Idx                     res     1                               'Encoder index
St1                     res     1                               'Previous state
St2                     res     1                               'Current state
T1                      res     1                               'Temp 1
T2                      res     1                               'Temp 2
Diff                    res     1                               'Difference, ie: -1, 0 or +1
IPosAddr                res     1                               'Address of current encoder position counter (Internal Memory)
MPosAddr                res     1                               'Address of current encoder position counter (Main Memory)
MSpeedAddr              res     1                               'Address of current encoder position counter(Speed) /ms (Main Memory)
IntPos                  res     16                              'Internal encoder position counter buffer)

Kye · 2010-07-19 15:53

Add a low pass filter.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Nyamekye,

John Abshier · 2010-07-19 16:18

Perhaps the code is fine. Why would one believe that the speed is 20 per millisecond every millisecond?

John Abshier

Julian800 · 2010-07-19 22:26

Hi Kye, I check the wave on scopemeter is fine. there is no change during reading.

Hi John, I attached encoder onto a stepper with motion controller and run tesing at 20000 microstep per second(encoder and stepper are both 2000 per revolution)

Is that something with timing issue( delay time or accumulate time)? or somthing else?

Post Edited (Julian800) : 7/19/2010 11:20:39 PM GMT

John Abshier · 2010-07-19 23:26

Just because you tell the motion controller to run 20000 microstep per second doesn't prove to me that the motion controller has 0 error. I get very supicious of any mechanical/electromechanical thing working with 0 error or variability.

>I should use 80_000, but from testig 76_000 give me result close to actual speed. I use 80Mhz system clock. Do you know why you have to use 76000? That is 5% from 80000.

Two points that may ring a bell: The reading is never over 20 and the 19s do not appear in a periodic manner.

John Abshier

Graham Stabler · 2010-07-20 00:05

I cannot see any problems with your code but it seems wrong that you should need to use 76000 as your value, you have a scope so it should be easy to check the pulse frequency to confirm.

Of course the resolution of your measurement is actually the time it takes to complete the loop but this is not very long really, only 40 instructions or so. The jitter you see it not that surprising, if you trigger the start of your counting with a pulse from the encoder then this will probably stop. Why does it happen? If your pulses are short and 1ms only just encloses 20 of the pulses then the 1ms period only has to occur just after a pulse to get a reading of 19. It also explains why you never see 21.

Graham

Julian800 · 2010-07-20 22:51

Hi John,
I checked the encoder signal on scope meter and that is 20,000 hz .

For using 76_000, I am not too clear with how to calculate time of assembly code. But I think the reason is some code is not count into time accumulation. Such as Mov CNT to register and calculate position counter difference between delay. That is why I am asking for help for your guys. Maybe someone with more experience with assembly can give me some suggestion. thanks John for your help. your suggestion let me thing more futher into my problem.

Hi Graham,

I also thought about the problem is jitter but that is not reading like 19 and following by a 21. I also see 21 in my speed reading in my post(but only once or twice in 100 ).
I think there is some thing with timing of my code, but I just can't fighure it out.

Graham Stabler · 2010-07-21 09:40

Julian,

Each assembly instruction takes four clock cycles apart from writes and reads to hub ram which take a bit longer. You can work out from that your expected resolution.

Graham

Need Help ASM problem with Quadrature Encoder with speed

Comments