Skip to content

Instantly share code, notes, and snippets.

@carlosdelfino

carlosdelfino/CORDIC_Demo.asm

Last active July 22, 2020 13:53
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save carlosdelfino/3f6c00800f48126fe223f66c6ca51d18 to your computer and use it in GitHub Desktop.
Save carlosdelfino/3f6c00800f48126fe223f66c6ca51d18 to your computer and use it in GitHub Desktop.
Download ZIP
Estudos de otimização trigonometria em C e ASM em especial Cortex-M e AVR
Raw
CORDIC_Demo.asm
;CORDIC Degrees to SIN COS (As Used On Youtube http://www.youtube.com/watch?v=Ze4UnE8R4FM)
;20x2 OLED display
;Craig Webster (IXIBA)
;Melboune Australia
;Updated 18/2/2014
;Found this usefull?
;Bitcoin: 1LqDCrj8QAUjACnjRNEw8vq3T9p2R5k5RW
;Use TAB space = 7
.nolist
.include "m88def.inc"
.list
;LCD pins ;LCD pins D0-D3 & R/W Join to gnd
.equ LCD_RS =PC0 ;LCD RS 0=control, 1=data
.equ LCD_EN =PC1 ;LCD Enable on falling edge
.equ LCD_D4 =PC2 ;LCD data bit D4 ;Bits D0-D3 not used (Tied low)
.equ LCD_D5 =PC3 ;LCD data bit D5
.equ LCD_D6 =PC4 ;LCD data bit D6
.equ LCD_D7 =PC5 ;LCD data bit D7
;Tack Switch
.equ SW_Up =PD0
.equ SW_Down =PD1
;.def =r0
;.def =r1
;.def =r2
;.def =r3
;.def =r4
;.def =r5
;.def =r6
;.def =r7
;.def =r8
;.def =r9
;.def =r10
;.def =r11
;.def =r12
;.def =r13
;.def =r14
;.def =r15
.def temp =r16
.def temp1 =r17
.def flags =r18
.def Loop_Count =r19
;.def =r20
.def Conversion_State =r21 ;Used to direct conversion of 0<>90 to 0<>360
.def Angle_L =r22 ;Angle 0000 000n nnnn nnnn . dddd dddd dddd dddd
.def Angle_H =r23
.def Angle_DPH =r24
.def Angle_DPL =r25
;r27,r26 Xh:Xl
;r29,r28 Yh:Yl
;r31,r30 Zh:Zl
;General Flags
.equ Negative =0
.equ Wait =1
;.equ =2
;.equ =3
;.equ =4
;.equ =5
;.equ =6
;.equ =7
;LCD screen positions
.equ LCD_SinPos =0x8c
.equ LCD_CosPos =0xcc
.equ LCD_AnglePOS =0xc0
;With no delay at 1mhz delay between writes is 48us
.equ LCD_120uS =40 ;Value for 120uS delay for LCD, 1Mhz=40, 2Mhz=80, 4Mhz=160
.equ LCD_600uS =50 ;Value for 600uS delay for LCD, 1Mhz=200 newhaven OLED ,SUS seems to work ok at <20us
.equ Del_15ms =15 ;Value for 15ms delay, 1Mhz=15, 2Mhz=30, 4Mhz=60, 8Mhz=120, 16Mhz=240
.equ Cordic1K =0x136E9DB5 ;Will result in a N3.Q29 number (2 guard bits)
; 0.6072529350088812561694 << 29 , Size of cordic expansion
.equ Cordic_Iterations =21 ;How many iteration in Cordic loop, Max=30, Recomended not less then 16
;(No point having more iteration than number of bits in Cordic1K)
; ;
;SIN(28.125) = 0.47331966718484338944103927991974
; 29 = 0f256f46 = 0.47331966 0
; 21 = 0f256f08 = 0.473319 545
; 20 = 0f256d45 = 0.47331 8705
; 19 = 0f2570cb = 0.4733 20385
; 18 = 0f2577d7 = 0.4733 23745
; 17 = 0f2585ef = 0.4733 30466
; 16 = 0f2569bf = 0.4733 17025
; 15 = 0f25315f = 0.473 290143
.equ Fraction_Count_Demo =0x2000 ;The fraction step for demo 0x20 = .125
;
;******************************************
;* *
;* Start/Init *
;* *
;******************************************
;
.CSEG
.org 0000
rjmp Reset
.org INT0addr ; External Interrupt Request 0
reti
.org INT1addr ; External Interrupt Request 1
reti
.org PCI0addr ; Pin Change Interrupt Request
reti
.org PCI1addr ; Pin Change Interrupt Request 0
reti
.org PCI2addr ; Pin Change Interrupt Request 1
reti
.org WDTaddr ; Watchdog Time-out Interrupt
reti
.org OC2Aaddr ; Timer/Counter2 Compare Match A
reti
.org OC2Baddr ; Timer/Counter2 Compare Match A
reti
.org OVF2addr ; Timer/Counter2 Overflow
reti
.org ICP1addr ; Timer/Counter1 Capture Event
reti
.org OC1Aaddr ; Timer/Counter1 Compare Match A
reti
.org OC1Baddr ; Timer/Counter1 Compare Match B
reti
.org OVF1addr ; Timer/Counter1 Overflow
reti
.org OC0Aaddr ; TimerCounter0 Compare Match A
reti
.org OC0Baddr ; TimerCounter0 Compare Match B
reti
.org OVF0addr ; Timer/Couner0 Overflow
reti
.org SPIaddr ; SPI Serial Transfer Complete
reti
.org URXCaddr ; USART Rx Complete
reti
.org UDREaddr ; USART, Data Register Empty
reti
.org UTXCaddr ; USART Tx Complete
reti
.org ADCCaddr ; ADC Conversion Complete
reti
.org ERDYaddr ; EEPROM Ready
reti
.org ACIaddr ; Analog Comparator
reti
.org TWIaddr ; Two-wire Serial Interface
reti
.org SPMRaddr ; Store Program Memory Read
reti
Reset: ldi temp,high(RAMEND) ;Set stack pointer
out SPH,temp
ldi temp,low(RAMEND)
out SPL,temp
;Watchdog
cli ;Disabe Watdog to prevent endless reset cycle (Code from data sheet)
wdr
in temp,MCUSR
cbr temp,1<<WDRF
out MCUSR,temp
lds temp,WDTCSR
sbr temp,(1<<WDCE | 1<<WDE)
sts WDTCSR,temp
ldi temp,0<<WDE
sts WDTCSR,temp
;CPU Clock
ldi temp,1<<CLKPCE ;Set clock prescaler
sts CLKPR,temp
ldi temp,( 0<<CLKPCE | 0<<CLKPS3 | 0<<CLKPS2 | 1<<CLKPS1 | 1<<CLKPS0)
sts CLKPR,temp ;8Mhz/8=1Mhz
; ldi temp,( 0<<CLKPCE | 0<<CLKPS3 | 0<<CLKPS2 | 1<<CLKPS1 | 0<<CLKPS0)
; sts CLKPR,temp ;8Mhz/4=2Mhz
; ldi temp,( 0<<CLKPCE | 0<<CLKPS3 | 0<<CLKPS2 | 0<<CLKPS1 | 1<<CLKPS0)
; sts CLKPR,temp ;8Mhz/2=4Mhz
;
;C Port
ldi temp,(1<<LCD_RS | 1<<LCD_EN | 1<<LCD_D4 | 1<<LCD_D5 | 1<<LCD_D6 | 1<<LCD_D7)
out ddrc,temp ;Set LCD interface pins to output
ldi temp, (1<<LCD_RS | 0<<LCD_EN)
out portc,temp ;Set initial pin val E to inactive, RS data
;
;D Port
ldi temp,(0<<SW_Down | 0<<SW_Up)
out ddrd,temp
ldi temp,(1<<SW_Down | 1<<SW_Up)
out portd,temp ;Pullups on
;Setup LCD
rcall LCD_Initialize_4bit
;
;Defaults
clr flags
cli
;
;*****************************************
;
; Main program
;
;*****************************************
;
Main:
ldi temp,0x80 ;Home Cursor, 1st line
rcall LCD_cnt
ldi temp,LOW(Line1)
rcall Send_String
ldi temp,0xc0 ;2nd line
rcall LCD_CNT
ldi temp,LOW(Line2)
rcall Send_String
ldi Angle_H,0 ;N16
ldi Angle_L,0
ldi Angle_DPH,0 ;Q16
ldi Angle_DPL,0
ldi Xl,low(Fraction_Count_Demo) ;Fraction added per loop
ldi Xh,high(Fraction_Count_Demo)
ldi temp,0 ;A zero for maths
Key_Loop:
sbis pind,SW_Down
rjmp Count_Down
sbis pind,Sw_Up
rjmp Count_Up
sbr flags,1<<Wait ;To give the effect of an Auto-repeat after button continuosly held
rjmp Key_loop
Count_Up:
add Angle_DPL,Xl
adc Angle_DPH,Xh
adc Angle_L,temp
adc Angle_H,temp
ldi temp1,high(360) ;Gone past 359.999, just reset the whole part to 0
cpi Angle_L,low(360)
cpc Angle_H,temp1
brlo DO_Conversion
ldi Angle_H,0
ldi Angle_L,0
rjmp DO_Conversion
Count_Down:
sub Angle_DPL,Xl
sbc Angle_DPH,Xh
sbc Angle_L,temp
sbc Angle_H,temp
brcc DO_Conversion
ldi Angle_H,high(360) ;Gone negative so reset to 360.00 - Fraction_Count_Demo
ldi Angle_L,low(360)
ldi Angle_DPH,0
ldi Angle_DPL,0
sub Angle_DPL,Xl
sbc Angle_DPH,Xh
sbc Angle_L,temp
sbc Angle_H,temp
rjmp DO_Conversion
DO_Conversion:
sts Cordic_Angle,Angle_H
sts Cordic_Angle+1,Angle_L
sts Cordic_Angle+2,Angle_DPH
sts Cordic_Angle+3,Angle_DPL
rcall CORDIC_Sin_Cos_Deg
rcall Display_Result
sbrs flags,Wait ;Auto-repeat Delay
rjmp Key_Loop
ldi Loop_Count,60
W_Lp:
rcall Delay15ms
dec loop_Count
brne W_Lp
cbr flags,1<<Wait
rjmp Key_Loop
;
;*****************************************
;
; CORDIC
;
;*****************************************
;
CORDIC_Sin_Cos_Deg: ;Calculate Sin Cos in Degrees using Cordic (Ver 2/8/2012)
;By Craig Webster (IXIBA)
;Cordic1K = 0x136E9DB5
;Cordic_Iterations = 16 > 30
;In: Angle N16.Q16 (32bit) (DSEG)
;Out: Sin N3.Q29S (32bit) & Cos N3.Q29S (32bit) (DSEG)
push r0 ;L TY
push r1 ; TY
push r2 ; TY
push r3 ;H TY
push r4 ;L Y
push r5 ; Y
push r6 ; Y
push r7 ;H Y
push r8 ;L Z
push r9 ; Z
push r10 ; Z
push r11 ;H Z
push r12 ;L X
push r13 ; X
push r14 ; X
push r15 ;H X
push r22 ;L TX
push r23 ; TX
push r24 ; TX
push r25 ;H TX
push Zl
push Zh
push Loop_Count
push temp
push Conversion_State
;Wrapper, Convert 0<>359 to 0<>89.9
; ;Z=Angle<<8 (N16.Q16 > N8.Q24), To have same radix as TAN Table
lds r8,(Cordic_Angle) ;Z, Needs to be shifted Left 8 bits to allighn radix
lds r11,(Cordic_Angle+1) ;so load into Z one bite over and use Lower byte r8
lds r10,(Cordic_Angle+2) ;witch will be zeroed later for high byte (Use r8:r11
lds r9,(Cordic_Angle+3) ;in quadrant determiner)
;No needed if sure to be in range
; ldi Zh,high(360) ;Sanity check, exit if >=360 deg
; ldi Zl,low(360)
; cp r11,Zl
; cpc r8,Zh
; brlo CSCD_InRange
; sec ;Return Carry=1, Error
; rjmp CSCD_X
;Conversion state=1 0=>,<90 deg ,Sin=Sin(n) ,Cos=Cos(n)
;Conversion state=2 90=>,<180 deg ,Sin=Cos(n) ,Cos=Inv(Sin(n))
;Conversion state=3 180=>,<270 deg,Sin=Inv(Sin(n)) ,Cos=Inv(Cos(n))
;Conversion state=4 270=>,<360 deg,Sin=Inv(Cos(n)) ,Cos=Sin(n)
CSCD_InRange:
ldi Zl,90
ldi Zh,0
ldi Conversion_State,0
CSCD_State_Lp:
inc Conversion_State
sub r11,Zl
sbc r8,Zh
brcc CSCD_State_Lp
add r11,Zl
adc r8,Zh
CSCD_Do_Conv:
;Game the system slightly, the first time through X=Y=Cordic1K, Preloading TX=TY=Cordic1K and
;juping in on +Z branch before Z adjust will skip some code & X,Y initilization
ldi r22,low(Cordic1K) ;TX=Cordic1K
ldi r23,byte2(Cordic1K)
ldi r24,byte3(Cordic1K)
ldi r25,byte4(Cordic1K)
movw r1:r0,r23:r22 ;TY=TX
movw r3:r2,r25:r24
ldi Zh,high(ArcTan_Tab<<1) ;Arctan table
ldi Zl,low(ArcTan_Tab<<1)
ldi Loop_Count,0
rjmp CSCD_Z_First ;Skip >>0
CSCD_Shift: ;Y & X >>Loop_Count
mov temp,Loop_Count
CSCD_Shift_Lp:
cpi temp,8 ;If >>8 or more then do whole byte shift
brsh CSCD_Shift_Byte
CSCD_Shift_Bit: ;Bit shift, must be less than 8 so run down to 0
asr r7 ;Y>>1
ror r6
ror r5
ror r4
asr r15 ;X>>1
ror r14
ror r13
ror r12
dec temp
brne CSCD_Shift_Bit
rjmp CSCD_Z_Sighn
CSCD_Shift_Byte: ;Shift whole byte to avoid 8 shifts
mov r4,r5 ;Y>>8
mov r5,r6
mov r6,r7
clr r7
sbrc r6,7 ;Extend sighn
com r7
mov r12,r13 ;X>>8
mov r13,r14
mov r14,r15
clr r15
sbrc r14,7 ;Extend sighn
com r15
subi temp,8
brne CSCD_Shift_Lp
CSCD_Z_Sighn:
sbrs r11,7 ;Branch on Z sighn bit
rjmp CSCD_Z_Pos
;Z is Negative
add r22,r4 ;TX += Y
adc r23,r5
adc r24,r6
adc r25,r7
sub r0,r12 ;TY -= X
sbc r1,r13
sbc r2,r14
sbc r3,r15
lpm r4,Z+ ;Y=(Arctab+), Use Y as temp storage
lpm r5,Z+
lpm r6,Z+
lpm r7,Z+
add r8,r4 ;Z += Y
adc r9,r5
adc r10,r6
adc r11,r7
rjmp CSCD_Finalize
CSCD_Z_Pos:
;Z is Positive
sub r22,r4 ;TX -= Y
sbc r23,r5
sbc r24,r6
sbc r25,r7
add r0,r12 ;TY += X
adc r1,r13
adc r2,r14
adc r3,r15
CSCD_Z_First:
lpm r4,Z+ ;Y=(Arctab+), Use Y as temp storage
lpm r5,Z+
lpm r6,Z+
lpm r7,Z+
sub r8,r4 ;Z -= Y
sbc r9,r5
sbc r10,r6
sbc r11,r7
CSCD_Finalize:
movw r5:r4,r1:r0 ;Y = TY
movw r7:r6,r3:r2
movw r13:r12,r23:r22 ;X = TX
movw r15:r14,r25:r24
inc Loop_Count
cpi Loop_Count,Cordic_Iterations
brsh CSCD_End_State
rjmp CSCD_Shift
;Done so use Conversion_State to adjust to correct quadrant
CSCD_End_State:
cpi Conversion_State,1 ;Sin=Sin(n), Cos=Cos(n)
brne CSCD_End_State_Setup_Inveres
rjmp CSCD_Done
CSCD_End_State_Setup_Inveres:
ldi temp,0xff
com r12 ;X will be the inv Cos(n)
com r13
com r14
com r15
sub r12,temp ;2's comp using sub -1 (FFFFFF)
sbc r13,temp
sbc r14,temp
sbc r15,temp
com r4 ;Y will be the inv Sin(n)
com r5
com r6
com r7
sub r4,temp
sbc r5,temp
sbc r6,temp
sbc r7,temp
CSCD_End_State2:
cpi Conversion_State,2 ;Sin=Cos(n), Cos=Inv(Sin(n))
brne CSCD_End_State3
mov r0,r22 ;Sin=Cos(n)
mov r1,r23
mov r2,r24
mov r3,r25
mov r22,r4 ;Cos=Inv(Sin(n))
mov r23,r5
mov r24,r6
mov r25,r7
rjmp CSCD_Done
CSCD_End_State3:
cpi Conversion_State,3 ;Sin=Inv(Sin(n)), Cos=Inv(Cos(n))
brne CSCD_End_State4
mov r0,r4 ;Sin=Inv(Sin(n))
mov r1,r5
mov r2,r6
mov r3,r7
mov r22,r12 ;Cos=Inv(Cos(n))
mov r23,r13
mov r24,r14
mov r25,r15
rjmp CSCD_Done
CSCD_End_State4: ;Sin=Inv(Cos(n)), Cos=Sin(n)
mov r22,r0 ;Cos=Sin(n)
mov r23,r1
mov r24,r2
mov r25,r3
mov r0,r12 ;Sin=Inv(Cos(n))
mov r1,r13
mov r2,r14
mov r3,r15
CSCD_Done:
;Last 2 bit rubbish
sts Cordic_Sin,r3 ;Sin 32bit N3.Q29S
sts Cordic_Sin+1,r2
sts Cordic_Sin+2,r1
sts Cordic_Sin+3,r0
sts Cordic_Cos,r25 ;Cos 32bit N3.Q29S
sts Cordic_Cos+1,r24
sts Cordic_Cos+2,r23
sts Cordic_Cos+3,r22
clc ;Retun Carry=0, no error
CSCD_X:pop Conversion_State
pop temp
pop Loop_Count
pop Zh
pop Zl
pop r25
pop r24
pop r23
pop r22
pop r15
pop r14
pop r13
pop r12
pop r11
pop r10
pop r9
pop r8
pop r7
pop r6
pop r5
pop r4
pop r3
pop r2
pop r1
pop r0
ret
;
;*****************************************
;
;Display Stuff
;
;*****************************************
;
Display_Result: ;Display Thata,Sin ,Cos
;In: Angle,Cordic_Sin (DSEG),Cordic_Cos (DSEG)
;Out: LCD
push Xh
push Xl
push Yh
push Yl
push Zh
push Zl
push temp
ldi temp,LCD_AnglePOS ;Display Angle
rcall LCD_cnt
movw Yh:Yl,Angle_H:Angle_L
rcall Hex_Dec_999LZ
ldi temp,'.'
rcall LCD_Dat
clr Yh
clr Yl
sbrs Angle_DPH,7 ;MSB worth 500
rjmp DR_AB2
ldi Yh,high(500)
ldi Yl,low(500)
DR_AB2:
ldi Xh,high(250)
ldi Xl,low(250)
sbrs angle_DPH,6 ;Worth 250
rjmp DR_AB3
add Yl,Xl
adc Yh,Xh
DR_AB3:
ldi Xh,high(125)
ldi Xl,low(125)
sbrs angle_DPH,5 ;Worth 125
rjmp DR_ABX
add Yl,Xl
adc Yh,Xh
DR_ABX:
rcall Hex_Dec_999
ldi temp,LCD_CosPos ;Display COS
rcall LCD_cnt
lds Xh,Cordic_Cos
lds Xl,Cordic_Cos+1
lds Zh,Cordic_Cos+2
lds Zl,Cordic_Cos+3
asr Xh ;Aligh fractinal part to byte N3.Q29 > N8.Q24 (preserve sighn)
ror Xl
ror Zh
ror Zl ;N4
asr Xh
ror Xl
ror Zh
ror Zl ;N5
asr Xh
ror Xl
ror Zh
ror Zl ;N6
asr Xh
ror Xl
ror Zh
ror Zl ;N7
asr Xh
ror Xl
ror Zh
ror Zl ;N8
cbr flags,1<<Negative ;Convert negative numers to positive, flag to indicate action
sbrs Xh,7
rjmp DR_CosPos
com Zl
com Zh
com Xl
com Xh
subi Zl,0xff ;2's comp using sub -1 (FFFF)
sbci Zh,0xff
sbci Xl,0xff
sbci Xh,0xff
sbr flags,1<<Negative ;Set flag to indicate its a negative num
DR_CosPos:
sts Bin_Frac,Xl
sts Bin_Frac+1,Zh
sts Bin_Frac+2,Zl
rcall BinFrac_To_BCD
ldi temp,' '
sbrc flags,Negative
ldi temp,'-'
rcall LCD_dat
ori Xh,0x30
mov temp,Xh
rcall LCD_dat
ldi temp,'.'
rcall LCD_dat
lds Xh,Bin_Frac_BCD
ldi temp,0xf0
and temp,Xh
swap temp
ori temp,0x30
rcall LCD_dat
ldi temp,0x0f
and temp,Xh
ori temp,0x30
rcall LCD_dat
lds Xh,Bin_Frac_BCD+1
ldi temp,0xf0
and temp,Xh
swap temp
ori temp,0x30
rcall LCD_dat
ldi temp,0x0f
and temp,Xh
ori temp,0x30
rcall LCD_dat
lds Xh,Bin_Frac_BCD+2
ldi temp,0xf0
and temp,Xh
swap temp
ori temp,0x30
rcall LCD_dat
ldi temp,LCD_SinPos ;Display Sin
rcall LCD_cnt
lds Xh,Cordic_Sin
lds Xl,Cordic_Sin+1
lds Zh,Cordic_Sin+2
lds Zl,Cordic_Sin+3
asr Xh ;Aligh fractional part to byte N3.Q29 > N8.Q24 (preserve sighn)
ror Xl
ror Zh
ror Zl ;N4
asr Xh
ror Xl
ror Zh
ror Zl ;N5
asr Xh
ror Xl
ror Zh
ror Zl ;N6
asr Xh
ror Xl
ror Zh
ror Zl ;N7
asr Xh
ror Xl
ror Zh
ror Zl ;N8
cbr flags,1<<Negative ;Convert negative numers to positive, flag to indicate action
sbrs Xh,7
rjmp DR_SinPos
com Zl
com Zh
com Xl
com Xh
subi Zl,0xff ;2's comp using sub -1 (FFFF)
sbci Zh,0xff
sbci Xl,0xff
sbci Xh,0xff
sbr flags,1<<Negative ;Flag to indicate its a negative number
DR_SinPos:
sts Bin_Frac,Xl
sts Bin_Frac+1,Zh
sts Bin_Frac+2,Zl
rcall BinFrac_To_BCD
ldi temp,' '
sbrc flags,Negative
ldi temp,'-'
rcall LCD_dat
ori Xh,0x30
mov temp,Xh
rcall LCD_dat
ldi temp,'.'
rcall LCD_dat
lds Xh,Bin_Frac_BCD
ldi temp,0xf0
and temp,Xh
swap temp
ori temp,0x30
rcall LCD_dat
ldi temp,0x0f
and temp,Xh
ori temp,0x30
rcall LCD_dat
lds Xh,Bin_Frac_BCD+1
ldi temp,0xf0
and temp,Xh
swap temp
ori temp,0x30
rcall LCD_dat
ldi temp,0x0f
and temp,Xh
ori temp,0x30
rcall LCD_dat
lds Xh,Bin_Frac_BCD+2
ldi temp,0xf0
and temp,Xh
swap temp
ori temp,0x30
rcall LCD_dat
DR_X: pop temp
pop Zl
pop Zh
pop Yl
pop Yh
pop Xl
pop Xh
ret
;
;
BinFrac_To_BCD: ;Convert Binary Fraction (Q24) to BCD *10 method (16 Digit) (Ver 29/10/2012)
;Removed trailing zero suppresion in this version, hope nuffin broken
;Reduce to 6 digits, no need to waist time doing all 16
;Convert to accept 24bit in (was 16 originaly)
;In: Bin_Frac (Q24)
;Out: Bin_Frac_BCD (upto 16 digits)
push r0
push r1
push r2
push r3
push r4
push r5
push Xl
push Xh
push Yl
push Yh
push Zl
push Zh
push Loop_Count
push temp
ldi Xh,high(Bin_Frac_BCD)
ldi Xl,low(Bin_Frac_BCD)
ldi temp,0
sts Bin_Frac_BCD,temp
sts Bin_Frac_BCD+1,temp
sts Bin_Frac_BCD+2,temp
sts Bin_Frac_BCD+3,temp
sts Bin_Frac_BCD+4,temp
sts Bin_Frac_BCD+5,temp
sts Bin_Frac_BCD+6,temp
sts Bin_Frac_BCD+7,temp
lds Zl,Bin_Frac
lds Yh,Bin_Frac+1
lds Yl,Bin_Frac+2
ldi Zh,10 ;For Mult 10
ldi temp,0 ;Build 2 BCD digits
ldi Loop_Count,6 ;6 BCD digits
BTB_LP:
clr r4 ;Multiply Zl:Yh:Yl * 10(Zh) = r5:r4:r3:r2 (Mul8x24_32U)
clr r5
mul Zh,Yl
movw r3:r2,r1:r0
mul Zh,Yh
add r3,r0
adc r4,r1
adc r5,r5 ;r5=0
mul Zh,Zl
add r4,r0
adc r5,r1
movw Yh:Yl,r3:r2 ;Keep lower 24 bits for next cycle
mov Zl,r4
swap temp
or temp,r5 ;r5 should only be 0<>9
sbrs Loop_Count,0 ;Store BCD evey 2nd cycle
rjmp BTB_CY1
st X+,temp
mov temp,Yh ;If zero then finish early
or temp,Yl
or temp,Zl
breq BTB_X
ldi temp,0
BTB_CY1:
dec Loop_Count
brne BTB_LP
BTB_X: pop temp
pop Loop_Count
pop Zh
pop Zl
pop Yh
pop Yl
pop Xh
pop Xl
pop r5
pop r4
pop r3
pop r2
pop r1
pop r0
ret
;
;
Hex_Dec_999: ;Display 0<>999
;In: Yh:Yl
;Out: LCD
push temp
push Loop_Count
ldi temp,high(1000) ;Check if within 0<>999
cpi Yl,low(1000)
cpc Yh,temp
brlo HDa999_100
ldi temp,'E'
rcall LCD_dat
ldi temp,'E'
rcall LCD_dat
ldi temp,'E'
rcall LCD_dat
rjmp HDa999S_X
HDa999_100:
clr Loop_Count
HDa999S_100L:
subi Yl,low(100) ;How many 100's
sbci Yh,high(100)
inc Loop_Count
brcc HDa999S_100L
dec Loop_Count
subi Yl,low(-100)
sbci Yh,high(-100)
ldi temp,0x30
or temp,Loop_Count
rcall LCD_dat
clr Loop_Count
HDa999S_10L:
subi Yl,low(10) ;How many 10's
sbci Yh,high(10)
inc Loop_Count
brcc HDa999S_10L
dec Loop_Count
subi Yl,low(-10)
sbci Yh,high(-10)
ldi temp,0x30
or temp,Loop_Count
rcall LCD_dat
ldi temp,0x30
or temp,Yl
rcall LCD_dat
HDa999S_X:
pop Loop_Count
pop temp
ret
;
;
Hex_Dec_999LZ: ;Display 0<>999 supress leading Zeros(Uses T bit!!!)
;In: Yh:Yl
;Out: LCD
push temp
push Loop_Count
set ;Use Tbit ripple zero blank
HD999S_Pos:
ldi temp,high(1000) ;Check if within 0<>999
cpi Yl,low(1000)
cpc Yh,temp
brlo HD999_100
ldi temp,'E'
rcall LCD_dat
ldi temp,'E'
rcall LCD_dat
ldi temp,'E'
rcall LCD_dat
rjmp HD999S_X
HD999_100:
clr Loop_Count
HD999S_100L:
subi Yl,low(100) ;How many 100's
sbci Yh,high(100)
inc Loop_Count
brcc HD999S_100L
dec Loop_Count
subi Yl,low(-100)
sbci Yh,high(-100)
tst Loop_Count
brne HD999_Show100
ldi temp,' '
rcall LCD_dat
rjmp HD999_10
HD999_Show100:
ldi temp,0x30
or temp,Loop_Count
rcall LCD_dat
clt ;Stop blank ripple
HD999_10:
clr Loop_Count
HD999S_10L:
subi Yl,low(10) ;How many 10's
sbci Yh,high(10)
inc Loop_Count
brcc HD999S_10L
dec Loop_Count
subi Yl,low(-10)
sbci Yh,high(-10)
brtc HD999_Show10
tst Loop_Count
brne HD999_Show10
ldi temp,' '
rcall LCD_dat
rjmp HD999S_Units
HD999_Show10:
ldi temp,0x30
or temp,Loop_Count
rcall LCD_dat
HD999S_Units:
ldi temp,0x30
or temp,Yl
rcall LCD_dat
HD999S_X:
pop Loop_Count
pop temp
ret
;
;
Send_String:
push Zl
push Zh
ldi Zh,high(LCD_Tables) ;All have same high byte addres
ldi ZL,low(LCD_Tables)
add Zl,temp
ldi temp,0
adc Zh,temp
lsl Zl
rol Zh
SS_Lp: lpm temp,z+ ;Loop until null (0x00)
tst temp
breq SS_End
rcall LCD_dat
rjmp SS_Lp
SS_End:
pop Zh
pop Zl
ret
;
;*****************************************
;
; LCD interface
;
;*****************************************
;
LCD_Initialize_4bit: ;Initialize LCD to 4 bit mode (OLED)
rcall Delay15ms
rcall Delay15ms
;Function Set
ldi temp,(0<<LCD_RS | 0<<LCD_EN | 0<<LCD_D7 | 0<<LCD_D6 | 1<<LCD_D5 | 1<<LCD_D4) ;8bit mode
out portc, temp
sbi portc, LCD_EN ;In worst case, in 4 bit mode expecting second nibble of data
nop
nop
nop
nop
nop
cbi portc, LCD_EN
rcall Delay15ms
sbi portc, LCD_EN ;In worst case, in 4 bit mode, 1st nibble of camand that will tell it to switch to 8bit mode
nop
nop
nop
nop
nop
cbi portc, LCD_EN
rcall Delay15ms
sbi portc, LCD_EN
nop
nop
nop
nop
nop
cbi portc, LCD_EN ;In worst case, will be in 8 bit mode now
ldi temp,0xff
LI_L1: dec temp
brne LI_L1
;Function Set
ldi temp,(0<<LCD_RS | 0<<LCD_EN | 0<<LCD_D7 | 0<<LCD_D6 | 1<<LCD_D5 | 0<<LCD_D4) ;4 Bit mode
out portc, temp
sbi portc, LCD_EN
nop
nop
nop
nop
nop
cbi portc, LCD_EN
rcall Delay15ms
sbi portc, LCD_EN ;Now in 4bit mode
nop
nop
nop
nop
nop
cbi portc, LCD_EN
ldi temp,0xff
LI_L2: dec temp
brne LI_L2
ldi temp,(0<<LCD_RS | 0<<LCD_EN | 1<<LCD_D7 | 0<<LCD_D6 | 0<<LCD_D5 | 0<<LCD_D4)
out portc, temp
sbi portc, LCD_EN
nop
nop
nop
nop
nop
cbi portc, LCD_EN
ldi temp,0x08 ;Display Off
rcall LCD_cnt
ldi temp,0x01 ;Display Clear
rcall LCD_cnt
rcall Delay15ms
ldi temp,0x06 ;Entry Mode Set
rcall LCD_cnt
ldi temp,0x02 ;Home Command
rcall LCD_cnt
ldi temp,0x10 ;Cursor/Shift
rcall LCD_cnt
ldi temp,0x0c ;Display On
rcall LCD_cnt
ret
;
;
LCD_cnt:
push temp1
ldi temp1,0<<LCD_RS ;RS=0 control
rjmp LCD_S
LCD_dat:
push temp1
ldi temp1,1<<LCD_RS ;RS=1 Data
LCD_S: push temp
cbr temp,0x0f ;Upper nibble
lsr temp
lsr temp
or temp1,temp
out portc,temp1
sbi portc,LCD_EN ;Pulse Enable
nop
nop
nop
nop
nop
cbi portc,LCD_EN
pop temp ;Lower nibble
cbr temp,0xf0
lsl temp
lsl temp
cbr temp1,0xfc ;Preserve control bits
or temp1,temp
out portc,temp1
sbi portc,LCD_EN ;Pulse Enable
nop
nop
nop
nop
nop
cbi portc,LCD_EN
ldi temp1,LCD_600uS
LCD_L2:dec temp1
brne LCD_L2
pop temp1
ret
;
;
Delay15ms:
push temp
push temp1
ldi temp1,Del_15ms
Del_L1:clr temp
Del_L2:dec temp
nop
brne Del_L2
dec temp1
brne Del_L1
pop temp1
pop temp
ret
end:
.org (end & 0xff00) + 0x100
tables:
;Atan(1)=45 > 45 * 0x01000000 = 0x2D000000
ArcTan_Tab: .dd 0x2D000000,0x1A90A731,0x0E094740,0x07200112 ;30
.dd 0x03938AA6,0x01CA3794,0x00E52A1A,0x007296D7
.dd 0x00394BA5,0x001CA5D9,0x000E52ED,0x00072976
.dd 0x000394BB,0x0001CA5D,0x0000E52E,0x00007297
.dd 0x0000394B,0x00001CA5,0x00000E52,0x00000729
.dd 0x00000394,0x000001CA,0x000000E5,0x00000072
.dd 0x00000039,0x0000001C,0x0000000E,0x00000007
.dd 0x00000003,0x00000001
endARC:
.org (endARC & 0xff00) + 0x100
LCD_Tables:
Line1: .db " SIN:--.-----",0,0
Line2: .db "---.---",0xdf,"COS:--.-----",0,0
.DSEG
;Cordic Transfer:
Cordic_Angle: ;Angle in 32bit N16.Q16 (0.0 > 359.0)
.byte 4
Cordic_Sin: ;Sin out, 32bit N3.Q29 ,-1.0 <> 1.0 = F00000 <> 100000, =n * 0x100000
.byte 4
Cordic_Cos:
.byte 4 ;Cos out, 32bit N3.Q29, -1.0 <> 1.0 = F00000 <> 100000, =n * 0x100000
;Binary fraction to BCD
Bin_Frac: ;Q24, Fraction to be converted to Decimal
.byte 3
Bin_Frac_BCD: ;Upto 16 BCD digits
.byte 8
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment