Calc theta, x, y from diff. drive encoders -- what am I doing wrong?

Zoot · 2008-08-30 22:37

Hey all -- I'm reading encoder info into a BS2p40. I'm pretty sure I've got my math right, but I'm getting garbage (values all over the place) on my theta.

The encoders are delivering accurate ticks info. See relevant code for comments. In the case of figuring theta (rads to brads) I'm working fixed point (256.256). If I get this working right, I may use some of Tracy Allen's long division/mult. to get better resolution, but as it stands, where I might expect to get 0 change in theta sometimes because of the resolution, I get swings all over the place. The raw dist (distL + distR/2) and in a straight line it's pretty much dead nuts on. The posx and posy are off because the theta jumps all over.

Note that the encoder deliver the signed word *change* since the host last requested a sample; the actual range is about -40 to 40 each sample MAX, with average being around -15 to 15).

Any suggestions, comments, mistakes pointed out, are welcome.


ioWord                VAR     Word 


buffer0 VAR Word
buff0   VAR buffer0.LOWBYTE
buff1   VAR buffer0.HIGHBYTE
buffer1 VAR Word
buff2   VAR buffer1.LOWBYTE
buff3   VAR buffer1.HIGHBYTE

buffer2 VAR Word
buff4   VAR buffer2.LOWBYTE
buff5   VAR buffer2.HIGHBYTE

buffer3 VAR Word
buff6   VAR buffer3.LOWBYTE
buff7   VAR buffer3.HIGHBYTE

buffer4 VAR Word
buff8   VAR buffer4.LOWBYTE
buff9   VAR buffer4.HIGHBYTE


motos  VAR Word
motoL  VAR motos.LOWBYTE
motoR  VAR motos.HIGHBYTE
moto   VAR motoL

accL   VAR Byte
accR   VAR Byte

ioByte VAR Byte
SPaddr VAR Byte

i      VAR Nib

sign VAR Bit
sign2 VAR Bit


WHEEL_BASE_CM        CON 296' in 10th cms
'WHEEL_BASE           CON WHEEL_BASE_CM * CMtoTicks_HM >> 16 + 1
WHEEL_BASE           CON 114 ' in ticks   113.664
WHEEL_BASE_HALF      CON WHEEL_BASE + 1 >> 1 ' FOR ROUNDing

'WHEEL_BASE_HM   CON $10000 + WHEEL_BASE_HALF / WHEEL_BASE  '431.651315789474
' 1 / wheel_base * $10000 = 576.576576576577 with **

WHEEL_BASE_HM        CON 576' for using ** instead of / WHEEL_BASE

'convert to brads 40.7436654315252 '128/PI 256/TWOPI '6.283185307
RadsToBrads_M        CON 41    ' 256/2PI = with *
RadsToBrads_HM       CON 48737 ' 256/2PI = 4.7436654315252 * $10000 = 48,736.8577204355 with ** + hi
RadsToBrads_Hi       CON 04    ' equiv to -- 256/2PI -- x ** .74 + ( x * 4 )
RadsToBrads_Hi256    CON RadsToBrads_Hi * 256 ' but we're working fixed decimal place in 256ths

' / base * toBrads = 0.358457079035800253378378378378 * $10000 with **    23491.843131690205405405405405381
DivWBtoBrads_HM CON 23492



'--------------------------------------

'preloads signed encoder ticks change since last sample
'buffer0 -- signed Word -- L ticks change since last reading
'buffer1 -- signed Word -- R ticks change since last reading
    GOSUB Get_Odometry

 

' dist = (left_dist + right_dist) / 2.0;

buffer2 = buffer0 + buffer1      ' add change since last reading
sign    = buffer2.BIT15          ' save sign
buffer2 = ABS buffer2 + 1 >> 1   ' /2 with rounding
buffer2 = -sign ^ buffer2 + sign ' restore sign

 

' theta += (r_dist - l_dist) / WHEEL_BASE;

buffer3 = buffer1 - buffer0      ' note that distances AND wheel base are in units of "ticks"
sign    = buffer3.BIT15          ' save sign
buffer3 = ABS buffer3 << 8       ' work in 256ths, i.e., fixed decimal point number from -127.255 to 127.255
buffer3 = buffer3 ** DivWBtoBrads_HM  ' equiv to below two statements, in one single high-multiply ...

