Cordic trig assembly code for PIC18f

[ScaleRobotics]

I thought we had this working already.

Thanks Darrel! Yes, we had the cordic working for pic16 chips (can be found here http://www.picbasic.co.uk/forum/show...6307#post46307 ), but this one is more better. I feel the need for speed, and this one performs a hypotenuse length routine while it does the atan function. Also a bit more precise than the other one.

I tried commenting out the banksel, and putting all the variables for the function in bank0, but I am still getting unexpected results.

Thanks a lot for the help.

Walter

[Darrel Taylor]

OK, next problem ...

There's an issue with the indirect addressing.
I don't use C, but I think it uses a "stack" structure to pass variables back and forth to functions.

All the POSTINC's and PLUSW's etc, are moving things around to unknown locations because PBP doesn't use a stack.

It's a little hard to follow (since I don't know how C works), so I thought I'd let you work on it too. Maybe you'll see something I don't (yet).

If I figure out what it's doing with the FSR's I'll let you know.
<br>

[Darrel Taylor]

Oh! It wasn't as hard as I thought it would be.

And yup, it's trying to pull the parameters off the stack, and would be overwriting the variables with random values from RAM.
I assume you've set those variables before calling the routine.

Commenting these should help...

  ; Set up the stack
;  movff FSR2L, POSTINC1
;  movff FSR1L, FSR2L


  ; Initialize _ang to passed parameter
;  movlw 0xfd
;  movff PLUSW2, _ang
;  movlw 0xfe
;  movff PLUSW2, _ang+1<hr>; Calculates the magnitude and direction of the given ordered pair
atan2_sqrt:

  ; Set up the stack
;  movff FSR2L, POSTINC1
;  movff FSR1L, FSR2L

  ; Initialize _x to passed parameter
;  movlw 0xfb
;  movff PLUSW2, _x
;  movlw 0xfc
;  movff PLUSW2, _x+1
;  movff POSTINC2, _x
;  movff POSTDEC2, _x+1

  ; Initialize _y to passed parameter
;  movlw 0xfd
;  movff PLUSW2, _y
;  movlw 0xfe
;  movff PLUSW2, _y+1
;  movlw 0x03
;  movff PLUSW2, _y+1
;  movlw 0x02
;  movff PLUSW2, _y

ADDED: OOPs, missed a couple ...

  ; Restore the stack to its previous state
;  movf POSTDEC1
;  movff INDF1, FSR2L

and there's another banksel atans in the as_loop_start: part.

[Darrel Taylor]

And then there's another problem. Or actually the same problem.

Those banksel atans ...

Somehow in C, the atans table that's in Flash by the DW statements, gets copied to RAM. Then it uses FSR0 to read the array. But it's strange that it uses the same label.

I think you'll have to create a WORD array in PBP and load it with the values from the atans data.
Either read the data or just have a few atans(0) = xxxx statements.

If the array's in BANK0 then you still won't need the banksel statements.

Mañana

[ScaleRobotics]

Thanks for all your work on this Darrel. I used all your suggestions, and the hypotenuse seems to work, but the angle and sin, cos do not. It seems to be doing something fishy because it is taking a few seconds to give me a result. Here is what I have done. If there are no glaring errors, perhaps I should try converting the cordic code that worked for the PIC16 to PIC18 code? This sounds like a very complex issue with the way the PIC18 is handling the addresses, so I very much appreciate the time you have spent on it.

define OSC 20

include "TRIG.inc"

main:
define       LED     PortD.3

'LCD defines taken out so that this code would fit in limited size 

ADCON1=15

TRISA=%00000011                         ' Set PORTA  
TRISB=%00000000                         ' Set PortB
TRISC=%00000000  
TRISD=%00000100  
'       Vdd     5 volts
'       Vss     Ground
'       Vo      20K potentiometer (or ground)
'       DB0-3   No connect

ang=5461
x=0
y=0

asm
    call sin_cos
endasm
    
Lcdout $fe, 1   ' Clear LCD screen
'toggle portd.3
        Pause 500       ' Wait for LCD to startup
lcdout $FE,1,#x,",",#y
lcdout $FE,$C0,#ang

end

'trig.inc here_________________________________________________
i       var byte     BANK0
j       Var byte     BANK0
quad    var byte     BANK0
x       var word     bank0
y       var word     bank0
ang     var word     bank0
dy      var word     bank0
dx      var word     bank0
atans   var word[14] bank0

