Cordic trig assembly code for PIC18f

Thread: Cordic trig assembly code for PIC18f

1. Cordic trig assembly code for PIC18f

I have been trying to use cordic code that was originally written for a PIC18F device. I have made some changes to it to try to make it compatible with PicBasic Pro, but I am getting some unexpected results. I am compiling it to a PIC18F4620 device using 2.50b. It compiles without any errors, and prints the values to an LCD, but the results vary, and are unexpected. Any ideas would be greatly appreciated.

Here is the original code:
http://www.chiefdelphi.com/media/papers/2016

Here is my stab at making it compatible:
Code:
```;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
;http://www.chiefdelphi.com/media/papers/2016
; FILE NAME:        trig.asm
; AUTHOR:           Patrick Fairbank
;
; DESCRIPTION:      This file contains functions implementing the COORDIC
;                   algorithm.
;
;
; LICENSE:          Users are free to use, modify, and distribute this code
;                   as they see fit.
;
;    sin_cos: input ang, output x = sin and y = cos
;
;
;     atan2_sqrt: input x and y coordinates
;     Output the calculated angle and hypotenuse values
;     as output:    ang = angle x = hypotenuse
;
;******************************************************************************/

i       var byte
j       Var byte
x       var word
y       var word
ang     var word
dy      var word
dx      var word

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

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

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

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

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

; 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
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
movf POSTINC0, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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:
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

; 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
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
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
movf PRODH, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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
movf PRODH, W
clrf WREG
movlw 0x4d
mulwf _dx
movf PRODL, W
movf PRODH, W
clrf WREG
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
bra as_output
btfss _ang+1,7

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

; Toggle the sign bit
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
; Export the functions to the linker
'GLOBAL sin_cos
'GLOBAL atan2_sqrt

'END```
I am using this as an includes file, but I am getting values so far from expected, that I can't figure out what is going on.

Any help would be greatly appreciated.
Last edited by ScaleRobotics; - 6th February 2009 at 23:21.

2. Here are the changes I have made to the assembly code to try to get it to work with PicBasic:

commented out EXTERN AARGB3, UDATA_ACS, IDATA, CODE, edited variable descriptions so that variables are declared in PicBasic.

Original Code:
Code:
```  ; Import the address the compiler uses to store 32-bit return values
EXTERN AARGB3

UDATA_ACS

; Variable declarations
i       RES 1
j       RES 1
x       RES 2
y       RES 2
ang     RES 2
dy      RES 2
dx      RES 2

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```
I edited out where the result is moved to AARGB3, and just use the word variables _x, _y, and _ang to store the results. Commented out Globals and End. And of course added ASM and ENDASM.

My code:
Code:
```i       var byte
j       Var byte
x       var word
y       var word
ang     var word
dy      var word
dx      var word

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

-snip

; 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

-snip

; 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
; Export the functions to the linker
'GLOBAL sin_cos
'GLOBAL atan2_sqrt

'END```
Please let me know if you have any suggestions. It does compile without errors, but I do not get useful data out. When I input 5461 as the angle, I get the sin=29998, and cos=405. The result should be 15000, and 25980. But then if I run it again, a different value pops up for cos, that still does not get close to expected values.

Thanks,

Walter
Last edited by ScaleRobotics; - 8th February 2009 at 19:35. Reason: left things out, and forgot a [/code]

3. Hi Walter,

Here's one thing I see...
Code:
```  ; Set up the main loop
sc_loop_start:
clrf i
banksel atans
lfsr FSR0, atans```
banksel will put it in the proper bank for a variable in RAM.
But he's trying to use it with with a Label in FLASH.

I'm sure it was just a mistake on his part because the lfsr is correct.
And with his ASM code at the very beginning, it probably would have come up with bank 0 without him even knowing it.

However, now that it's in an include file for PBP, that atans table will be at a much higher address in Flash and you'll end up in a different bank than where the variables are.

Just comment it out.

I was only 1/3 of the way thru when I found that.
If it doesn't help, let me know, I'll keep looking for more.

P.S. You should specify BANK0 for all the variables used with this routine.
<br>
Last edited by Darrel Taylor; - 9th February 2009 at 06:50. Reason: BANK0 VARS

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
Last edited by ScaleRobotics; - 7th September 2011 at 16:44.

5. 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>

6. 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...
Code:
```  ; 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 ...
Code:
```  ; Restore the stack to its previous state
;  movf POSTDEC1
;  movff INDF1, FSR2L```
and there's another banksel atans in the as_loop_start: part.
Last edited by Darrel Taylor; - 9th February 2009 at 10:48. Reason: Missed some

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

8. 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.

Code:
```define OSC 20

include "TRIG.inc"

main:
define       LED     PortD.3

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

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

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

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

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

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

; 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
movf POSTINC0, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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:
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

