roboforum.ru

$regfile = "m8def.dat"
'$crystal = 26601712
$crystal = 16000000
'----------------------KONFIGURACJA--------------------
Const Falloff = 1                                           'iloњж cykli po jakim sіupki opadn№, wolne opadanie
Const Lcd_offset = 1
Const Czulosc = 20
'----------------------STAЈE---------------------------

Const Timer1_h = _xtal / 44000
Const Timer1_l = _xtal / 2000
Reset Watchdog
Config Watchdog = 256
Stop Watchdog
Const Poziom_a = 8 -(czulosc * 0.4)


'------------------------ZMIENNE--------------------------
Dim Div As Iram Integer At 16
Dim K As Byte
Dim I As Byte
Dim Tmp_s As Integer
Dim Tmp_c As Integer
Dim Beta As Iram Byte
Dim Suma As Word
Dim Sam As Byte
Dim Sampling As Bit
Dim Rex_t As Integer , Imx_t As Integer
Dim Dane(32) As Integer
Dim Sample_h(32) As Integer
Dim Sample_l(32) As Integer
Dim Okno(32) As Byte
Dim Rex(16) As Integer
Dim Sinus(40) As Integer
Dim Wynik(16) As Byte , Wynik_o(16) As Byte
Dim Sing As Single
Dim Poziom As Integer
Dim Line1 As String * 16 , Line1d(16) As Byte At Line1 Overlay
Dim Line2 As String * 16 , Line2d(16) As Byte At Line2 Overlay
Dim Falloff_count(16) As Byte




$lib "lcd4busy.lbx"
Const _lcdport = Portd
Const _lcdddr = Ddrd
Const _lcdin = Pind
Const _lcd_e = 3
Const _lcd_rw = 2
Const _lcd_rs = 1
Config Lcd = 16 * 2
Initlcd
Waitms 10
Initlcd
Cls
Deflcdchar 1 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 31
Deflcdchar 2 , 0 , 0 , 0 , 0 , 0 , 0 , 31 , 31
Deflcdchar 3 , 0 , 0 , 0 , 0 , 0 , 31 , 31 , 31
Deflcdchar 4 , 0 , 0 , 0 , 0 , 31 , 31 , 31 , 31
Deflcdchar 5 , 0 , 0 , 0 , 31 , 31 , 31 , 31 , 31
Deflcdchar 6 , 0 , 0 , 31 , 31 , 31 , 31 , 31 , 31
Deflcdchar 7 , 0 , 31 , 31 , 31 , 31 , 31 , 31 , 31
Cls
Cursor Off


Config Adc = Single , Prescaler = 4 , Reference = Avcc
Start Adc

Config Timer1 = Timer , Prescale = 1
On Compare1a Sampleh
On Compare1b Samplel
Compare1a = Timer1_h
Compare1b = Timer1_l
Enable Timer1
Start Timer1

Enable Interrupts
Disable Int0
Disable Int1

For K = 0 To 31
Sinus(k + 1) = Lookup(k , Tab_sin)
   'Okno(k + 1) = 255
   'Okno(k + 1) = Lookup(k , Okno_blackman)
   'Okno(k + 1) = Lookup(k , Okno_hamming)
   Okno(k + 1) = Lookup(k , Okno_hanning)
Next K

Sinus(33) = Lookup(0 , Tab_sin)
Sinus(34) = Lookup(1 , Tab_sin)
Sinus(35) = Lookup(2 , Tab_sin)
Sinus(36) = Lookup(3 , Tab_sin)
Sinus(37) = Lookup(4 , Tab_sin)
Sinus(38) = Lookup(5 , Tab_sin)
Sinus(39) = Lookup(6 , Tab_sin)
Sinus(40) = Lookup(7 , Tab_sin)


Start Watchdog




Main:
Do
Reset Watchdog

Gosub Sample_h
Main_1:
If Sampling = 1 Then Goto Main_1

Gosub Sample_l
Gosub Copy_low
Gosub Dft
Gosub Przelicz_low
Gosub Copy_high
Gosub Dft
Gosub Przelicz_high

Main_3:
If Sampling = 1 Then Goto Main_3
Gosub Save
Loop







Copy_high:
   Poziom = 0

   For K = 1 To 32
      Div = Sample_h(k)
      asr r17:ror r16:asr r17:ror r16
      Sample_h(k) = Div
      Poziom = Poziom + Sample_h(k)
   Next K

   Div = Poziom
   asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
   asr r17:ror r16
   Poziom = Div

   For K = 1 To 32
      Dane(k) = Sample_h(k) - Poziom
      Dane(k) = Dane(k) * Okno(k)
      Div = Dane(k)
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      Dane(k) = Div
      If Dane(k) > 127 Then Dane(k) = 127
      If Dane(k) < -127 Then Dane(k) = -127
   Next K
Return



Copy_low:
   Poziom = 0

   For K = 1 To 32
      Div = Sample_l(k)
      asr r17:ror r16:asr r17:ror r16
      Sample_l(k) = Div
      Poziom = Poziom + Sample_l(k)
   Next K

   Div = Poziom
   asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
   asr r17:ror r16
   Poziom = Div

   For K = 1 To 32
      Dane(k) = Sample_l(k) - Poziom
      Dane(k) = Dane(k) * Okno(k)
      Div = Dane(k)
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      Dane(k) = Div
      If Dane(k) > 127 Then Dane(k) = 127
      If Dane(k) < -127 Then Dane(k) = -127
   Next K
Return




Dft:
For K = 1 To 15
Rex_t = 0
Imx_t = 0

   For I = 0 To 31
      Beta = I * K
      Beta = Beta And 31
      Tmp_s = Sinus(beta + 1) * Dane(i + 1)
      Tmp_c = Sinus(beta + 8) * Dane(i + 1)

      Div = Tmp_s
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      Tmp_s = Div

      Div = Tmp_c
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      asr r17:ror r16:asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
      Tmp_c = Div


     Rex_t = Rex_t + Tmp_c
     Imx_t = Imx_t - Tmp_s

   Next I

   Div = Rex_t
   asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
   Rex_t = Div

   Div = Imx_t
   asr r17:ror r16:asr r17:ror r16:asr r17:ror r16
   Imx_t = Div

   Tmp_c = Rex_t * Rex_t
   Tmp_s = Imx_t * Imx_t


   Tmp_c = Tmp_c + Tmp_s
   Rex(k + 1) = Sqr(tmp_c)
Next K
Return



Sample_l:
'pobiera 32 prуbki z czкstotliwoњci№ 2kHz
Config Adc = Single , Prescaler = Auto , Reference = Avcc
Start Adc

Timer1 = 0
Enable Compare1b
Sam = 0
Sampling = 1
Return


Samplel:
Timer1 = 0
Incr Sam
Sample_l(sam) = Getadc(1)                                   'NIЇSZE PASMO
If Sam = 32 Then
   Disable Compare1b
   Sampling = 0
End If
Return




Sample_h:
'pobiera 32 prуbki z czкstotliwoњci№ 44kHz
Config Adc = Single , Prescaler = 2 , Reference = Avcc
'ADC dziaіa juї doњж niestabilnie na preskalerze 2 ale na 4 juї sie nie wyrobi i prubkuje z f=37kHz
' przez duїe f pojawiaja sie szumy jak podajemy sygnaі z generatora
Start Adc

Timer1 = 0
Enable Compare1a
Sam = 0
Sampling = 1
Return

Sampleh:
Timer1 = 0
Incr Sam
Sample_h(sam) = Getadc(0)                                   'WYЇSZE PASMO
If Sam = 32 Then
   Disable Compare1a
   Sampling = 0
End If
Return





'---------OBLICZANIE WYSOKOЊCI DLA HIGH (6-10)--------------


Przelicz_high:
Suma = Rex(3)
Wynik(9) = Suma

Suma = Rex(4)
Wynik(10) = Suma

Suma = Rex(5)
Wynik(11) = Suma

Suma = Rex(6)
Wynik(12) = Suma

Suma = Rex(7)
If Rex(8) > Suma Then Suma = Rex(8)
Wynik(13) = Suma

Suma = Rex(9)
If Rex(10) > Suma Then Suma = Rex(10)
Wynik(14) = Suma

Suma = Rex(11)
If Rex(12) > Suma Then Suma = Rex(12)
If Rex(13) > Suma Then Suma = Rex(13)
Wynik(15) = Suma

Suma = Rex(14)
If Rex(15) > Suma Then Suma = Rex(15)
If Rex(16) > Suma Then Suma = Rex(16)
Wynik(16) = Suma
Return


'---------OBLICZANIE WYSOKOЊCI DLA LOW (1-5)--------------


Przelicz_low:
Suma = Rex(2)
Wynik(1) = Suma

Suma = Rex(3)
Wynik(2) = Suma

Suma = Rex(4)
Wynik(3) = Suma

