Zoot

08-31-2008, 05:37 AM

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 (http://1uffakind.com/robots/povBitMapBuilder.php)

1uffakind.com/robots/resistorLadder.php (http://1uffakind.com/robots/resistorLadder.php)

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

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 (http://1uffakind.com/robots/povBitMapBuilder.php)

1uffakind.com/robots/resistorLadder.php (http://1uffakind.com/robots/resistorLadder.php)

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