; 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
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
movf PRODH, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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
movf PRODH, W
clrf WREG
movlw 0x4d
mulwf _dx
movf PRODL, W
movf PRODH, W
clrf WREG
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
bra as_output
btfss _ang+1,7

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

; Toggle the sign bit
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```
Last edited by ScaleRobotics; - 10th February 2009 at 17:36.

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

Might be overwriting another variable.
<br>
Last edited by Darrel Taylor; - 10th February 2009 at 18:15. Reason: .

10. 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.
Last edited by ScaleRobotics; - 10th February 2009 at 19:00.

11. IT WORKS!
SWEET!

You make it easy Walter.

I know you just got it working, so you probably don't know this yet ...

If I remember correctly, the last version could calculate a bearing within ~200 miles.

What's the new resolution?
<br>

12. Resolution is pretty good!

180 degrees is covered by 0 to 32,767.

So thats .00549 degrees per unit. Now I have not tested the total error, but I have seen it vary from expected by .015 degrees or so, but that is pretty close!

Sin and cos return between -30,000 and +30,000 for -1 and 1. One of the cool things about the cordic, is that it returns cos AND sin at the same time. I have heard that it is 90 times faster than math.h.

Also, this allows atan2 for a point x, y and accepts values from 0 to 32,767, and gives the hypotenuse to x,y which for me is the distance. It also gets these values at the same time!

This way my distance values should be spot on. I am still limited to about 200 miles between waypoints, the way I am measuring the change in x, and the change in y. But I could do a divide function to break the units into something it could handle. I just have not needed to do this for my application.

What I was using it for was navigation between waypoints. The first cordic for PIC16 did a pretty good job. It was accurate for bearing by about half a degree or so. My distance calculations did a simple square and square root that gave me errors on the hypotenuse. For long distances, I got pretty reliable results, less than 1 percent off of predicted results, but close range I could be off by a lot more. I could have cleaned this up with some higher math, but I was running out of speed trying to read two NMEA sentences, convert to decimal degrees for lat/lon, and fly a model plane, at 5 hertz gps readings. I gave up one sentence, and all is working well with the PIC16. This will allow me to break into the PIC18 world, and I am sure it will handle both NMEA sentences, plus a few more things. Nice!
Last edited by ScaleRobotics; - 11th February 2009 at 05:48.

13. Working base cordic trig code for sin cos tan atan atan2 and hypotenuse

Here is the base working code for the PIC18 cordic. It is 600 bytes, and performs atan2, hypotenuse, sin, cos, and tan. Times will change with different angles and coordinate entries, but here are a few stats from Codetimer.bas: http://www.picbasic.co.uk/forum/cont...te-XYZ-command

sincos: Simulaneous result of SIN and COS
Time: 185.65924 usec
OSC Freq: 48 Mhz
-------------------------

atan: ATAN2 result as well as hypotenuse
Time: 178.32620 usec
OSC Freq: 48 Mhz
-------------------------

And just for comparison, here is some timing for using a slower chip and different cordic code, compared to math.h

And as a test, I ran the PBP code for SQR and ATN to compare it's speed.
Code:
```x = x ATN y
y = SQR ang```
took about 48.5 uS, but has much less precision.

PBP's SIN and COS function were much quicker at 4.16 uS, but again, much less precision than the cordic.

Code:
```'/*******************************************************************************
'* FUNCTION NAME:    sin_cos
'*
'* ARGUMENTS:        int angle (angle in 16-bit binary radians)
'*
'* RETURNS:          x = sin, y = cos (and can give tan: remember tan=sin/cos)
'*
'* DESCRIPTION:      The angle is given in 16-bit radians (on a scale of -32,768
'*                   to 32,767). The function simultaneously calculates the sine
'*                   and cosine of the angle as fractions of 30,000 (where 30,000
'*                   equates to 1 and -30,000 equates to -1) and returns them in
'*                   a sin_cos_struct.
'*
'* EXAMPLE:          ang = 5461 'radians (30 degrees);
'*                   gosub sincos;
'*                   result: x = 15000, y = 25980 'radians
'*                   For "decimal" divide by 3 to get 5000 (.5000) and 8660 (.8660)
'*                     To figure Tan result, use radians (x * 10000), then
'*                     tan = div32 y, result will be 5773 (for .5773)
'*******************************************************************************/

