hc-sr04.fs

\ Example code for using the hc-sr04 ultrasonic distance sensor
\ to measure distance in cm.
\ Requires math.txt and us.txt from flashforth repo

-hc-sr04

marker -hc-sr04

\ ports and registers
$0008 constant trig \ pin 11
$0010 constant echo \ pin 12
$0023 constant pinb
$0024 constant ddrb
$0025 constant portb

343 constant sound-speed \ speed of sound 0.0343 meters/ms
3960 constant max-range \ max range of sensor
sound-speed 10 / 2/ constant min-range \ min range of sensor

: init-hc-sr04 ( -- )
  trig ddrb mset \ set pin as output
  echo ddrb mclr \ set pin as input
;

\ toggle trigger pin
: trigger! ( -- ) portb c@ trig xor portb c! ;

\ check if echo pin is high or low
: echo? ( -- f ) pinb c@ echo and 0= if false else true then ;

\ 8 cycle pulse
: pulse! ( -- ) trigger! #10 us trigger! ;

\ Times how long it takes from echo to go from high to low in ms
\ First loop waits for echo pin to go to high and places ticks on r stack
\ Second loop waits for echo pin to go to low
: pulse-ms ( -- u )
  begin echo? dup if ticks >r then until
  begin echo? 0= until
  ticks r> -
;

\ Approx 10 microsecond resolution.
\ Estimating that the stack manipulation for the counter takes
\ 160 cycles if 16 cycles = 1 us
: pulse-10us ( -- u )
  begin echo? dup if 1 >r then until
  begin r> 1+ >r #1 us echo? 0= until
  r>
;

\ capture distance
\ calculate distance supplying time in ms and speed of sound
: dist? ( -- u ) pulse! pulse-ms sound-speed 2 */ ;

\ capture distance as a double length value
: ddist? ( -- u ) pulse! pulse-10us sound-speed um* d2/  ;

\ measure speed
: speed ( -- u ) dist? #1000 ms dist? swap - ;

\ format distance
: fmt-dist ( u -- caddr u ) 0 <# # [char] . hold #s rot sign #> ;

\ format value from double length distance value
: fmt-ddist ( d -- caddr u ) tuck dabs <# # # # [char] . hold #s rot sign #> ;

\ show distance formatted as centimeters
: show-dist ( u -- )
  dist?
  dup max-range > if ." too far " drop exit then
  dup min-range < if ." too close " drop exit then
  fmt-dist type ."  cm"
;

\ show distance for double length value as centimeters
: show-ddist ( u -- ) ddist? fmt-ddist type ." cm" ;

init-hc-sr04