;-----------------------------------------------
; Nick Pennella
; Frank Fernandes
; Robotics Assignment 5
;
; Code to get a sonar sensor in the botball kit
; (Devantech SRF04 Sensor) to work
; Now sending the result to the Cricket over the
; cricket bus
;-----------------------------------------------


;-----------------------
; Port Constants
;-----------------------

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

;-----------------------
; Global Variables
;-----------------------
global [nn start final dif]


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;   Print Functions taken from lctools    ;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

to prs :n
setnn :n
loop
  [if (read-rom nn) = 0 [stop]
   send read-rom nn
   setnn nn + 1]
end

to prh :n
prh-digit :n / 16
prh-digit :n % 16
cr
end

to prh-digit :n
ifelse :n < 10
   [send :n + 48]
   [send :n + 55]
end

to cr
send 13
end

to print :n
if :n < 0 [send 45 pn 0 - :n stop]
pn :n
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
cr
end

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

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


;--------------------------------------------------
; Function to initialize the register as an output
;--------------------------------------------------
to init
   write portb-ddr 0
end

;------------------------------------------------
; Function to send a trigger pulse to the sonar
;------------------------------------------------
to pulse
   setbit 6 portb
   clearbit 6 portb
end

;------------------------------------------------
; Main function to initiate a sonar ping
;------------------------------------------------
to ping

    ;-- Initialize the b register as an output
    init

    ;-- send a trigger pulse
    pulse


    ;-- wait for the edge of the echo pulse
    waituntil [testbit 0 portc]
    setstart timer

    ;-- wait for the other end of the echo pulse
    waituntil [ not testbit 0 portc]
    setfinal timer

    ;-- Print the difference in the times
    setdif final - start
    
    cr
    pn dif
    ;-- Distance calculated as 1inch/74milliseconds since
    ;-- it takes diff seconds round trip, we calculate the
    ;-- distance in inches as 1inch/(.074microseconds * 2 seconds)
    ;-- because its a round trip effort...
    ;pn dif * 6.75
    
    ;pn dif * 67.5
    prs "Nick
    bsend dif * 6.75 
end

;--------------------------------------------------------------
;Now the main function waits for the cricket to tell it to ping
;--------------------------------------------------------------
to slave
  waituntil [newbus?]
  if brcv = $180 [ping]
  slave
end