atans(0) = 16384
atans(1) = 9672
atans(2) = 5110
atans(3) = 2594
atans(4) = 1302
atans(5) = 652
atans(6) = 326
atans(7) = 163
atans(8) = 81
atans(9) = 41
atans(10) = 20
atans(11) = 10
atans(12) = 5
atans(13) = 3
atans(14) = 1



goto main

asm
    ;IDATA

; Table of arctan values

;atans   DW D'16384', D'9672', D'5110', D'2594', D'1302', D'652', D'326', D'163'
;        DW D'81', D'41', D'20', D'10', D'5', D'3', D'1'

    ;CODE

; Calculates the sine and cosine of the given angle
sin_cos:

  ; Set up the stack
  ;movff FSR2L, POSTINC1
  ;movff FSR1L, FSR2L

  ; Initialize _x to 18218
  movlw 0x2a
  movwf _x
  movlw 0x47
  movwf _x+1

  ; Initialize _y to 0
  clrf _y
  clrf _y+1

  ; Initialize _ang to passed parameter
  ;movlw 0xfd
  ;movff PLUSW2, _ang
  ;movlw 0xfe
  ;movff PLUSW2, _ang+1

  ; Initialize _quad to 0
  clrf _quad

  ; Check if the angle is greater than 16383 (90°)
sc_check_greaterthan:
  btfss _ang+1, 7
  btfss _ang+1, 6
  bra sc_check_lessthan
  bra sc_adjust_quad2
  
  ; Check if the angle is less than -16384 (-90°)
sc_check_lessthan:
  btfsc _ang+1, 7
  btfsc _ang+1, 6
  bra sc_setup_end

  ; If the angle is in quadrant 3, adjust it to quadrant 4
sc_adjust_quad3:
  negf _ang
  bc sc_negate_quad3
  comf _ang+1
  bra sc_adjust_end

  ; If the low byte negation causes a carry, negate the upper byte
sc_negate_quad3:
  negf _ang+1
  bra sc_adjust_end

  ; If the angle is in quadrant 2, adjust it to quadrant 1
sc_adjust_quad2:
  comf _ang
  comf _ang+1

  ; Toggle the sign bit and set the '_quad' flag
sc_adjust_end:
  btg _ang+1, 7
  setf _quad

  ; Multiply the angle by 2 to get better resolution
sc_setup_end:
  bcf STATUS, 0
  rlcf _ang
  rlcf _ang+1

  ; Set up the main loop
sc_loop_start:
  clrf _i
  ;CHK?RP _atans    ;was banksel _atans
  lfsr FSR0, _atans

    ; The main loop label
sc_loop:
    movff _x, _dy
    movff _x+1, _dy+1
    movff _i, _j
    movf _j
    bz sc_bs_x_done

      ; Loop to shift _dy right
sc_bs_x_loop:
      bcf STATUS, 0
      rrcf _dy+1
      rrcf _dy
      btfsc _x+1, 7
      bsf _dy+1, 7
      decfsz _j
      bra sc_bs_x_loop

    ; Calculate what needs to be added to _x
sc_bs_x_done:
    movff _y, _dx
    movff _y+1, _dx+1
    movff _i, _j
    movf _j
    bz sc_do_rotation

      ; Loop to shift _dx right
sc_bs_y_loop:
      bcf STATUS, 0
      rrcf _dx+1
      rrcf _dx
      btfsc _y+1, 7
      bsf _dx+1, 7
      decfsz _j
      bra sc_bs_y_loop

    ; Perform adding operations on _x, _y and _ang
sc_do_rotation:
    btfss _ang+1, 7
    bra sc_sub_angle

    ; If _ang is negative
    movf POSTINC0, W
    addwf _ang
    movf POSTINC0, W
    addwfc _ang+1
    movf _dx, W
    addwf _x
    movf _dx+1, W
    addwfc _x+1
    movf _dy, W
    subwf _y
    movf _dy+1, W
    subwfb _y+1
    bra sc_loop_bottom

    ; If _ang is positive
