; sonar code

global [sonar_dist s0_dist s1_dist s2_dist s3_dist s4_dist ping_time which_sonar sonar_req]

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

constants [[s0_trig_port 6] [s0_trig 2] [s0_echo_port 6] [s0_echo 1]]
constants [[s1_trig_port 6] [s1_trig 4] [s1_echo_port 6] [s1_echo 3]]
constants [[s2_trig_port 6] [s2_trig 6] [s2_echo_port 6] [s2_echo 5]]
constants [[s3_trig_port 7] [s3_trig 1] [s3_echo_port 7] [s3_echo 0]]
constants [[s4_trig_port 6] [s4_trig 7] [s4_echo_port 7] [s4_echo 2]]
; ---------------------------------------------

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

to cr
send 13
end


; ----------------------------------------


to init
    write portb-ddr $2B
    write portc-ddr $05
    clearbit s0_trig s0_trig_port
    clearbit s1_trig s1_trig_port
    clearbit s2_trig s2_trig_port
    clearbit s3_trig s3_trig_port
    clearbit s4_trig s4_trig_port

    setwhich_sonar 0
end

to sonar :trig :echo :trig_port :echo_port
    setping_time 1

    ; Set init bit, ping sent out when init is cleared
    setbit :trig :trig_port
    clearbit :trig :trig_port

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

    ; Wait for echo to be lowered, signalling that
    ; the ping has been received
    loop
    [
        ifelse ((testbit :echo :echo_port) < 1)
        [
	    setping_time (ping_time * 3.2)
	    stop
        ]
        [
	    setping_time (ping_time + 1)
        ]
    ]
end

to ping :trig :echo :trig_port :echo_port

    sonar :trig :echo :trig_port :echo_port

    ; sonar_dist = distance in centimeters
    setsonar_dist ((ping_time / 2) * 3.3145)

    if sonar_dist > 127
    [
        setsonar_dist 127
    ]
end

to main
    init
    loop
    [
        setbit 2 portc
        if newbus?
        [
            ; The cricket is asking us for our sensor data, determine which
            ; Sonar distance to send back.

            setsonar_req brcv
            if sonar_req = $180
            [
	        bsend s0_dist
                ;bsend 1
            ]

            if sonar_req = $181
            [
                bsend s1_dist
                ;bsend 2
            ]

            if sonar_req = $182
            [
                bsend s2_dist
                ;bsend 3
            ]

            if sonar_req = $183
            [
                bsend s3_dist
                ;bsend 4
            ]

            if sonar_req = $184
            [
                bsend s4_dist
                ;bsend 5
            ]
        ]

        ;
        ; Fake switch statement to determine which sensor to read next,
        ; round robin style
        ;
        if which_sonar = 0
        [
            ; Get a reading from Sonar 0
            ping s0_trig s0_echo s0_trig_port s0_echo_port
            sets0_dist sonar_dist
        ]

        if which_sonar = 2
        [
            ; Get a reading from Sonar 1
            ping s1_trig s1_echo s1_trig_port s1_echo_port
            sets1_dist sonar_dist
        ]

        if which_sonar = 4
        [
            ; Get a reading from Sonar 2
            ping s2_trig s2_echo s2_trig_port s2_echo_port
            sets2_dist sonar_dist
        ]

        if which_sonar = 6
        [
            ; Get a reading from Sonar 3
            ping s3_trig s3_echo s3_trig_port s3_echo_port
            sets3_dist sonar_dist
        ]

        if which_sonar = 8
        [
            ; Get a reading from sonar 4
            ping s4_trig s4_echo s4_trig_port s4_echo_port
            sets4_dist sonar_dist
        ]

        clearbit 2 portc

        ; Increment which_sonar and mod by 4 to determine which
        ; sensor to read next
        setwhich_sonar which_sonar + 1
        setwhich_sonar which_sonar % 9
    ]
end

to main_disabled
    init
    loop
    [
        ping s4_trig s4_echo s4_trig_port s4_echo_port
        print sonar_dist

        ;if newbus?
        ;[
        ;    if brcv = $180
        ;    [
        ;        bsend which_sonar
        ;        setwhich_sonar which_sonar + 1
        ;    ]
        ;]


    ]
end