View Full Version : Help!!! how to add speed in Quadrature Encoder Object

Julian800

05-24-2010, 01:48 PM

Hi All,

· I am using Jeff Martin's Quadrature Encoder v1.0 object for read encoder. I want to do some speed synchronize. If I read and change speed in Spin, that will result some position lost. I want to add speed value in Quadrature Encoder object. but I am not good at assemblyhttp://forums.parallax.com/images/smilies/shakehead.gif·.·I need some help !!

·Thanks!

Post Edited (Julian800) : 5/25/2010 11:52:21 PM GMT

Use the dual quadature encoder object in the obex.

It gives you the raw tick speed.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Nyamekye,

Julian800

05-25-2010, 07:18 AM

Hi Kye,

that is what I want, but quadature encoder object is not working for me( I get encoder count lost).

I don't know is that too hard to add speed in Quadrature Encoder object? anyone can give me some suggestion? I am try to understand assembly code and add speed(current tick speed per second)

Just capture the difference in the CNT value from one tick to another, as in the time elasped, and then divide the clkfreq by that.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Nyamekye,

Julian800

05-25-2010, 11:03 AM

I know this is part of code sample encoder count. but I don't know where to add read system counter CNT and calculate speed. HELP!!

:Sample················ mov···· IPosAddr, #IntPos·············· 'Reset encoder position buffer addresses

······················· movd··· :IPos+0, IPosAddr······························

······················· movd··· :IPos+1, IPosAddr

······················· mov···· MPosAddr, PAR··························

······················· 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··············· 'Loop for each encoder

······················· jmp···· #:Sample······················· 'Loop forever

Post Edited (Julian800) : 5/25/2010 4:08:40 AM GMT

Mmm, maybe another member can help. I am not familar with the parallax driver assembly.

Sorry,

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Nyamekye,

Julian800

05-31-2010, 09:06 AM

Kye said...

Just capture the difference in the CNT value from one tick to another, as in the time elasped, and then divide the clkfreq by that.

I did something similar·to work like what you said.· I run a cog with assembly to read·encoder count·with fix delay time and then·calculate encoder count per second.

But I get error to read a fix speed. My reading is speed +1 or -1 different with real speed at delay time as·one second. When I· use 1ms (use 1sec/1024) as delay, the error is enlarge by shift left·10 . Can anyone suggest what is the problem?· Or is there any other way to read speed of encoder?· Thanks!··

DAT·· ' get encoder speed

·········· org 0

AsmCode rdlong Delay, #0··· 'Get clock frequency

······· shr Delay, #10············ 'Divide by 1024

······· Mov MPosAddr, PAR····· ' save Postion arry address start

······· Mov SPDAddr, MPosAddr

······· add· SPDAddr, #12···· ' save speed address

······· rdlong· P1,· MPosAddr

·Loop·· mov Time, cnt 'Get current time

······· add Time, Delay

·······

·······

······· '<more code here> 'Perform operation

······· rdlong· P2,· MPosAddr

······· sub···· P2,· P1

·······

······· shl···· P2,· #10

······· wrlong· P2, SPDAddr

······· rdlong· P1,· MPosAddr

······· waitcnt Time, Delay 'Wait for 1/1024 second

······· jmp #Loop 'loop back

Speed·················· long··· 0

Delay·················· long··· 1

P1····················· res···· 1

P2····················· res···· 1

Time··················· res···· 1

MPosAddr··············· res···· 1

SPDAddr················ res···· 1·

JonnyMac

05-31-2010, 09:27 AM

The down-side of objects that are infinitely flexible is that they won't -- due to all the code required for that flexibility -- run as fast as something optimized for your application. I wrote about encoders in my column; my driver seems to work well, and the code should be easy to modify if you need.

