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
Bookmarks