\ 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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment