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)
' DB0-3 No connect
ang=5461
x=0
y=0
asm
call sin_cos
endasm
Lcdout $fe, 1 ' Clear LCD screen
'toggle portd.3
Pause 500 ' Wait for LCD to startup
lcdout $FE,1,#x,",",#y
lcdout $FE,$C0,#ang
end
'trig.inc here_________________________________________________
i var byte BANK0
j Var byte BANK0
quad var byte BANK0
x var word bank0
y var word bank0
ang var word bank0
dy var word bank0
dx var word bank0
atans var word[14] bank0
atans(0) = 16384
atans(1) = 9672
atans(2) = 5110
atans(3) = 2594
atans(4) = 1302
atans(5) = 652
atans(6) = 326
atans(7) = 163
atans(8) = 81
atans(9) = 41
atans(10) = 20
atans(11) = 10
atans(12) = 5
atans(13) = 3
atans(14) = 1
goto main
asm
;IDATA
; Table of arctan values
;atans DW D'16384', D'9672', D'5110', D'2594', D'1302', D'652', D'326', D'163'
; DW D'81', D'41', D'20', D'10', D'5', D'3', D'1'
;CODE
; Calculates the sine and cosine of the given angle
sin_cos:
; Set up the stack
;movff FSR2L, POSTINC1
;movff FSR1L, FSR2L
; Initialize _x to 18218
movlw 0x2a
movwf _x
movlw 0x47
movwf _x+1
; Initialize _y to 0
clrf _y
clrf _y+1
; Initialize _ang to passed parameter
;movlw 0xfd
;movff PLUSW2, _ang
;movlw 0xfe
;movff PLUSW2, _ang+1
; Initialize _quad to 0
clrf _quad
; Check if the angle is greater than 16383 (90°)
sc_check_greaterthan:
btfss _ang+1, 7
btfss _ang+1, 6
bra sc_check_lessthan
bra sc_adjust_quad2
; Check if the angle is less than -16384 (-90°)
sc_check_lessthan:
btfsc _ang+1, 7
btfsc _ang+1, 6
bra sc_setup_end
; If the angle is in quadrant 3, adjust it to quadrant 4
sc_adjust_quad3:
negf _ang
bc sc_negate_quad3
comf _ang+1
bra sc_adjust_end
; If the low byte negation causes a carry, negate the upper byte
sc_negate_quad3:
negf _ang+1
bra sc_adjust_end
; If the angle is in quadrant 2, adjust it to quadrant 1
sc_adjust_quad2:
comf _ang
comf _ang+1
; Toggle the sign bit and set the '_quad' flag
sc_adjust_end:
btg _ang+1, 7
setf _quad
; Multiply the angle by 2 to get better resolution
sc_setup_end:
bcf STATUS, 0
rlcf _ang
rlcf _ang+1
; Set up the main loop
sc_loop_start:
clrf _i
;CHK?RP _atans ;was banksel _atans
lfsr FSR0, _atans
; The main loop label
sc_loop:
movff _x, _dy
movff _x+1, _dy+1
movff _i, _j
movf _j
bz sc_bs_x_done
; Loop to shift _dy right
sc_bs_x_loop:
bcf STATUS, 0
rrcf _dy+1
rrcf _dy
btfsc _x+1, 7
bsf _dy+1, 7
decfsz _j
bra sc_bs_x_loop
; Calculate what needs to be added to _x
sc_bs_x_done:
movff _y, _dx
movff _y+1, _dx+1
movff _i, _j
movf _j
bz sc_do_rotation
; Loop to shift _dx right
sc_bs_y_loop:
bcf STATUS, 0
rrcf _dx+1
rrcf _dx
btfsc _y+1, 7
bsf _dx+1, 7
decfsz _j
bra sc_bs_y_loop
; Perform adding operations on _x, _y and _ang
sc_do_rotation:
btfss _ang+1, 7
bra sc_sub_angle
; If _ang is negative
movf POSTINC0, W
addwf _ang
movf POSTINC0, W
addwfc _ang+1
movf _dx, W
addwf _x
movf _dx+1, W
addwfc _x+1
movf _dy, W
subwf _y
movf _dy+1, W
subwfb _y+1
bra sc_loop_bottom
; If _ang is positive
sc_sub_angle:
movf POSTINC0, W
subwf _ang
movf POSTINC0, W
subwfb _ang+1
movf _dx, W
subwf _x
movf _dx+1, W
subwfb _x+1
movf _dy, W
addwf _y
movf _dy+1, W
addwfc _y+1
; Increment the counter and exit the loop if done
sc_loop_bottom:
incf _i
movlw 0x0f
cpfseq _i
bra sc_loop
; Negate _x if it was initially in quadrant 2 or 3
sc_finished:
btfss _quad, 7
bra sc_output
negf _x
bc sc_negate_x
comf _x+1
bra sc_output
; If the low byte negation causes a carry, negate the upper byte
sc_negate_x:
negf _x+1
; Output the calculated _x and _y values
sc_output:
;movff _y, AARGB3
;movff _y+1, AARGB3+1
;movff _x, AARGB3+2
;movff _x+1, AARGB3+3
; Restore the stack to its previous state
;movf POSTDEC1
;movff INDF1, FSR2L
return
; Calculates the magnitude and direction of the given ordered pair
atan2_sqrt:
; Set up the stack
;movff FSR2L, POSTINC1
;movff FSR1L, FSR2L
; Initialize _x to passed parameter
;movlw 0xfb
;movff PLUSW2, _x
;movlw 0xfc
;movff PLUSW2, _x+1
; movff POSTINC2, _x
; movff POSTDEC2, _x+1
; Initialize _y to passed parameter
;movlw 0xfd
;movff PLUSW2, _y
;movlw 0xfe
;movff PLUSW2, _y+1
; movlw 0x03
; movff PLUSW2, _y+1
; movlw 0x02
; movff PLUSW2, _y
; Initialize _ang to 0
clrf _ang
clrf _ang+1
; Initialize _quad to 0
clrf _quad
; If the point is in quadrant 2 or 3, make _x positive and set flag
as_check_negative:
btfss _x+1, 7
bra as_shift_x
setf _quad
negf _x
bc as_negate_x
comf _x+1
bra as_shift_x
; If the low byte negation causes a carry, negate the upper byte
as_negate_x:
negf _x+1
; Divide the _x coordinate by 2 to prevent overflowing
as_shift_x:
bcf STATUS, 0
rrcf _x+1
rrcf _x
; Divide the _y coordinate by 2 to prevent overflowing
as_shift_y:
bcf STATUS, 0
rrcf _y+1
rrcf _y
btfsc _y+1, 6
bsf _y+1, 7
; Set up the main loop
as_loop_start:
clrf _i
;CHK?RP _atans ;was banksel _atans
lfsr FSR0, _atans
; The main loop label
as_loop:
movff _x, _dy
movff _x+1, _dy+1
movff _i, _j
movf _j
bz as_bs_x_done
; Loop to shift _dy right
as_bs_x_loop:
bcf STATUS, 0
rrcf _dy+1
rrcf _dy
btfsc _x+1, 7
bsf _dy+1, 7
decfsz _j
bra as_bs_x_loop
; Calculate what needs to be added to _x
as_bs_x_done:
movff _y, _dx
movff _y+1, _dx+1
movff _i, _j
movf _j
bz as_do_rotation
; Loop to shift _dx right
as_bs_y_loop:
bcf STATUS, 0
rrcf _dx+1
rrcf _dx
btfsc _y+1, 7
bsf _dx+1, 7
decfsz _j
bra as_bs_y_loop
; Perform adding operations on _x, _y and _ang, shifting the _atans right one
as_do_rotation:
movff POSTINC0, PRODL
movff POSTINC0, PRODH
bcf STATUS, 0
rrcf PRODH
rrcf PRODL
btfsc _y+1, 7
bra as_sub_angle
; If _y is positive
movf PRODL, W
addwf _ang
movf PRODH, W
addwfc _ang+1
movf _dx, W
addwf _x
movf _dx+1, W
addwfc _x+1
movf _dy, W
subwf _y
movf _dy+1, W
subwfb _y+1
bra as_loop_bottom
; If _y is negative
as_sub_angle:
movf PRODL, W
subwf _ang
movf PRODH, W
subwfb _ang+1
movf _dx, W
subwf _x
movf _dx+1, W
subwfb _x+1
movf _dy, W
addwf _y
movf _dy+1, W
addwfc _y+1
; Increment the counter and exit the loop if done
as_loop_bottom:
incf _i
movlw 0x0e
cpfseq _i
bra as_loop
; Multiply the _x value by 19898 and divide by 2^14 to scale it
as_scale_x:
movff _x, _dx
movff _x+1, _dx+1
movlw 0xba
mulwf _dx
movff PRODH, _x
movlw 0x4d
mulwf _dx+1
movff PRODH, _dy
movff PRODL, _x+1
movlw 0xba
mulwf _dx+1
movf PRODL, W
addwf _x, F
movf PRODH, W
addwfc _x+1, F
clrf WREG
addwfc _dy, F
movlw 0x4d
mulwf _dx
movf PRODL, W
addwf _x, F
movf PRODH, W
addwfc _x+1, F
clrf WREG
addwfc _dy, F
movlw 0x06
movwf _j
as_scale_bs_loop:
bcf STATUS, 0
rrcf _dy
rrcf _x+1
rrcf _x
decfsz _j
bra as_scale_bs_loop
; Check if the quadrant was originally changed
as_check_quad:
btfss _quad, 7
bra as_output
btfss _ang+1,7
bra as_adjust_quad1
; If the angle is in quadrant 4, adjust it to quadrant 3
as_adjust_quad4:
negf _ang
bc as_negate_quad4
comf _ang+1
bra as_adjust_end
; If the low byte negation causes a carry, negate the upper byte
as_negate_quad4:
negf _ang+1
bra as_adjust_end
; If the angle is in quadrant 1, adjust it to quadrant 2
as_adjust_quad1:
comf _ang
comf _ang+1
; Toggle the sign bit
as_adjust_end:
btg _ang+1, 7
; Output the calculated angle and hypotenuse values
as_output:
;movff _ang, AARGB3
;movff _ang+1, AARGB3+1
;movff _x, AARGB3+2
;movff _x+1, AARGB3+3
; Restore the stack to its previous state
;movf POSTDEC1
;movff INDF1, FSR2L
return
endasm
Bookmarks