'/*******************************************************************************
'* FUNCTION NAME:    atan2_sqrt
'*
'* ARGUMENTS:        int y (y-coordinate)
'*                   int x (x-coordinate)
'*
'* RETURNS:          x = atan2 y,x , or atan if x = 30000 radians (1.0000 decimal)
'*                     for atan: (atan2 y,1 = atan y)
'*                     y = hypotenuse
'*                     atan2 or atan results will be in radians, see chart
'*
'* DESCRIPTION:      Given an ordered pair of coordinates, the function
'*                   simultaneously calculates the atan2 (the direction of the
'*                   position vector in 16-bit radians) and the square root of
'*                   the sum of the squares of the coordinates (the magnitude of
'*                   the position vector) and returns them in an
'*                   atan2_sqrt_struct.
'*
'* NOTES:            (1) The accuracy of the returned values increases as the
'*                   sizes of x and y increase. Consider multiplying both by a
'*                   scaling factor before calling the function.
'*                   (2) The function will fail for x and y values that result in
'*                   magnitues greater than 32,767 (the size of a signed int).
'*
'* EXAMPLE:          atan2_sqrt_struct bar;
'*                   int x = 25980, y = 15000;
'*                   gosub atan;
'*                   for the angle in radians: x = 5461
'*                     for the hypotenuse: y = 30000
'*******************************************************************************/```
Code:
```;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
; http://www.chiefdelphi.com/media/papers/2016
; FILE NAME:        trig.inc
; AUTHOR:           Patrick Fairbank
;                   Modified by Walter Dunckel (Scale Robotics Inc.) with help from Darrel Taylor
;                   http://www.scalerobotics.com/cordic.html
; DESCRIPTION:      This file contains functions implementing the CORDIC
;                   algorithm, or how to get a 16 bit sin, cos, tan2 and hypotenuse result
;
; USAGE:            Add this file to your PicBasic Pro project using INCLUDE "TRIG.inc"
;                   Then fill x,y values for atan2, or fill ang value for sincos
;                   then either GOSUB sincos or GOSUB atan
; LICENSE:          Users are free to use, modify, and distribute this code
;                   as they see fit.
;
;******************************************************************************/

i       var byte     BANK0
j       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[15] 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 OverAtan

sincos:

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

; Initialize _y to 0
clrf _y
clrf _y+1

; Check if the angle is greater than 16383 (90°)
sc_check_greaterthan:
btfss _ang+1, 7           ;*
btfss _ang+1, 6           ;*
bra sc_check_lessthan     ;

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

negf _ang                   ;
comf _ang+1                 ;

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

comf _ang                   ;
comf _ang+1                 ;

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

; 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
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
movf POSTINC0, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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:
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:

endasm

return          'Done with sincos , return

;######################################################################
; Calculates the magnitude and direction of the given ordered pair
;atan_sqrt:

atan:
asm

; Initialize _ang to 0
clrf _ang
clrf _ang+1

; 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
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
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
movf PRODH, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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
movf PRODH, W
clrf WREG
movlw 0x4d
mulwf _dx
movf PRODL, W
movf PRODH, W
clrf WREG
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
bra as_output
btfss _ang+1,7

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

; Toggle the sign bit
btg _ang+1, 7

; Output the calculated angle and hypotenuse values
as_output:
endasm

return

OverAtan:```
Last edited by ScaleRobotics; - 21st April 2011 at 02:57. Reason: Update with cleaner code

14. Added this content to post 13
Last edited by ScaleRobotics; - 5th February 2011 at 07:46. Reason: Added this content to post 13

15. WOW, I have enough trouble doing trig on a calculator. Thank You both for this. Could you please assist us mathematicly challenged with a couple of lines of example code as to how to use this masterpiece? I get the part where my program has main: respondant to your goto main, but how do I use the functions? Or is that what post 8 is about?

16. No problem, probably should have done that in the first place.

Use the includes for trig.inc at the beginning of the program

When you are doing an atan2 (you get the hypotenuse for free) load up values x and y.

For instance
Code:
```x = 25980
y = 15000```
what do these numbers mean? Well for atan2 they mean a point at location x,y from origin. This function accepts numbers up to from -32,767 to 32,767

Code:
`call atan`
result, ang = 5461 or about 30 degrees radian (60 degrees deg).

You will get your result in the variable called "ang" for angle and "x" for hypotenuse. Now hypotenuse will be in the same scale as your x and y coordinates, but the angle is represented by 0 = 0 and 32,767 = 180 degrees. I believe that -5461 will be 120 degrees. So to figure your angle, 1 degree = 46602/256 (or about 182) of these radians. Update, since radians start at 90 degrees, you have to do a little math to calc degrees.

Now for sincos. This calculates the sin and cos simultaneously.

Load you angle "ang" with an angle value using the funky radians from above

Code:
```ang = 5461
call sincos```
results, sin(60 degrees) = x = 25981 , cos(60 degrees) = y = 15004

Now to make sense of your results, instead of using the strange radians from above, you get a result of -30,000 to 30,000. So converting the above results by dividing by 30,000, you get
sin(60) = .8660333 and cos(60)= .5001333 . These results are pretty close to what I get on my calculator, .8660254 and .500000 respectively.
Last edited by ScaleRobotics; - 13th February 2009 at 10:01.

