Efficient Fixed-Point Trigonometry For PIC16F


+ Reply to Thread
Results 1 to 13 of 13
  1. #1
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default Efficient Fixed-Point Trigonometry For PIC16F

    Microchip has an application note AN1061 http://ww1.microchip.com/downloads/e...tes/01061A.pdf which talks about using the ATAN function, among other things. I can't wait to use it for a project I am working on ........ I just need to know how. .....

    I am a newbie when it comes to adding includes. I tried a few different ways to add the assembly code for the atan, and I could not get it to compile. I tried using the math16.asm file as an include in a new main program, I also tried cutting and pasting the assembly into a main.pbp program with asm and endasm lines on the front and back. Can someone steer me in the right direction? This cordic function would be perfect for me.

    I get the following error when trying to compile with MPASM using MicroCode Studio Plus: Error[149] Directive only allowed when generating an object file.
    I get this for multiple line listings of the assembly code, but this does not seem to make sense for the lines it references. Like

    movlw 0x00 ; Y_H & Y_L is loaded with 0x0000

    Thanks for your help.

    Walter
    Attached Files Attached Files
    Last edited by ScaleRobotics; - 15th November 2007 at 16:47.

  2. #2
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,959

    Default

    Hi Walter,

    The MATH16.ASM file looks interesting.
    It's written as Relocatable Object Code, so isn't directly compatible with PBP.

    But with a bit of work you could modify it.

    Statements like UDATA, CODE , GLOBAL , and END get removed, or commented.

    Names with RES are variables, so they need to be defined in Basic.
    Something like ...
    Code:
    X    VAR WORD
    @X_H = _X + 1 
    @X_L = _X
    And some of the labels need to be renamed.
    DIV and MUL already exist is PBP.
    DT

  3. #3
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default

    Thanks for the pointers. I was able to get ATAN working! Wooohooo! I must have done something to the sincos function though. I get the same result, no matter what variable I enter into x. It is supposed to cos the value of x and give the result in y, and sin the value of x, and give the result in z. I'm a little brain dead right now, so I will have to check it further in the morning.

    Code:
    ;****************************************************************************
    ;*	Microchip Technology Inc. 2006					*
    ;*	Filename: Main.asm									*
    ;*																*
    ;****************************************************************************
    
    DEFINE __16F877A 1				; list directive to define processor
    DEFINE OSC 20                          ' Define Oscillator Speed
    DEFINE LOADER_USED 1
    
    ' 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_H         VAR     byte
    ITERATION  VAR      byte
    X_L         VAR     byte
    Y_H         VAR     byte
    Y_L         VAR     byte
    Z_H         VAR     byte
    Z_L         VAR     byte
    X_TMP_H     VAR     byte
    X_TMP_L     VAR     byte
    Y_TMP_H     VAR     byte
    Y_TMP_L     VAR     byte
    ITERAT_TMP  VAR     byte
    Di          VAR     byte
    COSCH       VAR     byte
    COSCL       VAR     byte
    Counter1    VAR     byte
    SHIFT       VAR     byte
    x           var     word
    y           var     word
    z           var     word
    i           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
    portb.0 = 0
    pause 500
    lcdout $FE,1,"Test"
    portb.0 = 1
    
    Start:
    x = 250
    y = 0
    
    X_L = x.byte0
    X_H = x.byte1
    
    Y_L = y.byte0             'load values of y into Y_L ,just for atan, but does not hurt to leave 
    Y_H = y.byte1             'dito
    
    call sincos
    y.byte0 = Y_L             ' edit out if looking at atan
    y.byte1 = Y_H              'same here
    
    'Call atan
    
    
    z.byte0 = Z_L
    z.byte1 = Z_H
    'z=(z*100)/284                                       'corrects our output to an angle in degrees for atan
    'lcdout $FE,1,#z    
    
    
    
    portb.0 = 0
    lcdout $FE,1,#y
    lcdout $FE,$C0,#z 
    end '***********************************
    
    atan:
        asm
            call ATAN
        endasm
    return
    
    sincos:
        asm
            call SINCOS
        endasm
    return
    
    asm
    ;__________________________________________________________________________
    FSR_ptr	macro	addr
    	banksel	addr
    	bcf		STATUS,IRP
    	btfsc		STATUS,RP1
    	bsf		STATUS,IRP
    	movlw		LOW addr
    	movwf		FSR
    			endm
    
    	#define 	Dir	_Di,0
    	#define 	RV		_Di,1
    	#define	_C		STATUS,0
    	#define	_Z		STATUS,2
    
    Z_stepH_tbl
    	addwf	PCL,f
    	dt	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
    Z_stepL_tbl
    	addwf	PCL,f
    	dt	0x80,0x4C,0x28,0x14,0x0A,0x05,0x03,0x01
    Z_NstepH_tbl
    	addwf	PCL,f
    	dt	0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
    Z_NstepL_tbl
    	addwf	PCL,f
    	dt	0x80,0xB4,0xD8,0xEC,0xF6,0xFB,0xFD,0xFF
    
    ATAN
    	banksel	   _Z_H				; Z_H & Z_L is loaded with 0x0000
    	movlw		0x00				; Y & X is the input
    	movwf		_Z_H				; X(i+1)	=	Xi - Yi*_Di*2^-i
    	movlw		0x00				; Y(I+1)	=	Yi + Xi*_Di*2^-i
    	movwf		_Z_L				; Z(i+1)	=	Zi - _Di*ATAN(2^-i)
    	banksel	   _Di					; _Di		=	1 if Yi < 0 ELSE _Di	=	0
    	bsf		      RV					; Set Vectoring Mode
    	goto		CORDIC_Alg		
    
    SINCOS
    	banksel	   _Y_L				; Z_H & Z_L is the input
    	movlw		0x00				; Y_H & Y_L is loaded with 0x0000	
    	movwf		_Y_L				; X_H & X_L is loaded with 6070 (1/An constant)
    	movlw		0x00			
    	movwf		_Y_H			
    	movlw		LOW .6070	
    	movwf		_X_L				; X(i+1)	=	Xi - Yi*_Di*2^-i
    	movlw		HIGH .6070		; Y(I+1)	=	Yi + Xi*_Di*2^-i
    	movwf		_X_H				; Z(i+1)	=	Zi - _Di*ATAN(2^-i)
    	banksel	   _Di					; _Di		=	0 if Zi < 0 ELSE _Di	=	1
    	bcf		      RV					; Set Rotational Mode
    
    CORDIC_Alg
    	banksel	_ITERATION
    	clrf	_ITERATION
    Rotation_loop						
    BUFF_X_Y							; Buffers both X & Y into temporary registers 
    	banksel 	_Y_L
    	movf		_Y_L, W			; Move Y --> T_TEMP
    	movwf		_Y_TMP_L
    	movf		_Y_H, W
    	movwf		_Y_TMP_H
    	movf		_X_L, W			; Move Y --> T_TEMP
    	movwf		_X_TMP_L
    	movf		_X_H, W
    	movwf		_X_TMP_H
    
    	banksel	   _Di					; Vectoring or Rotational Mode?
    	bcf		      Dir
    	btfsc		RV
    	goto		Rot
    Vec
    	btfss		_Z_H, 7			; Determine the state of _Di (1 OR -1)
    	bsf		  Dir				; _Di=1 when Z_H is pos
    	goto		Cordic
    Rot
    	btfsc		_Y_H, 7			; Determine the state of _Di (1 OR -1)
    	bsf		  Dir				; _Di=1 when Y_H is neg
    
    Cordic
    	movfw		_ITERATION
    	movwf		_SHIFT				; Load number of shifts
    	bsf		      _SHIFT,7			; Shift right (Division)
    	FSR_ptr	   _Y_TMP_L			; Load FSR
    	call		Shifter			; Execute shift
    
    	btfsc		Dir
    	call		COM_YTEMP		; Get the 2's complement of teh TEMP register 
    SUM_X								; Calculate X(i+1)
    	movf		_Y_TMP_H, W		
    	addwf		_X_H, F
    	movf		_Y_TMP_L, W
    	addwf		_X_L, F
    	btfsc		_C
    	incf		_X_H, F
    	
    	movfw		_ITERATION
    	movwf		_SHIFT				; Load number of shifts
    	bsf		  _SHIFT,7			; Shift right (Division)
    	FSR_ptr	   _X_TMP_L			; Load FSR
    	call		Shifter			; Execute shift
    
    	btfss		Dir
    	call		COM_XTEMP		; Get the 2's complement of teh TEMP register
    SUM_Y								; Calulate Y(i+1)
    	movf		_X_TMP_H, W		
    	addwf		_Y_H, F
    	movf		_X_TMP_L, W
    	addwf		_Y_L, F
    	btfsc		_C
    	incf		_Y_H, F
    
    	btfss		Dir
    	goto		ADD_Z
    
    	movlw		HIGH Z_NstepH_tbl	
    	movwf		PCLATH
     	movfw		_ITERATION		; Fetch appropriate value from table
    	call		Z_NstepH_tbl	
    	addwf		_Z_H, F
    	movlw		HIGH Z_NstepL_tbl
    	movwf		PCLATH
     	movfw		_ITERATION
    	call		Z_NstepL_tbl	
    	addwf		_Z_L, F
    	btfsc		_C
    	incf		_Z_H, F
    	goto		Rot_Loop_out
    
    ADD_Z
    	movlw		HIGH Z_stepL_tbl
    	movwf		PCLATH
     	movfw		_ITERATION
    	call		Z_stepH_tbl
    	addwf		_Z_H, F
    	movlw		HIGH Z_stepH_tbl
    	movwf		PCLATH
     	movfw		_ITERATION
    	call		Z_stepL_tbl
    	addwf		_Z_L, F
    	btfsc		_C
    	incf		_Z_H, F
    
    Rot_Loop_out
    	incf		_ITERATION,f
    	btfsc		_ITERATION,3
    	return
    	goto		Rotation_loop
    
    COM_YTEMP
    	comf		_Y_TMP_L, F		; Complememt Y_TMP
    	comf		_Y_TMP_H, F
    	movlw		01H
    	addwf		_Y_TMP_L, F
    	btfsc		_C
    	incf		_Y_TMP_H, F
    	return 
    COM_XTEMP
    	comf		_X_TMP_L, F		; Complememt X_TMP
    	comf		_X_TMP_H, F
    	movlw		01H
    	addwf		_X_TMP_L, F
    	btfsc		_C
    	incf		_X_TMP_H, F
    	return
    
    Shifter							; Function for shifting 16bit signed REG i times 
    	banksel	   _SHIFT				;  left/right MUL/DIV
    	movfw		_SHIFT				; INPUTS: FSR & IRP are pointing to LOW REG
    	andlw		0x0F				; SHIFT = d000 XXXX where XXXX = # of shifts 
    	btfsc		_Z					; 	and d = dir of shift
    	return						; check for zero interations
    	movwf		_Counter1
    	btfss		_SHIFT,0x7
    	goto		CORDIC_MUL
    	
    CORDIC_DIV
    	rrf		INDF, F			; Divide INDF/2 INDF --> low byte
    	bcf		INDF, 7
    	decf 		FSR, F
    	rrf		INDF, F	 
    	incf 		FSR, F
    	btfsc		_C
    	bsf		INDF, 7			; Carry from the upper byte to the lower byte
    	decf 		FSR, F
    	bcf		INDF, 7			; Check to see if TEMP should be a positive number
    	btfsc		INDF, 6
    	bsf		INDF, 7			; Was a negative number, force it negative again 
    	incf 		FSR, F
    	decfsz	_Counter1, F
    	goto		CORDIC_DIV
    	return
    	
    CORDIC_MUL
    	bcf		_C
    	rlf		INDF, F
    	decf 		FSR, F
    	rlf		INDF, F
    	bcf		INDF, 7
    	btfsc		_C
    	bsf		INDF, 7
    	incf 		FSR, F
    	decfsz	_Counter1, F
    	goto		CORDIC_MUL
    	return
    ;______________________________________________________________________________
    endasm
    Last edited by ScaleRobotics; - 16th November 2007 at 10:40.

  4. #4
    Join Date
    Sep 2004
    Location
    montreal, canada
    Posts
    6,898

    Default

    Carefull when using Banksel and PBP, i would suggest you to replace all BANKSEL for CHK?RP

    CHK?RP is to PBP what BANKSEL is to ASM.

    Using BANKSEL will make weird things and lost compiler because he don't know where he was before etc etc... i understand why... i just can't explain it correctly.

    Darrel could you shed some lights and explain it in english
    Steve

    It's not a bug, it's a random feature.
    There's no problem, only learning opportunities.

  5. #5
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default

    Thanks a lot mister_e,

    I added your suggestion, which will probably help in the long run. However, I still get the same result for Cos(x) = 71 no matter what I give as x. When edited out of some of the lines that had "global" in them, as suggested in an earlier post, was I supposed to replace "global" with something else? Here are some of the lines I deleted from various places in the program:

    GLOBAL X_L, X_H, Y_L, Y_H, Z_L, Z_H, SHIFT
    GLOBAL ATAN
    GLOBAL SINCOS
    GLOBAL Shifter

    I don't really understand what these lines did in the original assembly code.
    I also wonder if I defined any of the variables wrong. RV, Dir, _C, and _Z are defined inside the assembly code, and the other variables are named in the pbp code. Strange that the ATAN functions great, just the SINCOS having difficulties.

  6. #6
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,959

    Default

    The Input to SINCOS is the Z variable. (Not X)

    I've got it working here, but it seems like it's not right.
    Input range is from -256 to +255.

    I've plotted the results. Does it look right to you?

    <img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2133&stc=1&d=119533895 6">
    <br>
    Attached Images Attached Images  
    DT

  7. #7
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default

    Actually, it is perfect!!!!
    They do something weird to signify the angle. For actual angle z, degrees = z * 90/256, or z/2.844444444444 = degrees. Output/1000 = the fractional result.

    Attached is part of their excel spread sheet showing inputs and outputs vs actuals.
    <img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2134&d=1195310172">
    Many thanks for your time,

    Walter
    Attached Images Attached Images  
    Last edited by ScaleRobotics; - 17th November 2007 at 15:17.

  8. #8
    Join Date
    Jul 2003
    Location
    Colorado Springs
    Posts
    4,959

    Default

    For actual angle z, degrees = z * 90/256
    That helps! Now the SIN and COS results all fall into place.

    But, still having problems with the ATAN function.
    Can't seem to get anything meaningfull out of it.
    And that spreadsheet won't run for me, so I can't tell what it's supposed to do.

    Do you have any examples for that too?
    <br>
    DT

  9. #9
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default

    I have another excel example that I will post (when I get to my other computer).This one can't look at it either. But basically, it works like this:

    Enter X and Y co-ordinates like x = 12631 and y = 11881, then perform atan.

    <img src="http://www.picbasic.co.uk/forum/attachment.php?attachmentid=2136&d=1195349246">

    The accuracy shows that when either x or y is really low, that errors in the way it simplifies the equation are a bit large. But with higher numbers, the accuracy is within one degree or so.

    Result pops out as Z. Z is the angle to the co-ordinates. (if I remember right the answer was 134?), but it is in the same weird format. Z * 90 / 256 gets you back to a readable degree format, which if I remember right give you about 47 degrees. You will have to know what quadrant you are in to figure out if you add 90 degrees to result by figuring out if x or y or both are positive or negative.

    I made the 20 mhz PIC do 10,000 of these calculations, and it took less than 5 seconds. Over 2000 per second! Pretty cool!
    I wish I got this excited about trig 22 years ago, when I was taking the class ......
    Attached Images Attached Images  
    Last edited by ScaleRobotics; - 18th November 2007 at 01:35.

  10. #10
    Join Date
    Sep 2004
    Location
    montreal, canada
    Posts
    6,898

    Default

    looking at the asm... i'm aware that my asm skills sucks, but, i don't understand why they used the line in red...
    Code:
    ATAN
    	banksel	   _Z_H				; Z_H & Z_L is loaded with 0x0000
    	movlw		0x00				; Y & X is the input
    	movwf		_Z_H				; X(i+1)	=	Xi - Yi*_Di*2^-i
    	movlw		0x00				; Y(I+1)	=	Yi + Xi*_Di*2^-i
    	movwf		_Z_L				; Z(i+1)	=	Zi - _Di*ATAN(2^-i)
    	banksel	   _Di					; _Di		=	1 if Yi < 0 ELSE _Di	=	0
    	bsf		      RV					; Set Vectoring Mode
    	goto		CORDIC_Alg		
    
    SINCOS
    	banksel	   _Y_L				; Z_H & Z_L is the input
    	movlw		0x00				; Y_H & Y_L is loaded with 0x0000	
    	movwf		_Y_L				; X_H & X_L is loaded with 6070 (1/An constant)
    	movlw		0x00			
    	movwf		_Y_H			
    	movlw		LOW .6070	
    	movwf		_X_L				; X(i+1)	=	Xi - Yi*_Di*2^-i
    	movlw		HIGH .6070		; Y(I+1)	=	Yi + Xi*_Di*2^-i
    	movwf		_X_H				; Z(i+1)	=	Zi - _Di*ATAN(2^-i)
    	banksel	   _Di					; _Di		=	0 if Zi < 0 ELSE _Di	=	1
    	bcf		      RV					; Set Rotational Mode
    w already hold 0x00 and they reload it again for the next MOVWF? BTW i would use CLRF instead... no big deal, it remove 4 instructions... long run, it may decrease a little bit the calculation latency. OK OK no big improvement, but... i couldn't resist. maybe i'll find something else?

    so, once modified, the above code section should look like...
    Code:
    ATAN
    	CHK?RP	_Z_H		; Z_H & Z_L is loaded with 0x0000
    				; Y & X is the input
    	clrf	_Z_H		; X(i+1) = Xi - Yi*_Di*2^-i
    				; Y(I+1) = Yi + Xi*_Di*2^-i
    	clrf	_Z_L		; Z(i+1) = Zi - _Di*ATAN(2^-i)
    	CHK?RP	_Di		; _Di	 = 1 if Yi < 0 ELSE _Di	= 0
    	bsf	RV		; Set Vectoring Mode
    	goto	CORDIC_Alg		
    
    SINCOS
    	CHK?RP	_Y_L		; Z_H & Z_L is the input
    				; Y_H & Y_L is loaded with 0x0000	
    	clrf	_Y_L		; X_H & X_L is loaded with 6070 (1/An constant)
    				
    	clrf	_Y_H	
    	movlw	LOW .6070
    	movwf	_X_L		; X(i+1) = Xi - Yi*_Di*2^-i
    	movlw	HIGH .6070	; Y(I+1) = Yi + Xi*_Di*2^-i
    	movwf	_X_H		; Z(i+1) = Zi - _Di*ATAN(2^-i)
    	CHK?RP	_Di		; _Di	 = 0 if Zi < 0 ELSE _Di	= 1
    	bcf	RV		; Set Rotational Mode
    But yeah, i think we're not at the optimization stage yet
    Last edited by mister_e; - 18th November 2007 at 02:34.
    Steve

    It's not a bug, it's a random feature.
    There's no problem, only learning opportunities.

  11. #11
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default Cordic Trig Include file for PIC16

    Looks great! Good catch.

    Here is a paste able version of the code to integrate into picbasic. (just takes up about 180 words). It loads the bytes needed for assembly. It uses X_ Y_ and Z_ word variables to load the bytes needed for the assembly routine.

    Code:
    '****************************************************************
    '*  Name    : trig-pic16.inc                                    *
    '*  Author  : Walter Dunckel                                    *
    '*  Notice  : Copyright (c) 2009 Scale Robotics Inc.            *
    '*          : All Rights Reserved                               *
    '*  Date    : 9/6/2011                                          *
    '*  Version : 2.1                                               *
    '*  Notes   : Consolodated some variables to save ram and space *
    '*          :                                                   *
    '* to convert degrees to radians Z_ * 256 / 90                  *
    '* to convert radians to degrees Z_ * 90 / 256                  *
    '*                                                              *
    '* to perform ATAN, enter values for X_ and Y_ :                *
    '* X_ = 12631                                                   *
    '* Y_ = 11881                                                   *
    '* call ATAN                                                    *
    '* 'result will be in Z_ radians = 134                          *
    '* Z_ * 90/256 = 47 degrees                                     *
    '*                                                              *
    '* To find the result of Cos and Sin:                           *
    '*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  *
    '* 'results will be located here: cos(Z_) = X_ :sin(Z_) = Y_    *
    '* in thousandths, for instance 9999 ~ 1.000                    *
    '****************************************************************
    'more info: http://www.picbasic.co.uk/forum/showthread.php?t=7502
    'and PIC18 version http://www.picbasic.co.uk/forum/showthread.php?t=10528
    
    X_          var     word bank0
    Y_          var     word bank0
    Z_          var     word bank0
    X_H         VAR     X_.byte1 
    X_L         VAR     X_.byte0
    Y_H         VAR     Y_.byte1
    Y_L         VAR     Y_.byte0
    Z_H         VAR     Z_.byte1
    Z_L         VAR     Z_.byte0
    X_TMP_H     VAR     byte bank0
    X_TMP_L     VAR     byte bank0
    Y_TMP_H     VAR     byte bank0
    Y_TMP_L     VAR     byte bank0
    ITERAT_TMP  VAR     byte bank0
    ITERATION   VAR     byte bank0
    Di          VAR     byte bank0
    COSCH       VAR     byte bank0
    COSCL       VAR     byte bank0
    Counter1    VAR     byte bank0
    SHIFT       VAR     byte bank0
    
    
    '***********************************
    goto jump_trig
    
    
    asm
    ;__________________________________________________________________________
    FSR_ptr macro addr
           CHK?RP          addr
           bcf             STATUS,IRP
           btfsc           STATUS,RP1
           bsf             STATUS,IRP
           movlw           LOW addr
           movwf           FSR
        endm
    
           #define     Dir _Di,0
           #define     RV          _Di,1
           #define     _C          STATUS,0
           #define     _Z          STATUS,2
    
    Z_stepH_tbl
           addwf   PCL,f
           dt      0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
    Z_stepL_tbl
           addwf   PCL,f
           dt      0x80,0x4C,0x28,0x14,0x0A,0x05,0x03,0x01
    Z_NstepH_tbl
           addwf   PCL,f
           dt      0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
    Z_NstepL_tbl
           addwf   PCL,f
           dt      0x80,0xB4,0xD8,0xEC,0xF6,0xFB,0xFD,0xFF
    
    _ATAN
           CHK?RP          _Z_H            ; Z_H & Z_L is loaded with 0x0000
           clrf            _Z_H            ; Y & X is the input
           clrf            _Z_L            ; X(i+1)    =   Xi - Yi*_Di*2^-i
                                           ; Y(I+1)    =   Yi + Xi*_Di*2^-i
                                           ; Z(i+1) =  Zi - _Di*ATAN(2^-i)
           CHK?RP          _Di                 ; _Di = 1 if Yi < 0 ELSE _Di = 0
           bsf             RV          ; Set Vectoring Mode
           goto            CORDIC_Alg
    _SINCOS
           CHK?RP          _Y_L     ; Z_H & Z_L is the input
           clrf            _Y_L     ; Y_H & Y_L is loaded with 0x0000
           clrf            _Y_H     ; X_H & X_L is loaded with 6070 (1/An constant)
           movlw           LOW .6070
           movwf           _X_L       ; X(i+1)        =       Xi - Yi*_Di*2^-i
           movlw           HIGH .6070 ; Y(I+1)        =       Yi + Xi*_Di*2^-i
           movwf           _X_H       ; Z(i+1)        =       Zi - _Di*ATAN(2^-i)
           CHK?RP          _Di            ; _Di   =       0 if Zi < 0 ELSE _Di =  1
           bcf             RV     ; Set Rotational Mode
    CORDIC_Alg
           CHK?RP          _ITERATION
           clrf            _ITERATION
    Rotation_loop
    BUFF_X_Y                          ; Buffers both X & Y into temporary registers
           CHK?RP          _Y_L
           movf            _Y_L, W    ; Move Y --> T_TEMP
           movwf           _Y_TMP_L
           movf            _Y_H, W
           movwf           _Y_TMP_H
           movf            _X_L, W    ; Move Y --> T_TEMP
           movwf           _X_TMP_L
           movf            _X_H, W
           movwf           _X_TMP_H
           CHK?RP          _Di            ; Vectoring or Rotational Mode?
           bcf             Dir
           btfsc           RV
           goto            Rot
    Vec
           btfss           _Z_H, 7    ; Determine the state of _Di (1 OR -1)
           bsf             Dir    ; _Di=1 when Z_H is pos
           goto            Cordic
    Rot
           btfsc           _Y_H, 7    ; Determine the state of _Di (1 OR -1)
           bsf             Dir    ; _Di=1 when Y_H is neg
    Cordic
           movfw           _ITERATION
           movwf           _SHIFT     ; Load number of shifts
           bsf             _SHIFT,7 ; Shift right (Division)
           FSR_ptr         _Y_TMP_L       ; Load FSR
           call            Shifter    ; Execute shift
           btfsc           Dir
           call            COM_YTEMP  ; Get the 2's complement of teh TEMP register
    SUM_X                             ; Calculate X(i+1)
           movf            _Y_TMP_H, W
           addwf           _X_H, F
           movf            _Y_TMP_L, W
           addwf           _X_L, F
           btfsc           _C
           incf            _X_H, F
           movfw           _ITERATION
           movwf           _SHIFT                  ; Load number of shifts
           bsf             _SHIFT,7                ; Shift right (Division)
           FSR_ptr         _X_TMP_L                ; Load FSR
           call            Shifter                 ; Execute shift
           btfss           Dir
           call            COM_XTEMP  ; Get the 2's complement of teh TEMP register
    SUM_Y                                          ; Calulate Y(i+1)
           movf            _X_TMP_H, W
           addwf           _Y_H, F
           movf            _X_TMP_L, W
           addwf           _Y_L, F
           btfsc           _C
           incf            _Y_H, F
           btfss           Dir
           goto            ADD_Z
           movlw           HIGH Z_NstepH_tbl
           movwf           PCLATH
           movfw           _ITERATION       ; Fetch appropriate value from table
           call            Z_NstepH_tbl
           addwf           _Z_H, F
           movlw           HIGH Z_NstepL_tbl
           movwf           PCLATH
           movfw           _ITERATION
           call            Z_NstepL_tbl
           addwf           _Z_L, F
           btfsc           _C
           incf            _Z_H, F
           goto            Rot_Loop_out
    
    ADD_Z
           movlw           HIGH Z_stepL_tbl
           movwf           PCLATH
           movfw           _ITERATION
           call            Z_stepH_tbl
           addwf           _Z_H, F
           movlw           HIGH Z_stepH_tbl
           movwf           PCLATH
           movfw           _ITERATION
           call            Z_stepL_tbl
           addwf           _Z_L, F
           btfsc           _C
           incf            _Z_H, F
    Rot_Loop_out
           incf            _ITERATION,f
           btfsc           _ITERATION,3
           return
           goto            Rotation_loop
    COM_YTEMP
           comf            _Y_TMP_L, F             ; Complememt Y_TMP
           comf            _Y_TMP_H, F
           movlw           01H
           addwf           _Y_TMP_L, F
           btfsc           _C
           incf            _Y_TMP_H, F
           return
    COM_XTEMP
           comf            _X_TMP_L, F             ; Complememt X_TMP
           comf            _X_TMP_H, F
           movlw           01H
           addwf           _X_TMP_L, F
           btfsc           _C
           incf            _X_TMP_H, F
           return
    Shifter                        ; Function for shifting 16bit signed REG i times
           CHK?RP          _SHIFT      ;  left/right MUL/DIV
           movfw           _SHIFT      ; INPUTS: FSR & IRP are pointing to LOW REG
           andlw           0x0F        ; SHIFT = d000 XXXX where XXXX = # of shifts
           btfsc           _Z          ;       and d = dir of shift
           return                      ; check for zero interations
           movwf           _Counter1
           btfss           _SHIFT,0x7
           goto            CORDIC_MUL
    
    CORDIC_DIV
           rrf             INDF, F   ; Divide INDF/2 INDF --> low byte
           bcf             INDF, 7
           decf            FSR, F
           rrf             INDF, F
           incf            FSR, F
           btfsc           _C
           bsf             INDF, 7   ; Carry from the upper byte to the lower byte
           decf            FSR, F
           bcf             INDF, 7 ;Check to see if TEMP should be positive number
           btfsc           INDF, 6
           bsf             INDF, 7 ;Was a negative number, force it negative again
           incf            FSR, F
           decfsz          _Counter1, F
           goto            CORDIC_DIV
           return
    
    CORDIC_MUL
           bcf             _C
           rlf             INDF, F
           decf            FSR, F
           rlf             INDF, F
           bcf             INDF, 7
           btfsc           _C
           bsf             INDF, 7
           incf            FSR, F
           decfsz          _Counter1, F
           goto            CORDIC_MUL
           return
    ;______________________________________________________________________________
    endasm
    jump_trig:
    Attached Files Attached Files
    Last edited by ScaleRobotics; - 7th September 2011 at 18:09. Reason: Cleared up code a little, and added description

  12. #12
    Join Date
    Feb 2006
    Location
    Gilroy, CA
    Posts
    1,530

    Default

    Then you can do some pretty cool stuff!

    Like find the bearing to and distance to a lat-lon waypoint (solving for latitude adjustment of Longitude!)

    Here is my ugly code. Needs to be cleaned up ... but it works for waypoints that are within 3.2000 degrees of eachother (200 miles or so). Angle still needs to be corrected depending which quadrant the waypoint lies.

    Code:
    DEFINE __16F877A 1				; list directive to define processor
    DEFINE OSC 20                          ' Define Oscillator Speed
    DEFINE LOADER_USED 1
    
    ' 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
    X_H         VAR     byte
    X_L         VAR     byte
    Y_H         VAR     byte
    Y_L         VAR     byte
    Z_H         VAR     byte
    Z_L         VAR     byte
    X_TMP_H     VAR     byte
    X_TMP_L     VAR     byte
    Y_TMP_H     VAR     byte
    Y_TMP_L     VAR     byte
    ITERAT_TMP  VAR     byte
    ITERATION  VAR      byte
    Di          VAR     byte
    COSCH       VAR     byte
    COSCL       VAR     byte
    Counter1    VAR     byte
    SHIFT       VAR     byte
    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
    
    pause 500
    lcdout $FE,1,"Test"
    
    ' to convert degrees to radians Z_ * 256 / 90
    ' to convert radians to degrees Z_ * 90 / 256
    Start:
    x = 0
    y = 0
    z = 37              'input current latitude 37.xxx for longitude correction (lon smaller at higher latitude)
    lat_dif = 11881         'difference between current position and waypoint in latitude 1.1881 deg
    lon_dif = 15816         'difference between current position and waypoint in longitude 1.5816 deg
    i = z * 256 / 90        'changes z to the right format +256 to -256
    'each 1/3 degree should equal about 1 radian need to figure out how to get this accuracy......
    if (z*256)//90 >= 45 then i = i +1        'if answer is .5 or more, go to next highest integer (round number)
    Z_ = i                    'put result in Z_
    Call sincos
    i = X_ * lon_dif        ' X_ = cos(lat_current)
    lon_dif = div32 10000      ' get the value of lon_dif * the fraction for lon correction (reduces lon_dif)
    
    X_ = lon_dif 'load lon_dif into X_ , prepare for ATAN function
    Y_ = lat_dif 'load lat_dif into Y_ , prepare for ATAN function
    
    call atan                        'perform ATAN assembly function
    
    angle = Z_
    angle_deg = Z_ *90/256          'puts angle in degrees into the variable
    
    'Below code computes distance between waypoints
    'Z_ is already loaded with angle from above result
    
    call sincos
    i = lon_dif * 10000         'get lon_dif ready to divide by fraction
    distance = div32 X_       'divide by cos(Z_)gives latitude distance in d.dddd or really ddddd
    distance = distance * 69  'get miles units
    distance = div32 100      'hundreths of miles    xx.xx miles
    
    lcdout $FE,1,#distance                                          
    lcdout $FE,$C0,#angle_deg 'not corrected for quadrant, ie. + 180, or + 90, etc. only bare triangle angle
    end '***********************************
    Last edited by ScaleRobotics; - 18th November 2007 at 16:49.

  13. #13
    Join Date
    Dec 2008
    Posts
    16

    Default

    For anyone interested, the latest code was posted by Scalerobotics here:

    http://sites.picbasic.net/index.php?...d=91&pageid=62

    (the downloads don't seem to be working right now)

    I believe the 16 bit code as posted on the above link did need the following minor tweaks:

    Instead of:

    lathome_min = lathome_min + 33*((((gpsdata[char_A+4] -48)*1000)+((gpsdata[char_A+5]-48)*100)+((gpsdata[char_A+7]-48)*10)+(gpsdata[char_A+8]-48))*5)//3

    However, I believe it should read as follows so that the modulus is calculated first:

    lathome_min = lathome_min + 33*(((((gpsdata[char_A+4] -48)*1000)+((gpsdata[char_A+5]-48)*100)+((gpsdata[char_A+7]-48)*10)+(gpsdata[char_A+8]-48))*5)//3)


    Second:

    latdest_min = latdest_min + (tempbyte*33) 'use remainder and add to latdest_min

    should read as follows:

    latdest_min = latdest_min*5/3 + (tempbyte*33) 'use remainder and add to latdest_min

Posting Permissions

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