Cordic trig assembly code for PIC18f

[ScaleRobotics]

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:


;************************************************* ******************************
; --- CORDIC TRIG LIBRARY ---
;http://www.chiefdelphi.com/media/papers/2016
; FILE NAME: trig.asm
; AUTHOR: Patrick Fairbank
; LAST MODIFIED: Jan. 16, 2008
;
; DESCRIPTION: This file contains functions implementing the COORDIC
; algorithm.
;
; USAGE: Add this file to your project.
;
; 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
quad 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

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

[ScaleRobotics]

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:


; Import the address the compiler uses to store 32-bit return values
EXTERN AARGB3


UDATA_ACS

; Variable declarations
i RES 1
j RES 1
quad 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:


i var byte
j Var byte
quad 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

[Darrel Taylor]

Hi Walter,
I thought we had this working already. :eek:

Here's one thing I see...
; 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>

[ScaleRobotics]

I thought we had this working already. :eek:
Thanks Darrel! Yes, we had the cordic working for pic16 chips (can be found here http://www.picbasic.co.uk/forum/showthread.php?t=7502&p=46307#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. :confused:

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.

[Darrel Taylor]

IT WORKS!
SWEET!

You make it easy Walter.

I know you just got it working, so you probably don't know this yet ...
I'm curious about the accuracy.

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

What's the new resolution?
<br>

[ScaleRobotics]

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!

[ScaleRobotics]

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/content.php?r=175-Code-Timer-How-long-does-it-take-to-execute-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
http://www.picbasic.co.uk/forum/attachment.php?attachmentid=5149&d=1296892329

And as a test, I ran the PBP code for SQR and ATN to compare it's speed.


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.



'/************************************************** *****************************
'* 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
'************************************************* ******************************/


;************************************************* ******************************
; --- CORDIC TRIG LIBRARY ---
; http://www.chiefdelphi.com/media/papers/2016
; FILE NAME: trig.inc
; AUTHOR: Patrick Fairbank
; LAST MODIFIED: FEB. 1, 2011 to make it cleaner
; 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
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[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

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

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

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

return

OverAtan:
5148

[ScaleRobotics]

Added this content to post 13

[Archangel]

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?

[ScaleRobotics]

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


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

now call your atan subroutine


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



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.

[ScaleRobotics]

For instance


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.

[ScaleRobotics]

For anyone else like me that forgot all they learned about radians.... This should help. These go from 0 to 65,535.

[Archangel]

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 . . . " :D <b>So Anyway</b> the numbers you plugged in represent . . .Units, of my choosing . . .and to use this

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.

[ScaleRobotics]

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.



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)

[Archangel]

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.

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

[Archangel]

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.

[ScaleRobotics]

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.

[ScaleRobotics]

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



'/************************************************** *****************************
' 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
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[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

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

return

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

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

return

endasm

OverTrig: 'jump over code

[samuelaaronward]

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

Thanks for your help!
Samuel Aaron Ward
[email protected]
(740) 357-8016 cell

[ScaleRobotics]

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

[Dave]

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

[ScaleRobotics]

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:


'************************************************* ***************
'* Name : UNTITLED.BAS *
'* Author : Walter Dunckel *
'* Notice : Copyright (c) 2009 Scale Robotics Inc. *
'* : All Rights Reserved *
'* Date : 10/7/2009 *
'* Version : 1.0 *
'* Notes : *
'* : *
'************************************************* ***************
DEFINE __16F877A 1 ; list directive to define processor
DEFINE OSC 20 ' Define Oscillator Speed
DEFINE LOADER_USED 0
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/repository/91/trig_pic16.txt

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

[Art]

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.

[Darrel Taylor]

Use MPASM for the assembler.
<br>

[Art]

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

[ScaleRobotics]

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?

[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


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.

[Art]

Use MPASM for the assembler.
<br>

It seems that this line becomes offensive:

'@ DEVICE LVP_OFF,BOD_OFF,HS_OSC '
Which is how I set configuration.
Art.

[ScaleRobotics]

You need to use a different syntax for MPASM.

like this:


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


More (much more) here: http://www.picbasic.co.uk/forum/showthread.php?t=543

[ScaleRobotics]

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


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:

http://www.picbasic.co.uk/forum/attachment.php?attachmentid=3944&stc=1&d=1264910958

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

[Art]

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

[B-O Eliasson]

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

[B-O Eliasson]

The problem is solved. I found another version of trig.inc in post 13, and it works well for me. I have to put the word variables x,y into long variables before doing any calculations on them.

[ScaleRobotics]

The problem is solved. I found another version of trig.inc in post 13, and it works well for me. I have to put the word variables x,y into long variables before doing any calculations on them.

That's a good bare bones cordic version as long as you don't mind manipulating the radians. So you could either use that and do math to your liking, or you could use TrigL , which you can find with my tilt example here: http://www.picbasic.co.uk/forum/content.php?r=210-Angle-determine-tilt-from-3-axis-g-sensor . If you use TrigL.inc, you would probably want to add a line to replace goto main with goto overcordic
and at the end of the TrigL.inc add a line overcordic:

Keep us posted, I'd love to see your project.

Walter

[chrisshortys]

Hi Walter, I have been looking at this for some time now and keep coming back to this post. I am actually attempting to do a similar project to the tilt sensor project which you have built only on a single axis. I believe I am almost there now and just need a few pointers! I am looking at your code atn2. the following is the equation i require to solve: tan-1 ((zresult - calval)/SQR ((5017 - calval)^2 - (zresult - calval)^2))
Now from reading your posts would I be right in thinking that if i do the following:


x=0
y= ((zresult - calval)/SQR ((5017 - calval)^2 - (zresult - calval)^2))

call atn2


Am I right in thinking that this would convert my equation as I am not interested in the hyp or does the x vaule come into play?

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

6268


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.

[readitaloud]

Hi,

I am using your TRIG.inc in a small program. The values appear correct from (ang) = 0 to 9000, 0 to 90.00 degrees. For values, 9000 < (ang) < 18000 ,the
values for x and y appear to switch. As an example with an (ang) value of 8900 or 89.00 degrees I receive a value back x = 29995 and y = 525. This seems correct.
For an (ang) value of 9100 or 91.00 degrees I receive back x = -523 and y = 29995. For 91.00 degrees I would expect x = 29995 and y = -523 ?

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

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]

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

[readitaloud]

Walter,

I added three lines as shown in red. With the additions all 4 quadrants are returning correct magnitude and sign values.

See attached test file.

- Martin


;************************************************* *****************************/


i var byte BANK0
j Var byte BANK0
quad var byte BANK0
x var word bank0
y var word bank0
ang var word bank0
ang_temp 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 overcordic


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
ang_temp = ang ' Store ang.
if ang < 9001 then ang = 9000 - ang
if ang > 9000 then ang = 27000 - (36000 - ang)
ang = ang * 466
ang = div32 25662716272

asm
call sin_cos
endasm
if ang_temp > 9000 then y = ~y + 1 ' Perform 2's complement of y if ang > 90 for cos(ang).
return


asm


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

[ScaleRobotics]

Thanks Martin,

That works! But there is a little less math in post 49. It makes the rest of the cordic do what its supposed to for the rest of the quadrants. Thanks for pointing it out.

Here is TRIG.inc V1.5

[mark_s]

Walter,

Thank you for your original work and the update.

Martin, thanks for bringing up again.

I was trying to make this work last year. My results were similar to Martin's.
Assuming it was something in my code. I had to settle for pbp ATN.
Can't wait to try it out

Best regards

[readitaloud]

Walter,,

Yes the statement: IF ang > 9000 then ang = 45000 - ang, does the job. For some reason the first time I tried using it the results were crazy so I simply tried to make a patch. I must have typed in something incorrectly. Excellent job Walter ! Thanks for this piece of code.

73's de Martin

[bikxsici]

hi ScaleRobotics , i am interested in your codes posted on #51. and i have a problem.
...
ang=3000
call sincos
x=60395, y=39551
...
for all ang values i get same numbers, i dont understan what is the problem? thank you

[ScaleRobotics]

That's weird. Maybe you can show us some of your code?