'above equivalent to:
' ( ( r_dist - l_dist ) / wheelbase ) * ( 256/TWOPI )
   'buffer3 = buffer3 ** WHEEL_BASE_HM   '  / wheel_base
   'buffer3 = buffer3 ** RadsToBrads_HM + ( buffer3 * RadsToBrads_Hi256 ) 'convert rads to brads * 40.7436654315252 or 128/PI 256/TWOPI (6.283185307)

buffer3 = -sign ^ buffer3 + sign ' restore sign -- signed theta change in brads, fixed decimal point


GET theta, Word ioWord           ' add to running theta (256ths)
ioWord = ioWord + buffer3
PUT theta_LSB, ioWord.BYTE0
PUT theta_MSB, ioWord.BYTE1

ioByte = ioWord + 128 >> 8       ' round to whole brads for sine and cosine ops

 

' X_pos += inches * COS(theta);
' Y_pos += inches * SIN(theta);

sign = buffer2.BIT15             ' buffer2 is distance traveled since last sample from first steps
buffer2 = ABS buffer2

buffer0 = COS ioByte             ' x pos
sign2 = sign ^ buffer0.BIT15     ' save sign, XOR with dist sign
buffer0 = ABS buffer0 */ 516      '  normalize -- sin/127 = x/256 for use with */
buffer0 = buffer2 * buffer0  MAX $FF7F + 128 >> 8 ' equiv to  */ with rounding
buffer0 = -sign2 ^ buffer0 + sign2 ' restore sign


buffer1 = SIN ioByte             ' y pos cos
sign2 = sign ^ buffer1.BIT15     ' save sign, XOR with dist sign
buffer1 = ABS buffer1 << 1       ' * 2 = normalize -- sin/128 = x/256 for use with */
buffer1 = buffer2 * buffer1 + 128 >> 8 ' equiv to  */ with rounding
buffer1 = -sign2 ^ buffer1 + sign2 ' restore sign


GET posx, Word ioWord            ' add to running x position
ioWord = ioWord + buffer0
PUT posx_LSB, ioWord.BYTE0
PUT posx_MSB, ioWord.BYTE1


GET posy, Word ioWord            ' add to running y position
ioWord = ioWord + buffer1
PUT posy_LSB, ioWord.BYTE0
PUT posy_MSB, ioWord.BYTE1

'--------------------------------------

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php

Post Edited (Zoot) : 9/7/2008 2:26:10 PM GMT

Tracy Allen · 2008-08-31 15:17

Zoot,
Do you DEBUG "buffer3" in the steps it takes from the raw data to theta? That I think would help to reveal where it is blowing up. I don't see offhand.

How many ticks per revolution of the wheel?

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

Zoot · 2008-08-31 15:48

144 ticks per wheel. Each sampling from the encoder handling SX usually brings in a change of anywhere from 1-40 ticks or so (no more). I decided to just leave everything in common units of "ticks", the wheelbase, distance, etc. to keep things simple so everything does't need to be converted to cm. or some other common unit.

I am going to setup a step by step debug sometime over the weekend so I could watch each step in the calculations, but probably won't get to that till tomorrow night (trying to take a real weekend off!).

I get perfect data in (signed word representing change in ticks since last reading) and have tested that side of things extensively w/o error.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php

erco · 2008-08-31 16:06

Zoot: I don't have time to dig into your code right now, but I like your direction a lot. I'm a big fan of dead reckoning, and here are two interesting articles with good odometry·info, although the code is not for the Stamp. Enlightening nonetheless.

http://www.seattlerobotics.org/encoder/200010/dead_reckoning_article.html
http://www.seattlerobotics.org/encoder/200108/using_a_pid.html

erco

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
·"If you build it, they will come."

Zoot · 2008-09-04 17:52

FYI -- theta is working perfectly. I believe that my larger program had a bad DEBUG syntax (printing theta as signed word instead of unsigned byte.byte). I'm still getting some occasionally weird results on x/y position, but my basic code algorithm seems fine.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php

