; Richard Irons      February 25, 2003
; LogoChip control code
; Lab 5

global [ping_time sonar_dist delay_val bus_word total_sonar_dist]

constants [[portb 6][portb-ddr $86][portc 7][portc-ddr $87]]

to init
    write portb-ddr 0
end

;to click-on
;    setbit 6 portb
;end

;to click-off
;    clearbit 6 portb
;end

;to beep
;    repeat 100 [click-on delay 50 click-off delay 50]
;end

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

to red 
    setbit 5 portb
    clearbit 6 portb
end

to redoff
   clearbit 5 portb
end

to green
    clearbit 5 portb 
    setbit 6 portb
end

to greenoff
    clearbit 6 portb
end


to clear_dev_board_leds
   clearbit 1 portb
   clearbit 2 portb
   clearbit 3 portb
   clearbit 4 portb
end


; Loop and get sonar readings
to pings :num_pings

    settotal_sonar_dist 0

    repeat :num_pings
    [
       if newbus?
       [
           ; Check for command to turn sonar off
           if brcv = $181
           [
              clear_dev_board_leds
              stop
           ]
       ]

       green ; indicate that a ping is going out

        ; Set init bit, ping sent out when init is cleared
       setbit 7 portb
       clearbit 7 portb

       ; Wait for echo to be raised, signals
       ; that ping has been sent
       waituntil  [ ((testbit 0 portc) =  1) ]

       resett ; reset the timer

       ; Wait for echo to be lowered, signals
       ; that ping has been received
       waituntil  [ ((testbit 0 portc) <  1) ]

       setping_time timer ; Get timer value

       setdelay_val (ping_time / 2)

      ; repeat 100 [click-on delay ping_time click-off delay delay_val]

       ; Half time in microseconds / time of sound to travel 1 inch in microseconds
       setsonar_dist (((ping_time / 2) * 1000) /  74 )

      ; Set LED sonar distance indicators
       ifelse (sonar_dist < 48) [ setbit 1 portb ][clearbit 1 portb]  ; No more than four feet away
       ifelse (sonar_dist < 36) [ setbit 2 portb ][clearbit 2 portb]  ; No more than three feet away
       ifelse (sonar_dist < 24) [ setbit 3 portb ][clearbit 3 portb]  ; No more than two feet away
       ifelse (sonar_dist < 12) [ setbit 4 portb ][clearbit 4 portb]  ; No more than one foot away

       ;Increment contact distance accumulator
       settotal_sonar_dist (total_sonar_dist + sonar_dist)

       red ; Indicate that no reading is being taken

       wait 1
   ]

   ;send contact distance back to cricket
   bsend (total_sonar_dist / :num_pings)

   clear_dev_board_leds
end


to slave

   ; Wait for a bus command
   waituntil [newbus?]

   if brcv = $180
   [
      ; wait for a ping count
      waituntil [newbus?]
      pings brcv
   ]
   
   slave
end


; Main function called from command center
to main
  init
  clear_dev_board_leds
  red
  slave
end