Suma = Rex(5)
If Rex(6) > Suma Then Suma = Rex(6)
Wynik(4) = Suma

Suma = Rex(7)
If Rex(8) > Suma Then Suma = Rex(8)
Wynik(5) = Suma

Suma = Rex(9)
If Rex(10) > Suma Then Suma = Rex(10)
Wynik(6) = Suma

Suma = Rex(11)
If Rex(12) > Suma Then Suma = Rex(12)
If Rex(13) > Suma Then Suma = Rex(13)
Wynik(7) = Suma

Suma = Rex(14)
If Rex(15) > Suma Then Suma = Rex(15)
If Rex(16) > Suma Then Suma = Rex(16)
Wynik(8) = Suma
Return




Save:


'-------------OBLICZANIE KOLEJNYCH (K) SЈUPKУW---------------

For K = 1 To 16                                             'w ci№gu jednej pкtli obliczany i wyњwietlany 1 sіupek 1/10
Sing = Wynik(k) * 0.1

'If K = 1 Then Sing = Sing * 0.5                             ' umnie jakos zawysoko skacz№, byж moze przez brak filtrуw mozna usunaж
'If K = 2 Then Sing = Sing * 0.75


Sing = Log10(sing)

Sing = Czulosc * Sing
Tmp_c = Sing + Poziom_a




'-----------PILNOWANIE GRANICY SЈUPKУW---------------

If Tmp_c < 0 Then Tmp_c = 0
If Tmp_c > 16 Then Tmp_c = 16

Wynik(k) = Tmp_c                                            'przeniesienie wyniku z TMP_C do zmiennej WYNIK


If Wynik(k) > Wynik_o(k) Then
   Wynik_o(k) = Wynik(k)
Else
   If Falloff_count(k) = Falloff Then
      If Wynik_o(k) > 0 Then Decr Wynik_o(k)
      Falloff_count(k) = 0
   End If
   Incr Falloff_count(k)
End If

Line1d(k) = Lookup(wynik_o(k) , L1)
Line2d(k) = Lookup(wynik_o(k) , L2)

Next K



'-------------WЈASNE WYЊWIETLANIE LCD---------
Locate 1 , Lcd_offset
Lcd Line1;
Locate 2 , Lcd_offset
Lcd Line2;


Return

L1:
Data 32 , 32 , 32 , 32 , 32 , 32 , 32 , 32 , 32 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 255

L2:
Data 32 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 255 , 255 , 255 , 255 , 255 , 255 , 255 , 255 , 255



' tabela 32 liczb 16 bitowych ze znakiem
' zaіoїony format: MSB - czкњж caіkowita, LSB - czкњж uіamkowa
' kwantyzacja: 0.19635 rad
' tabela I жwiartki sin(x), 8 pozycji (co 11,25 stopnia od 0 do 78.75)
Tab_sin:
Data 0% , 50% , 98% , 142% , 181% , 213% , 237% , 251%
' tabela II жwiartki sin(x), (co 11,25 stopnia od 90 do 178.75)
Data 255% , 251% , 237% , 213% , 181% , 142% , 98% , 50%
' tabela III i IV жwiartki sin(x), (co 11,25 stopnia od 180 do 258.75 i 270 - 348.75)
' zanegowane 2 poprzednie tabele
Data 0% , -50% , -98% , -142% , -181% , -213% , -237% , -251%
Data -255% , -251% , -237% , -213% , -181% , -142% , -98% , -50%



Okno_hanning:
Data 0 , 3 , 10 , 23 , 40 , 60 , 84 , 109 , 134 , 160 , 184 , 206 , 225 , 240 , 250 , 255 , 255 , 250 , 240 , 225 , 206 , 184 , 160 , 134 , 109 , 84 , 60 , 40 , 23 , 10 , 3 , 0

Okno_hamming:
Data 20 , 23 , 30 , 42 , 57 , 76 , 97 , 120 , 144 , 168 , 190 , 210 , 228 , 241 , 251 , 255 , 255 , 251 , 241 , 228 , 210 , 190 , 168 , 144 , 120 , 97 , 76 , 57 , 42 , 30 , 23 , 20

Okno_blackman:
Data 0 , 1 , 4 , 10 , 18 , 31 , 48 , 69 , 94 , 122 , 151 , 181 , 208 , 230 , 246 , 255 , 255 , 246 , 230 , 208 , 181 , 151 , 122 , 94 , 69 , 48 , 31 , 18 , 10 , 4 , 1 , 0