fl

{
UltraSonicSensor

PropForth5.5
2013/04/28 8:19:30

P0 -- UltraSonicSensor Tranmitter
P1 -- UltraSonicSensor Receiver

}

\ a cog special register
[ifndef ctra
h1F8	wconstant ctra 
]

[ifndef frqa
h1FA	wconstant frqa
]

[ifndef phsa
h1FC	wconstant phsa
]

\ _cfo ( n1 -- n2 ) n1 - desired frequency, n2 frqa/frqb 
[ifndef _cfo
: _cfo clkfreq 1- min                        \ max 79999999Hz 
0 swap clkfreq um/mod                        \ divide n1(hi)0(lo) by clkfreq 
swap clkfreq 2/ >= abs + 
; 
]

0 wconstant ping_out
1 wconstant ping_in
d20 wconstant T                              \ Temperature 20degree

h1000_0000 ping_out + constant ping_T        \ NCO mode
h3000_0000 ping_in + constant ping_R         \ NEG Detector mode 
d331500 d600 T u* + constant sound_v         \ Sound velocity[mm/sec]

\ Get distance
\ ( -- )
: distance
ping_out pinout
cnt COG@ d4000000 +                          \ 50msec

begin
     d40000 _cfo frqa COG!                   \ Put 40kHz to UltraSonic-transmitter     
     cnt COG@ d16000 +                       \ Set start cnt
     ping_T ctra COG!
     0 waitcnt drop                          \ Wait 200usec
     0 frqa COG!                             \ Reset NCO-mode
                    
     0 phsa COG!
     1 frqa COG!
     ping_R ctra COG!
     ping_in >m ping_in >m waitpeq           \ Wait for ping_in to go high
     phsa COG@ d16000 + dup ." phsa=".

     d125 u* d10000 u/                       \ Get usec     
     sound_v u*                              \ mm/usec
     d1000000 u/                             \ mm/sec
     2/ . ." mm" cr                       
                  
     d4000000 waitcnt                        \ Wait 50msec
     fkey? swap drop
until
drop
;