www.parallax.com/Portals/0/Downloads/docs/cols/nv/prop/col/nvp6.pdf (http://www.parallax.com/Portals/0/Downloads/docs/cols/nv/prop/col/nvp6.pdf)

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Jon McPhalen

Hollywood, CA

JonnyMac

05-31-2010, 09:29 AM

Just a follow-up after looking at that object: if you think you're having a speed problem based on sending data to a serial display, it could be that you're overrunning your serial buffer -- I went through this when testing my own encoder driver.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Jon McPhalen

Hollywood, CA

Julian800

06-01-2010, 07:45 AM

Hi JonnyMac

thanks for your help. I don't have problem to read the encoder count, the problem is how to sample speed of encoder. I am doing something like speed lock for stepper. I need to read accurate speed value and update every 1 ms.

Now I run my assembly code to sample a fix speed , but I can't get stable reading that always 1 count less or more. Is that result by using one cog for encoder read then write to hub memory , the other cog read hub memory and do sample speed(as show above)? Anyone have experience with encoder speed control? Please give me some suggestion. Thanks

Hi pussycat,

I am working with the same Jeff Martin's Quadrature Encoder v1.0 object, and was curious if you had made progress?

If you don't have it yet get Harprits' book Programming the Propeller with Spin, it just got published. It has some good stuff on motors and encoders. Although it is based on Spin and not PASM, it may still give you some good ideas.

Post Edited (rpdb) : 6/12/2010 5:27:11 AM GMT

Julian800

06-17-2010, 08:16 PM

I realize that previous way· of using ·other cog read hub memory count value and calculating speed may missing count during reading time.·· Now I am trying to add speed calculation into encoder object.· ·I only use· Jeff’s Quadrature Encoder v1.0 ·monitor one encoder . I add up loop time( system cnt) ·and compare with fix delay (e.g. 1ms), go into delay record current encoder count and subtract with previous count. the difference should be ·speed (count/ms),···· But I can’t get my code working .The result is not accurate (I can get 16 out of 20 accurate , then some with +or –one count).

I also try to use·following two line of code to·sum up speed and compare with encoder count. I get back with some value -9000130000. Is there anything I did wrong?

·················· ··'· add····· totalRes, CurCount

·····················'· wrlong·· after, TotalcuAddr

my code is

······················· 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

······················· Mov···· totalRes,· #0·············

: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···· TotalcuAddr,MPosAddr

······················· Add···· TotalcuAddr,#12

··················································

······················· 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

speedCode

······················ mov···· after, cnt······················· ' read current cnt

······················· subs··· after, before·················· ' caculate·loop time

······················· mov···· before , cnt····················' read cnt for next caculation

······················· add···· time,after

······················· Cmp···· Delay, time··· Wc

······················ if_NC jmp···· #:Sample

······················ Mov···· CurCount,IntPos·············

······················ Sub···· CurCount, PreCount·

······················ wrlong· CurCount, MSpeedAddr

·····················'· add····· totalRes, CurCount

·····················'· wrlong·· after, TotalcuAddr

······················ Mov···· PreCount,IntPos

······················ Mov····· time, #0·

······················· jmp···· #:Sample

'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···· 79_954

Pin···················· long··· 0······························ 'First pin connected to first encoder

TotEnc················· long··· 0

TotalcuAddr············ res···· 1

totalRes··············· res···· 1······························ 'Total number of encoders

before················· res···· 1

after·················· res···· 1

PreCount··············· res···· 1

CurCount··············· 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· encoder speed (Main Memory)

IntPos················· res···· 16····························· 'Internal encoder position counter buffer

FIT 496

John Abshier

06-17-2010, 09:28 PM

Look at the PUB ReadDelta(EncID) method. It is one line of nearly unreadable Spin. In my robot control programs I have a main loop that executes at a fixed rate (20 - 50 Hz). I call ReadDelta. Since the loop is a constant time a call to ReadDelta gives a speed of encoder ticks per delta time. A constant multiply or divide will convert to physical units (meters/sec, mph, inches/sec, etc.).

John Abshier

Julian800

06-25-2010, 07:34 AM

John Abshier said...

Look at the PUB ReadDelta(EncID) method. It is one line of nearly unreadable Spin. In my robot control programs I have a main loop that executes at a fixed rate (20 - 50 Hz). I call ReadDelta. Since the loop is a constant time a call to ReadDelta gives a speed of encoder ticks per delta time. A constant multiply or divide will convert to physical units (meters/sec, mph, inches/sec, etc.).

John Abshier

I agree with that line of ReadDelta is real hard to understand. Is that possible to put that function into assembly part?

John Abshier

06-25-2010, 07:58 AM

In your main program can you not do the following fast enough?

NewEncoder := Pos ' make a copy of encoder count

Speed := NewEncoder - OLdEncoder ' calculate speed in encoder ticks per cycle time (1ms?)

OldEncoder := NewEncoder ' save encoder reading

I think he reason you got the large negative number by adding up encoder counts is that they turn negative after 2^31.

John Abshier

Beau Schwabe (Parallax)

06-25-2010, 09:33 PM

I have not looked at the code in detail and my idea is just a theory, but it might be applicable in a situation such as this or similar.

Assuming that the pulses are required to be read in a symmetrical manor, i.e. equal or balanced amount of time on vs. time off .... I think it would almost have to be for proper operation.

...anyway, this would require running and syncing two COGs in such a way that the sampling between them is exactly 180 Deg out of phase. Once established, then by adding the result of the two COGs together would effectively be the same as doubling your resolution.

If however the object as it is uses both edges of the quadrature signal to increment the counter then the two cogs mentioned above would need to be 90 Deg out of phase. Even further, if the object uses any edge transition to increment the counter (using a truth table to determine which direction to increment the counter), then the two cogs mentioned above would need to be 45 Deg out of phase.

Like I said though, I haven't looked at the code in detail, and it may already be so tight that a 45 deg or any deg of phase shift may not be possible. Just thought I would throw the idea out there since I know that the technique can be applied elsewhere on similar types of measurements.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Beau Schwabe (mailto:bschwabe@parallax.com)

IC Layout Engineer

Parallax, Inc.

'not yet tested

con

ticks_per_rev = 36

OBJ

Encoder : "QuadratureEncoder"

Var

long revolutions 'Global var to pass speed in rpm, rps etc

PUB Speed | old_speed, current_speed, T, dT

'Start the QuadratureEncoder here if not started elsewhere

dT := clkfreq/50 'setup for accurate time base of 20mS

repeat

T := cnt

waitcnt(T += dT) 'wait here to execute once every 20ms

current_speed := Encoder.ReadDelta(0) 'ticks per 20mS

revolutions := ((current_speed - old_speed) / ticks_per_rev) 'divide this amount by your encoder resolution

'multiply by 50 to get rps or 3000 to get rpm

old_speed := current_speed 'save for next iteration

I am working on this and have not yet had a chance to test.

Post Edited (rpdb) : 6/25/2010 10:51:46 PM GMT