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:
Cordic Trig Include file for PIC16
Bookmarks