17. Originally Posted by scalerobotics
For instance
Code:
```x = 25980
y = 15000```
.....and that is 30 degrees
My perspective was all wrong in the above post. I was supposed to be lying down and facing East, that is the radian way.

to convert to degrees ......

Imagine a circle with degree headings, but instead of starting at the top, and going clockwise, as the numbers get higher, we are going to do something completely different. Starting at 90 degrees, we are going to go counterclockwise while our radians rise. From zero to 32,767 is the arc from 90 degrees (through 0) to 270 degrees. To complete the circle, our arc continues from 32,768 to 65,535 as it travels from 270 degrees to 90 degrees in a counter clockwise rotation.

So, as you can see there is just a little bit of manipulation to get the result in angle, but it really isn't too bad.

Thanks Joe.
Last edited by ScaleRobotics; - 13th February 2009 at 09:52.

18. For anyone else like me that forgot all they learned about radians.... This should help. These go from 0 to 65,535.
Last edited by ScaleRobotics; - 5th February 2011 at 18:05.

19. Thank You Walter,
It is definitely going to take some time to get my head around this, unfortunate side effect of growing up during the Vietnam war, where we were expected to become soldiers who got shipped there and returned in a nicely bagged and boxed container. My algebra teacher was a retired navel captain, who taught spit about the subject but spoke extensively about his career and travels in Tahiti. That notwithstanding I will get this. I am sure nobody who participates in this forum is going to point at me in the store and say "hey there is the dummy who couldn't understand that code Walter wrote . . . " <b>So Anyway</b> the numbers you plugged in represent . . .Units, of my choosing . . .and to use this
Code:
```include"trig.inc"
main:
ang=5461
x=0
y=0

asm
call sin_cos
endasm

Lcdout \$fe, 1   ' Clear LCD screen

lcdout \$FE,1,#x,",",#y
lcdout \$FE,\$C0,#ang```
Correct? Figure I do not need to know how to build a truck to drive one.

20. Hey Joe,

No you definitely do not need to know how it works. But if you find out, please let me know. The assembly code only seems to put me to sleep when I read it. Something about rotating angle, shifting bits zzzzz.....

Your code will work, but it can be even simpler since it has the includes file in it. You don't have to get in to the asm endasm, as the sincos function is already there.

Code:
```include"trig.inc"
main:
ang=5461
x=0
y=0

call sincos

Lcdout \$fe, 1   ' Clear LCD screen

lcdout \$FE,1,#x,",",#y
lcdout \$FE,\$C0,#ang```
It is that easy. Only a little bit harder if you want to do anything with the data returned in x and y (or ang and x for atan function)

21. Originally Posted by scalerobotics
You don't have to get in to the asm endasm, as the sincos function is already there.
Funny, that was a copy paste, I don't know where the asm / endasm went, glad to know it can be omitted ( call sincos ). Just 1 (I hope) more question: Where does the number ang=5461 come from ? I apologize if I seem tedious, I really haven't a clue. Thanks.

22. That is the hardest part. Darn radians are 90 degrees off from the start.

For example

0 = 90 degrees
5461 = 60 degrees
10922 = 30 degrees
16383 = 0 degrees (or North)
21844 = 330 degrees
32767 = 270 degrees
38228 = 240 degrees

and so on.....

You definitely do not have to know this, but I think it is interesting:
In the time it takes for a regular Sin, or Cos 16 bit function on a PIC18 to give a result, light travels 3000 kilometers
The time it takes for this cordic 16 bit result, light travels 33.3 kilometers.
How do you say "Your mileage may vary" when you use kilometers?
Last edited by ScaleRobotics; - 13th February 2009 at 20:20.

23. Originally Posted by scalerobotics
That is the hardest part. Darn radians are 90 degrees off from the start.

For example

0 = 90 degrees
5461 = 60 degrees
10922 = 30 degrees
16383 = 0 degrees (or North)
21844 = 330 degrees
32767 = 270 degrees
38228 = 240 degrees

and so on.....

You definitely do not have to know this, but I think it is interesting:
In the time it takes for a regular Sin, or Cos 16 bit function on a PIC18 to give a result, light travels 3000 kilometers
The time it takes for this cordic 16 bit result, light travels 33.3 kilometers.
How do you say "Your mileage may vary" when you use kilometers?
Moving counter clockwise from the 3 o'clock position . . .I see for every 30 degrees the numbers change 5461 or 182.03333333333333333333333333333 per degree or 65532 for a full circle which is a number that fits nicely in a word variable.

24. Originally Posted by Joe S.
Moving counter clockwise from the 3 o'clock position . . .I see for every 30 degrees the numbers change 5461 or 182.03333333333333333333333333333 per degree or 65532 for a full circle which is a number that fits nicely in a word variable.
Well now, that's better than all my blabbering! Looks like you got it down.

