; LogoChip Logo code for Lab 3
; sonar driver code by Ruslan Shamayev

constants
  [[portb 6][portb-addr $86]]

global [t1 t2 diff dval]

to sonar
  ; wait for a bus command
  waituntil [newbus?]

  ; if sonar code then proceed
  if brcv = $180 [sonar_act]

  sonar
end

to sonar_act
  sonar_init
  sonar_trig

  ; wait for the low-to-high of echo pulse
  waituntil [testbit 7 portb]
  ; sett1 timer
  resett

  ; wait for the high-to-low of echo pulse
  waituntil [not testbit 7 portb]
  sett2 timer

  ; get the difference
  ; setdiff t2 - t1
  setdiff t2
  print diff

  ; translate value to cm  div. by 29
  ; calc. one way trip, div. by 2
  ; mult. by 1000 to get microsec val 
  setdval ((((diff * 100) / 2) * 10) / 29)
  print dval

  ; send the diff value on the bus
  bsend diff
end    
  
to sonar_trig
  ; toggle output to trigger  sonar
  setbit 6 portb
  clearbit 6 portb
end

to sonar_init
  ; bit 6 of portb is output
  clearbit 6 portb-addr	
  ; bit 7 of portb is input	
  setbit 7 portb-addr  
end

to delay :n
    repeat :n [no-op]
end

; the code below this line is from the lctools file
to print :n
if :n < 0 [send 45 pn 0 - :n stop]
pn :n
cr
end

to pn :n
if :n > 9999 [pdigit :n 10000]
if :n > 999 [pdigit :n 1000]
if :n > 99 [pdigit :n 100]
if :n > 9 [pdigit :n 10]
pdigit :n 1
end

to pdigit :n :d
send ((:n / :d) % 10) + 48
end

; inserts a cariage return
to cr
send 13
end