sc_sub_angle:
    movf POSTINC0, W
    subwf _ang
    movf POSTINC0, W
    subwfb _ang+1
    movf _dx, W
    subwf _x
    movf _dx+1, W
    subwfb _x+1
    movf _dy, W
    addwf _y
    movf _dy+1, W
    addwfc _y+1

    ; Increment the counter and exit the loop if done
sc_loop_bottom:
    incf _i
    movlw 0x0f
    cpfseq _i
    bra sc_loop

  ; Negate _x if it was initially in quadrant 2 or 3
sc_finished:
  btfss _quad, 7
  bra sc_output
  negf _x
  bc sc_negate_x
  comf _x+1
  bra sc_output

  ; If the low byte negation causes a carry, negate the upper byte
sc_negate_x:
  negf _x+1

  ; Output the calculated _x and _y values
sc_output:
  ;movff _y, AARGB3
  ;movff _y+1, AARGB3+1
  ;movff _x, AARGB3+2
  ;movff _x+1, AARGB3+3

  ; Restore the stack to its previous state
  ;movf POSTDEC1
  ;movff INDF1, FSR2L

  return


; Calculates the magnitude and direction of the given ordered pair
atan2_sqrt:

  ; Set up the stack
  ;movff FSR2L, POSTINC1
  ;movff FSR1L, FSR2L

  ; Initialize _x to passed parameter
  ;movlw 0xfb
  ;movff PLUSW2, _x
  ;movlw 0xfc
  ;movff PLUSW2, _x+1
;  movff POSTINC2, _x
;  movff POSTDEC2, _x+1

  ; Initialize _y to passed parameter
  ;movlw 0xfd
  ;movff PLUSW2, _y
  ;movlw 0xfe
  ;movff PLUSW2, _y+1
;  movlw 0x03
;  movff PLUSW2, _y+1
;  movlw 0x02
;  movff PLUSW2, _y

  ; Initialize _ang to 0
  clrf _ang
  clrf _ang+1

  ; Initialize _quad to 0
  clrf _quad

  ; If the point is in quadrant 2 or 3, make _x positive and set flag
as_check_negative:
  btfss _x+1, 7
  bra as_shift_x
  setf _quad
  negf _x
  bc as_negate_x
  comf _x+1
  bra as_shift_x

  ; If the low byte negation causes a carry, negate the upper byte
as_negate_x:
  negf _x+1

  ; Divide the _x coordinate by 2 to prevent overflowing
as_shift_x:
  bcf STATUS, 0
  rrcf _x+1
  rrcf _x

  ; Divide the _y coordinate by 2 to prevent overflowing
as_shift_y:
  bcf STATUS, 0
  rrcf _y+1
  rrcf _y
  btfsc _y+1, 6
  bsf _y+1, 7

  ; Set up the main loop
as_loop_start:
  clrf _i
  ;CHK?RP _atans    ;was banksel _atans
  lfsr FSR0, _atans

    ; The main loop label
as_loop:
    movff _x, _dy
    movff _x+1, _dy+1
    movff _i, _j
    movf _j
    bz as_bs_x_done

      ; Loop to shift _dy right
as_bs_x_loop:
      bcf STATUS, 0
      rrcf _dy+1
      rrcf _dy
      btfsc _x+1, 7
      bsf _dy+1, 7
      decfsz _j
      bra as_bs_x_loop

    ; Calculate what needs to be added to _x
as_bs_x_done:
    movff _y, _dx
    movff _y+1, _dx+1
    movff _i, _j
    movf _j
    bz as_do_rotation

      ; Loop to shift _dx right
as_bs_y_loop:
      bcf STATUS, 0
      rrcf _dx+1
      rrcf _dx
      btfsc _y+1, 7
      bsf _dx+1, 7
      decfsz _j
      bra as_bs_y_loop

    ; Perform adding operations on _x, _y and _ang, shifting the _atans right one
as_do_rotation:
    movff POSTINC0, PRODL
    movff POSTINC0, PRODH
    bcf STATUS, 0
    rrcf PRODH
    rrcf PRODL
    btfsc  _y+1, 7
    bra as_sub_angle

    ; If _y is positive
    movf PRODL, W
    addwf _ang
    movf PRODH, W
    addwfc _ang+1
    movf _dx, W
    addwf _x
    movf _dx+1, W
    addwfc _x+1
    movf _dy, W
    subwf _y
    movf _dy+1, W
    subwfb _y+1
    bra as_loop_bottom

    ; If _y is negative