25. 16 bit trig for sin cos tan2 and hypotenuse

Here's one for people like me who think in degrees and not radians:

sincos example:
input ang in degrees.dd example: ang = 3000 (= 30.00 degrees)
call sincos
result: x = 15004 , Y = 25981
so, 15004/30000 = sin(ang) = 0.5001 and 25981/30000 = cos(ang) = 0.8660

atan2 example:
input x,y co-ordinates
x = 25981, y = 15000
call atan2
result: x = 29998 = hypotenuse or distance to x,y, y = angle to x,y in degrees.dd = 5998 (=59.98 degrees), ang = 5463 = radians 0 to 65535

Code:
```'/*******************************************************************************
'                  TRIG.inc for PIC18 V 1.5
'/*******************************************************************************
'* FUNCTION NAME:    sincos
'*
'* ARGUMENTS:        ang (angle deg.dd format example: 35999 = 359.99 deg)
'*
'* RETURNS:          x = sin(ang) , y = cos(ang)
'*
'* DESCRIPTION:      The angle is given in degrees.dd (see above)
'*                   The function simultaneously calculates the sine
'*                   and cosine of the angle as fractions of 30,000 (where 30,000
'*                   equates to 1 and -30,000 equates to -1) and returns them in
'*                   x, and y as x=sin(ang), y = cos(ang).
'*
'* EXAMPLE:          ang = 3000 (30.00 degrees) x = 15004 , Y = 25981
'*                   so, 15004/30000 = 0.5001 and 25981/30000 = 0.8660
'*
'*******************************************************************************/

'/*******************************************************************************
'* FUNCTION NAME:    atan2
'*
'* ARGUMENTS:        int x (x-coordinate)
'*                   int y (y-coordinate)
'*
'* RETURNS:          atan2 of x,y:(angle to x,y) and the hypotenuse (distance) of x,y
'*
'* DESCRIPTION:      Given an ordered pair of coordinates, the function
'*                   simultaneously calculates the atan2 (the direction of the
'*                   position as y degrees, and ang radians) and the square root of
'*                   the sum of the squares of the coordinates (the magnitude of
'*                   the position vector) as x
'*
'*
'* NOTES:            (1) The accuracy of the returned values increases as the
'*                   sizes of x and y increase. Consider multiplying both by a
'*                   scaling factor before calling the function.
'*                   (2) The function will fail for x and y values that result in
'*                   magnitues greater than 32,767 (the size of a signed int).
'*
'* EXAMPLE:          atan2
'*                   x = 25980, y = 15000;
'*                   results:
'*                   ang = angle in radians 0 to 65535
'*                   x = distance = 30000 (same units as x and y)
'*                   y = 6000 (angle in degrees 60.00 degrees)
'*******************************************************************************/

;*******************************************************************************
; --- CORDIC TRIG LIBRARY ---
;http://www.chiefdelphi.com/media/papers/2016
; FILE NAME:        trig.inc
; AUTHOR:           Patrick Fairbank
; Last Modified     Feb 15, 2012 - fixed bug in sincos which calculated wrong
;                   result for sin and cos in quadrants 2,3 and 4. Thanks Martin!
;                   FEB. 15, 2009 to make it PicBasic compatible and added degree conversion
;                   Modified by Walter Dunckel (Scale Robotics Inc.) with help from Darrel Taylor
;                   http://www.scalerobotics.com/79-cordic-for-picbasic.html
; DESCRIPTION:      This file contains functions implementing the COORDIC
;                   algorithm, or how to get a 16 bit sin, cos, tan2 and hypotenuse result
;
; USAGE:            Add this file to your PicBasic Pro project using INCLUDE "TRIG.inc"
;                   and add a line with main: directly below the include line
;                   Then fill x,y values for atan2, or fill ang value for sincos
;                   then either CALL sincos or CALL atan2
; LICENSE:          Users are free to use, modify, and distribute this code
;                   as they see fit.
;
;    sincos: input ang, output x = sin(ang) and y = cos(ang)
;
;
;     atan2: input x and y coordinates
;     Output the calculated angle and hypotenuse values
;     as output:    y = angle in degrees, ang = angle in radians, x = hypotenuse
;
;******************************************************************************/
'   Example output using TRIG.inc using hserout
'   HSEROUT ["Angle = ",dec a,", sin(ang) = ",sdec x,", cos(ang) = ",sdec y,13]
'Angle = 0, sin(ang) = 5, cos(ang) = 30000
'Angle = 10, sin(ang) = 5209, cos(ang) = 29544
'Angle = 20, sin(ang) = 10266, cos(ang) = 28188
'Angle = 30, sin(ang) = 15004, cos(ang) = 25981
'Angle = 40, sin(ang) = 19287, cos(ang) = 22981
'Angle = 50, sin(ang) = 22983, cos(ang) = 19285
'Angle = 60, sin(ang) = 25983, cos(ang) = 14998
'Angle = 70, sin(ang) = 28188, cos(ang) = 10264
'Angle = 80, sin(ang) = 29544, cos(ang) = 5207
'Angle = 90, sin(ang) = 30000, cos(ang) = 3
'Angle = 100, sin(ang) = 29543, cos(ang) = -5223
'Angle = 110, sin(ang) = 28189, cos(ang) = -10276
'Angle = 120, sin(ang) = 25974, cos(ang) = -15010
'Angle = 130, sin(ang) = 22973, cos(ang) = -19293
'Angle = 140, sin(ang) = 19274, cos(ang) = -22992
'Angle = 150, sin(ang) = 14991, cos(ang) = -25989
'Angle = 160, sin(ang) = 10249, cos(ang) = -28190
'Angle = 170, sin(ang) = 5192, cos(ang) = -29546
'Angle = 180, sin(ang) = -9, cos(ang) = -29999
'Angle = 190, sin(ang) = -5216, cos(ang) = -29544
'Angle = 200, sin(ang) = -10271, cos(ang) = -28184
'Angle = 210, sin(ang) = -15013, cos(ang) = -25977
'Angle = 220, sin(ang) = -19296, cos(ang) = -22976
'Angle = 230, sin(ang) = -22987, cos(ang) = -19275
'Angle = 240, sin(ang) = -25986, cos(ang) = -14992
'Angle = 250, sin(ang) = -28199, cos(ang) = -10258
'Angle = 260, sin(ang) = -29549, cos(ang) = -5195
'Angle = 270, sin(ang) = -30000, cos(ang) = 5
'Angle = 280, sin(ang) = -29544, cos(ang) = 5213
'Angle = 290, sin(ang) = -28188, cos(ang) = 10266
'Angle = 300, sin(ang) = -25981, cos(ang) = 15004
'Angle = 310, sin(ang) = -22981, cos(ang) = 19287
'Angle = 320, sin(ang) = -19283, cos(ang) = 22985
'Angle = 330, sin(ang) = -14998, cos(ang) = 25983
'Angle = 340, sin(ang) = -10260, cos(ang) = 28190
'Angle = 350, sin(ang) = -5207, cos(ang) = 29544

i       var byte     BANK0
j       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[15] 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 OverTrig

atan2:
asm
call atan2_sqrt
endasm
'convert to degrees.dd    y is degrees
If ang < 16384 then y = 16383 - ang
if ang > 16383 then
y = 65535 - ang
y = y + 16383     'correct 90 degrees for radian
endif
y = y * 256     'divides radians to get degrees within 57ppm
y = div32 466    'degrees.dd is y, radians is ang

return

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 radians
ang = ang * 466
ang = div32 256
asm
call sin_cos
endasm
return

asm

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

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

; Initialize _y to 0
clrf _y
clrf _y+1

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

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

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

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

; 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
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
movf POSTINC0, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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:
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:

return

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

; Initialize _ang to 0
clrf _ang
clrf _ang+1

; 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
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
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
movf PRODH, W
movf _dx, W
movf _dx+1, W
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
movf _dy+1, W

; 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
movf PRODH, W
clrf WREG
movlw 0x4d
mulwf _dx
movf PRODL, W
movf PRODH, W
clrf WREG
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
bra as_output
btfss _ang+1,7

negf _ang
comf _ang+1

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

comf _ang
comf _ang+1

; Toggle the sign bit
btg _ang+1, 7

; Output the calculated angle and hypotenuse values
as_output:

return

endasm

OverTrig:  'jump over code```
Last edited by ScaleRobotics; - 15th February 2012 at 16:37. Reason: fixed sincos bug corrected in v1.5