kevinb2050 · 2008-09-05 01:37

Hi I just got my encoders this week so i am new to this subject.
I am running continuous servos controlled by a PSC (servo controller) and trying to get my head around Phil Philgrims article "Applying the Boe-Bot Encoders"
Questions
1. are you using continuous rotating servos
2. If so are you pulsing them or are they running on a PSC (servo controller)

Zoot · 2008-09-05 01:57

1. no
2. no

I've got homebrew optical encoder discs on the main wheels themselves (144 ticks per revolution or about .26cm per tick). Homebrew electronics (Hammatsu emitter/detectors) hooked up to a "slave" SX. The SX drives 8 servos with ramps, handles encoder, velocity and odometry tasks, and sends output motor speeds to an MD22 motor controller (via I2C). A Stamp is the main host for the 'bot, running several peripherals, sensors, etc.

Phil's writeup is really good -- but keep in mind that rather than pure velocity control (independent of odometry) he took a really elegant approach of pulse management to keep the Boe-bot moving straight and turning accurately. Also his method of figuring a vector heading based on actual angular tick resolution is really cool.

I'm not going for that kind of precision -- I mostly need the velocity control and the odometry is a bonus. I don't neeed 1000s of meters of dead-reckoning, just a "good enough" idea of orientation (e.g. if the 'bot needs to turn 180 degrees, it will do so with a reasonable realiablity). I used the straight-up trig version of calculating theta (in radians) and x/y.

Here' a link to some of the hardware: http://forums.parallax.com/showthread.php?p=719074

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php

kevinb2050 · 2008-09-05 02:35

My problem at the moment is that I went with the PSC because 1-I ran out of I/O and 2-so I could do other things while the boe-bot was in motion -
However I expect now that with the encoders I need to monitor the clicks all the time and will not be able to go do other things in the program.
I played last night using debug to see how the encoders reacted and found that if I was away from the clicks to long I missed the count

Zoot · 2008-09-05 03:07

Yes, if you are using anything external to drive motors/servos, then you need a device that will sample the encoder ticks all the time, and report to the host. In this case the SX handles all that, freeing the Stamp up to do other things.

The beauty of PhiPi's writeup is that he uses the time *in-between* servo pulses to do everything else. Keep in mind that the Boe-bot is very slow (relatively) and with only 16 ticks per wheel this becomes doable.

You can use counter ICs, program an SX, use another Stamp, PWMPal has counters that would work also.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php

kevinb2050 · 2008-09-05 03:29

thanks zoot - I was considering a counter IC as I am running a BS2e and a BS2 onboard - the BS2 connected serialialy to the BS2e - the BS2 looks after a speech board and led display - but if I used the BS2 to do the counting during movements with the BS2e calling for counts when needed it may work - however I then will have a delay in serial comms which may effect the navigation.

Post Edited (kevinb2050) : 9/5/2008 3:49:50 AM GMT

Zoot · 2008-09-07 14:25

FYI -- this code is working phenomenally well with one change -- when going straight, I was getting 0 position change... why? I didn't account for 16 bit rollover in my rounding when normalizing and scaling position from theta and distance. Code has been revised above, but these are the relevant changes:

buffer0 = ABS buffer0 */ 516      '  normalize -- sin/127 = x/256 for use with */
buffer0 = buffer2 * buffer0  MAX $FF7F + 128 >> 8 ' equiv to  */ with rounding

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php

kevinb2050 · 2008-09-08 01:52

Just an update on my progress
Got the encoders working - connected direct to BS2e (no counting IC)
1- send command to the PSC to run servos until encoder count x
2- get sample from encoders
3- when reached preset count x send command to PSC to stop/change direction of servos
I have a delay in the code of Pause 20 and have not experienced any miss count
when I increase the pause to 40 I begin to miss encoder pulses

I expect that with continuous motion that I will only have a window of 20us to apply any calculation
if I don't use counting ICs. This should be enough as Phil's paper indicated as I only need to look for
any inputs from ir and ping sensors in this period.
So Far So Good - now to work out the calcs - God I hate Trig

Calc theta, x, y from diff. drive encoders -- what am I doing wrong?

Comments