as_sub_angle:
    movf PRODL, W
    subwf _ang
    movf PRODH, W
    subwfb _ang+1
    movf _dx, W
    subwf _x
    movf _dx+1, W
    subwfb _x+1
    movf _dy, W
    addwf _y
    movf _dy+1, W
    addwfc _y+1

    ; Increment the counter and exit the loop if done
as_loop_bottom:
    incf _i
    movlw 0x0e
    cpfseq _i
    bra as_loop

  ; Multiply the _x value by 19898 and divide by 2^14 to scale it
as_scale_x:
  movff _x, _dx
  movff _x+1, _dx+1
  movlw 0xba
  mulwf _dx
  movff PRODH, _x
  movlw 0x4d
  mulwf _dx+1
  movff PRODH, _dy
  movff PRODL, _x+1
  movlw 0xba
  mulwf _dx+1
  movf PRODL, W
  addwf _x, F
  movf PRODH, W
  addwfc _x+1, F
  clrf WREG
  addwfc _dy, F
  movlw 0x4d
  mulwf _dx
  movf PRODL, W
  addwf _x, F
  movf PRODH, W
  addwfc _x+1, F
  clrf WREG
  addwfc _dy, F
  movlw 0x06
  movwf _j
as_scale_bs_loop:
    bcf STATUS, 0
    rrcf _dy
    rrcf _x+1
    rrcf _x
    decfsz _j
    bra as_scale_bs_loop

  ; Check if the quadrant was originally changed
as_check_quad:
  btfss _quad, 7
  bra as_output
  btfss _ang+1,7
  bra as_adjust_quad1

  ; If the angle is in quadrant 4, adjust it to quadrant 3
as_adjust_quad4:
  negf _ang
  bc as_negate_quad4
  comf _ang+1
  bra as_adjust_end

  ; If the low byte negation causes a carry, negate the upper byte
as_negate_quad4:
  negf _ang+1
  bra as_adjust_end

  ; If the angle is in quadrant 1, adjust it to quadrant 2
as_adjust_quad1:
  comf _ang
  comf _ang+1

  ; Toggle the sign bit
as_adjust_end:
  btg _ang+1, 7

  ; Output the calculated angle and hypotenuse values
as_output:
  ;movff _ang, AARGB3
  ;movff _ang+1, AARGB3+1
  ;movff _x, AARGB3+2
  ;movff _x+1, AARGB3+3

  ; Restore the stack to its previous state
  ;movf POSTDEC1
  ;movff INDF1, FSR2L

  return

endasm

[Darrel Taylor]

There are 15 elements in the array (0-14).

Might be overwriting another variable.
<br>

[ScaleRobotics]

Once again you saved the day!

IT WORKS! I will clean it up a little bit and repost, so others may enjoy it. Looks to be about the same speed as the original author posted. Around 9,000 times per second on a 48mhz chip.

[ScaleRobotics]

Hmmm,

Well, I can't test it from here, but this seems like it might work. (If not, I will think harder and do some testing). Thanks for the feedback Martin.

sincos:
    'use angle as deg.dd for example 35999 is 359.99 degrees
    if ang < 9001 then ang = 9000 - ang              'change degrees to radians
    if ang > 9000 then ang = 45000 - ang            'change degrees to crazy radians
    ang = ang * 466
    ang = div32 256

 asm
        call sin_cos
endasm
return

[ScaleRobotics]

I think I found my error in sincos. I had 36000 where 27000 now is. This did not compensate for the 90 degree shift for radians. Let me know if this works better.

sincos:
    'use angle as deg.dd for example 35999 is 359.99 degrees
    if ang < 9001 then ang = 9000 - ang              'change degrees to radians
    if ang > 9000 then ang = 27000 - (36000 - ang)   'change degrees to radians
    ang = ang * 466
    ang = div32 256
    
    asm
        call sin_cos
    endasm
return

[readitaloud]

This is getting closer. The magnitude and sign values for the sin(ang) are all good. The magnitude term for the cos(ang) is good except the sign ,msb is incorrect for quadrants 2,3 and 4. Here is a list of what I get FOR ang = 0 to 350 STEP 1000 ;