26. TRIG for non-integer angles using a PIC16F684?

Hey, nerds! I'm relatively new to PICs, and right now I'm using the PIC16F684.

I'm trying to write a program for a mobile robot with two stepper motors for differential steering - to navigate it along a predefined path (or function). In order to do this, I need to be able to calculate trig values using Assembly code (yikes!). Not only that, but due to the physical geometry of my robot, I need to calculate the sine and cosine of multiples of 0.72 (non-integers).

Will this TRIG.INC file work with non-integers? If not, is there a way to get around this (DP shifting)?

Samuel Aaron Ward
[email protected]
(740) 357-8016 cell

27. Hello Samuel,

Can you explain the significance of the .72, or maybe what you are calculating. I put on my nerd glasses, but it's not helping. There might not enough resolution in the PIC16 cordic version, but then, I do not have a grasp of what you are measuring, so maybe it would be fine.

The PIC16 cordic is here http://www.picbasic.co.uk/forum/showthread.php?t=7502

Thanks,
Walter

28. scalerobotics, the .72 probably represents 7.2 degrees per step for the motor... Stepper motors usually come in 1.8 and 7.2 degrees per step...

Dave Purola,
N8NTA

29. Thanks Dave!

In that case, the PIC16 should work for you Samuel. But it does need some modification to enable you to find the angle throughout 360 degrees. The PIC16 cordic only does 0 to 90 degrees. But the rest can be done pretty easily with PicBasic. I think it handles around 2000 iterations per second, but I may have been doing a few more equations when I measured that.

