+ Reply to Thread
Results 1 to 40 of 55

 6th February 2009, 18:11 #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 ; 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
Any help would be greatly appreciated.Last edited by ScaleRobotics;  6th February 2009 at 22:21.

 8th February 2009, 18:22 #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 32bit 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
My code:
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
Thanks,
WalterLast edited by ScaleRobotics;  8th February 2009 at 18:35. Reason: left things out, and forgot a [/code]

 9th February 2009, 05:45 #3
Hi Walter,
I thought we had this working already.
Here's one thing I see...Code:; Set up the main loop sc_loop_start: clrf i banksel atans lfsr FSR0, atans
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 05:50. Reason: BANK0 VARS
DT

 9th February 2009, 06:18 #4I thought we had this working already.
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.
WalterLast edited by ScaleRobotics;  7th September 2011 at 15:44.

 9th February 2009, 06:59 #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>DT

 9th February 2009, 08:22 #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
Code:; Restore the stack to its previous state ; movf POSTDEC1 ; movff INDF1, FSR2L
Last edited by Darrel Taylor;  9th February 2009 at 09:48. Reason: Missed some
DT

 9th February 2009, 11:05 #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ñanaDT

 10th February 2009, 16:33 #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 ADCON1=15 TRISA=%00000011 ' Set PORTA TRISB=%00000000 ' Set PortB TRISC=%00000000 TRISD=%00000100 ' Vdd 5 volts ' Vss Ground ' Vo 20K potentiometer (or ground) ' DB03 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
Last edited by ScaleRobotics;  10th February 2009 at 16:36.

 10th February 2009, 17:09 #9
There are 15 elements in the array (014).
Might be overwriting another variable.
<br>Last edited by Darrel Taylor;  10th February 2009 at 17:15. Reason: .
DT

 10th February 2009, 17:28 #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 18:00.

 10th February 2009, 22:14 #11IT WORKS!
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>DT

 11th February 2009, 04:45 #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 04:48.

 11th February 2009, 10:08 #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...teXYZcommand
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
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 16bit binary radians) '* '* RETURNS: x = sin, y = cos (and can give tan: remember tan=sin/cos) '* '* DESCRIPTION: The angle is given in 16bit 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 (ycoordinate) '* int x (xcoordinate) '* '* 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 16bit 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 ; 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:
Last edited by ScaleRobotics;  21st April 2011 at 01:57. Reason: Update with cleaner code

 11th February 2009, 10:12 #14
Added this content to post 13
Last edited by ScaleRobotics;  5th February 2011 at 06:46. Reason: Added this content to post 13

 13th February 2009, 04:54 #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?
If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
.
Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants  but debt is the money of slaves
Tyranny is defined when law enforcement is allowed to make the law.
.
There simply is no "Happy Spam" If you do it you will disappear from this forum.

 13th February 2009, 06:20 #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
now call your atan subroutine
Code:call atan
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
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 09:01.

 13th February 2009, 07:42 #17
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 08:52.

 13th February 2009, 09:06 #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 17:05.

 13th February 2009, 16:03 #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 thisCode: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
If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
.
Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants  but debt is the money of slaves
Tyranny is defined when law enforcement is allowed to make the law.
.
There simply is no "Happy Spam" If you do it you will disappear from this forum.

 13th February 2009, 18:20 #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

 13th February 2009, 18:32 #21If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
.
Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants  but debt is the money of slaves
Tyranny is defined when law enforcement is allowed to make the law.
.
There simply is no "Happy Spam" If you do it you will disappear from this forum.

 13th February 2009, 19:04 #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 19:20.

 14th February 2009, 00:57 #23If you do not believe in MAGIC, Consider how currency has value simply by printing it, and is then traded for real assets.
.
Gold is the money of kings, silver is the money of gentlemen, barter is the money of peasants  but debt is the money of slaves
Tyranny is defined when law enforcement is allowed to make the law.
.
There simply is no "Happy Spam" If you do it you will disappear from this forum.

 14th February 2009, 02:27 #24

 15th February 2009, 21:09 #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 coordinates
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 (xcoordinate) '* int y (ycoordinate) '* '* 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/79cordicforpicbasic.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
Last edited by ScaleRobotics;  15th February 2012 at 15:37. Reason: fixed sincos bug corrected in v1.5

 5th October 2009, 20:57 #26
TRIG for noninteger 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 (nonintegers).
Will this TRIG.INC file work with nonintegers? If not, is there a way to get around this (DP shifting)?
Thanks for your help!
Samuel Aaron Ward
aaron@zmm.com
(740) 3578016 cell

 6th October 2009, 07:07 #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,
Walterhttp://www.scalerobotics.com

 6th October 2009, 11:32 #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

 7th October 2009, 17:15 #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. * '* : 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 "trigpic16.inc" ' Shutdown comparators ADCON1 = 7 ; Turn of all A/D CMCON = 7 ' Set the port directions ' 0=output 1=input TRISA=%00000011 ' Set PORTA 013 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
http://www.scalerobotics.com

 27th January 2010, 12:37 #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.

 27th January 2010, 13:14 #31
Uhoh.. just including the line:
include "trigpic16.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.

 27th January 2010, 18:53 #32
Use MPASM for the assembler.
<br>DT

 28th January 2010, 03:51 #33
Thanks Darrel,
I'm working on hardware now, but I'll try again as soon as possible.
Any idea on my first question?

 28th January 2010, 05:34 #34

 28th January 2010, 05:46 #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;
x240,y30 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 0359 (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.

 29th January 2010, 12:34 #36
Last edited by Art;  29th January 2010 at 12:41.

 29th January 2010, 18:20 #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 followon 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

 31st January 2010, 04:57 #38
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%29Last edited by ScaleRobotics;  31st January 2010 at 05:20.

 31st January 2010, 05:21 #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

 4th March 2011, 07:29 #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.
BerntOve
Similar Threads

How much code space do PBP statements use.
By Darrel Taylor in forum Code ExamplesReplies: 5Last Post:  13th February 2009, 21:31 
Loop with two motor and 2 sensors
By MrRoboto in forum mel PIC BASICReplies: 4Last Post:  8th December 2008, 23:40 
Making Program Code Space your playground...
By Melanie in forum Code ExamplesReplies: 15Last Post:  19th July 2008, 08:26 
4 Chanel Dmx512 ready assembly code to PBP ?
By syscoder in forum mel PIC BASIC ProReplies: 10Last Post:  21st March 2007, 23:55 
Your Suggestions: Assembly code material?
By dw_picbasic in forum GeneralReplies: 1Last Post:  2nd February 2007, 17:33
Bookmarks