ang = 0 sin(ang) = 5 cos(ang) = 30000
ang = 1000 sin(ang) = 5209 cos(ang) = 29544
ang = 2000 sin(ang) = 10266 cos(ang) = 28188
ang = 3000 sin(ang) = 15004 cos(ang) = 25981
ang = 4000 sin(ang) = 19287 cos(ang) = 22981
ang = 5000 sin(ang) = 22983 cos(ang) = 19285
ang = 6000 sin(ang) = 25983 cos(ang) = 14998
ang = 7000 sin(ang) = 28188 cos(ang) = 10264
ang = 8000 sin(ang) = 29544 cos(ang) = 5207
ang = 9000 sin(ang) = 30000 cos(ang) = 3
ang = 10000 sin(ang) = 29544 cos(ang) = 5207
ang = 11000 sin(ang) = 28188 cos(ang) = 10264
ang = 12000 sin(ang) = 25983 cos(ang) = 14998
ang = 13000 sin(ang) = 22983 cos(ang) = 19285
ang = 14000 sin(ang) = 19287 cos(ang) = 22981
ang = 15000 sin(ang) = 15004 cos(ang) = 25981
ang = 16000 sin(ang) = 10266 cos(ang) = 28188
ang = 17000 sin(ang) = 5209 cos(ang) = 29544
ang = 18000 sin(ang) = 5 cos(ang) = 30000
ang = 19000 sin(ang) = -5207 cos(ang) = 29544
ang = 20000 sin(ang) = -10260 cos(ang) = 28190
ang = 21000 sin(ang) = -14998 cos(ang) = 25983
ang = 22000 sin(ang) = -19283 cos(ang) = 22985
ang = 23000 sin(ang) = -22981 cos(ang) = 19287
ang = 24000 sin(ang) = -25981 cos(ang) = 15004
ang = 25000 sin(ang) = -28188 cos(ang) = 10266
ang = 26000 sin(ang) = -29544 cos(ang) = 5213
ang = 27000 sin(ang) = -30000 cos(ang) = 5
ang = 28000 sin(ang) = -29549 cos(ang) = -5195
ang = 29000 sin(ang) = -28199 cos(ang) = -10258
ang = 30000 sin(ang) = -25986 cos(ang) = -14992
ang = 31000 sin(ang) = -22987 cos(ang) = -19275
ang = 32000 sin(ang) = -19296 cos(ang) = -22976
ang = 33000 sin(ang) = -15013 cos(ang) = -25977
ang = 34000 sin(ang) = -10271 cos(ang) = -28184
ang = 35000 sin(ang) = -5216 cos(ang) = -29544


- Martin

[ScaleRobotics]

Thanks Martin,

I'll take a look at it tonight. It should be easy enough to switch them around. With the pic16 solution, you have to work in quadrants. So that's probably where my error is.

[ScaleRobotics]

Hi Chris,

Well, I might have to admit that the cordic may not be the best solution for a single axis tilt sensor. It is a natural for a dual or triple axis sensor. If you just have a single axis sensor, I might suggest a lookup table. Now I'm not really good at converting mathematical equations back and forth, so I can't say for sure if you are on the right track with the one you have. It doesn't resemble the ones I have used enough for me to tell. But I can show you some that I think would work. And, perhaps yours would work too. I just can't tell from here.

Here is the app note I worked with before. As you can see, the arctan equation "theta = arctan(Ax/Az)" on page 4 is a pretty cool thing. Of course, this one requires a two axis sensor, but it keeps resolution up to about 1.3 degrees. It also fits in pretty well with are atan2 cordic.
Attachment 4533

Now for a single axis sensor, your resolution at near 90 degree angles goes down to about 9 degree accuracy. This seems to lend itself better to just a lookup table. But, if you really wanted to use the cordic, you could.

For a single axis sensor, one equation for determining angle is arcsin (x) where x is acceleration in g's . Our cordic, can't do arcsin, but it can do arctan. We can use the following equation from Wikipedia:




and solve for theta.

atan(y) = atan2(y,1)

But, in our case, we are feeding y with a value which at its peak, = 1g. So, whatever value your sensor gives for 1g, you should fill in for the "1" value. y will equal 2 arctan x/((1+sqr(1-x^2)) , then run it through the cordic. I just feel if its going to be off by up to 9 degrees (because we are using a single sensor) why not just use a lookup table.