Here is some code that works on a Pic16F877a:
Code:
```'****************************************************************
'*  Name    : UNTITLED.BAS                                      *
'*  Author  : Walter Dunckel                                    *
'*  Notice  : Copyright (c) 2009 Scale Robotics Inc.            *
'*  Date    : 10/7/2009                                         *
'*  Version : 1.0                                               *
'*  Notes   :                                                   *
'*          :                                                   *
'****************************************************************
DEFINE __16F877A 1				; list directive to define processor
DEFINE OSC 20                    ' Define Oscillator Speed
include "trig-pic16.inc"

' Shutdown comparators
ADCON1 = 7                        ; Turn of all A/D
CMCON  = 7
' Set the port directions
' 0=output 1=input
TRISA=%00000011                         ' Set PORTA  0-1-3 used for AD
TRISB=%11011100                         ' Set PortB
TRISC=%10000000
TRISD=%00000000                       ' Set PortC USART RX as an input
porta.2 = 0
porta.3 = 0

x           var     word
y           var     word
z           var     word
i           var     word
lon_dif     var     word
lat_dif     var     word
angle       var     word
angle_deg   var     word
distance    var     word
j           var     word

;---------[change these to match your hardware]------------------------------
DEFINE LCD_DREG PORTD             ; Set LCD Data port                B C   D
DEFINE LCD_DBIT 0                 ; Set starting Data bit (0 or 4)   4 0
DEFINE LCD_RSREG PORTA            ; Set LCD Register Select port     A A
DEFINE LCD_RSBIT 3                ; Set LCD Register Select bit      3 2
DEFINE LCD_EREG PORTA             ; Set LCD Enable port              A A
DEFINE LCD_EBIT 1                 ; Set LCD Enable bit               1 5
DEFINE LCD_BITS 4                 ; Set LCD bus size (4 or 8 bits)   4 4
DEFINE LCD_LINES 2                ; Set number of lines on LCD       2 2
'----------------------------------------------------------------------------
clear
lcdout \$FE,1
' to convert degrees to radians Z_ * 256 / 90
' to convert radians to degrees Z_ * 90 / 256
Start:
x = 0
y = 0
z = 0

Z_ = 90                    'put angle in Z_ in degrees
Z_ = (Z_ * 255)/90     'convert degrees to radians
Call sincos                 'perform sin and cos on Z_ value
'cos(Z_) = X_ :sin(Z_) = Y_

lcdout \$FE,1,"cos Z=",#X_
lcdout \$FE,\$C0,"sin Z=",#Y_
end```
And the pic16 trig file: http://sites.picbasic.net/media/uhp2...trig_pic16.txt

30. Nice work, but I am also challenged with maths.
Will this allow me to do point rotation?

Say I have an origin x=0 & y=0, and another point x=4, y=4.
Can I find out the angle of the second coordinates where the first set is the origin,
and then rotate the second point say 180 degrees around the origin ?
Art.

31. Uh-oh.. just including the line:
include "trig-pic16.inc"
causes this:

Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 213 : [235] Opcode Expected Instead of 'dt'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 216 : [235] Opcode Expected Instead of 'dt'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 219 : [235] Opcode Expected Instead of 'dt'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 222 : [235] Opcode Expected Instead of 'dt'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 237 : [226] Numeric Constant or Symbol Name Expected
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 239 : [226] Numeric Constant or Symbol Name Expected
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 269 : [235] Opcode Expected Instead of 'movfw'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 283 : [235] Opcode Expected Instead of 'movfw'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 301 : [235] Opcode Expected Instead of 'movfw'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 306 : [235] Opcode Expected Instead of 'movfw'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 316 : [235] Opcode Expected Instead of 'movfw'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 321 : [235] Opcode Expected Instead of 'movfw'
Error C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 349 : [235] Opcode Expected Instead of 'movfw'
Warn C:\PROGRA~1\MPLAB\PROJECT\SCROLL.ASM 934 : [102] Code Crosses Boundary @ 800h

I'm using a 16F877 too.

32. Use MPASM for the assembler.
<br>

33. Thanks Darrel,
I'm working on hardware now, but I'll try again as soon as possible.
Any idea on my first question?

34. Originally Posted by Art
Nice work, but I am also challenged with maths.
Will this allow me to do point rotation?

Say I have an origin x=0 & y=0, and another point x=4, y=4.
Can I find out the angle of the second coordinates where the first set is the origin,
and then rotate the second point say 180 degrees around the origin ?
Art.
Hi Art,

You can definitely find the angle in the first part of your question. As far as rotation, I have to think a little more. Should be possible though.

Are you rotating one of your LED displays? If so, are we talking full rotation, or just 90, 180, 270 degrees?

35. Ideally, to potentially be able to do full rotation of a pixel.
Yes, I have a matrix LED display in mind
I wouldn't care whether or not it is equally divided into 360 degrees.
I already have made routines that make it easy to draw to the screen.

I can do this in C on the Sony PSP like this
Code:
```xx = 240;
yy = 30;
x = xx - 240;
y = yy - 136;

theta = rotation;
thetab = 360 - theta;
thetab = (3.141592 / 180) * theta;

xnew = cos(thetab)*x - sin(thetab)*y;
ynew = sin(thetab)*x + cos(thetab)*y;

xnew = xnew + 240;
ynew = ynew + 136;```
Where x-240, y-136 are coordinates for the centre of the screen,
x-240,y-30 is the arbitrary point I want to rotate (the one given is a point directly above the centre of the screen),
rotation is the real angle 0-359 (0 can also be 360).
xnew and ynew are the new (rotated) coordinates.
I don't know if this works with microcontrollers even if you do use a C compiler.

36. Originally Posted by Darrel Taylor
Use MPASM for the assembler.
<br>
It seems that this line becomes offensive:
Code:
`'@ DEVICE LVP_OFF,BOD_OFF,HS_OSC				'`
Which is how I set configuration.
Art.
Last edited by Art; - 29th January 2010 at 13:41.

37. You need to use a different syntax for MPASM.

like this:
Code:
```Example: To set the PIC for Internal Oscillator allowing use of the OSC pins as I/O...

@ __config _INTRC_OSC_NOCLKOUT

Only one config statement is allowed when using MPASM, so multiple definitions follow-on from each other each connected to it’s previous buddy with an &.

Example: Typical Settings for a 16F628…

@ __config _INTRC_OSC_NOCLKOUT & _WDT_ON & _PWRTE_ON & _MCLRE_OFF & _BODEN_ON & _LVP_OFF & _CP_ALL & _DATA_CP_ON```

38. Originally Posted by Art
Ideally, to potentially be able to do full rotation of a pixel.
Yes, I have a matrix LED display in mind
I wouldn't care whether or not it is equally divided into 360 degrees.
I already have made routines that make it easy to draw to the screen.

I can do this in C on the Sony PSP like this
Code:
```xx = 240;
yy = 30;
x = xx - 240;
y = yy - 136;

theta = rotation;
thetab = 360 - theta;
thetab = (3.141592 / 180) * theta;

xnew = cos(thetab)*x - sin(thetab)*y;
ynew = sin(thetab)*x + cos(thetab)*y;

xnew = xnew + 240;
ynew = ynew + 136;```
Where x-240, y-136 are coordinates for the centre of the screen,
x-240,y-30 is the arbitrary point I want to rotate (the one given is a point directly above the centre of the screen),
rotation is the real angle 0-359 (0 can also be 360).
xnew and ynew are the new (rotated) coordinates.
I don't know if this works with microcontrollers even if you do use a C compiler.
Yes, it looks like you could use the same type of math. The cordic lets you compute sin and cos in a single operation. It is very quick with this type of thing, and can perform about 2000 of these per second.

So you would just have to solve:

as you have in your above equation. The PIC18 cordic is a little better, and I documented it more. But if you still want to do it with your PIC16, I am here to help.

Wikipedia talks a little bit about rotation here: http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
Last edited by ScaleRobotics; - 31st January 2010 at 06:20.

39. I'm not quite ready to do it yet, only because I'm in the middle of something else,
but regarding the LED displays, I've got a blitter routine handling sprites now.
It would be really nice to see if it could handle rotation of a sprite (pixel by pixel).

Thanks for your offer to help, I'll surely take you up on it very soon

40. Re: Cordic trig assembly code for PIC18f

Hi !
I'm trying to write a program to calculate sunrise and sunset. The pic is 18F2550 and I'm using long variables and PBPL. I've tried to use the TRIG.INC but it seems that I cant use it in PBPL, Error: DIV32. Is there a way to rewrite the program so that it will work with PBPL?
I'm using Picbasic Pro 2.50 and MPASM 8.15.
Bernt-Ove

Posting Permissions

• You may not post new threads
• You may not post replies
• You may not post attachments
• You may not edit your posts