Not a good attempt at the logochip

global [total_val total_move_ave ptr temp]

to init_SRF05
setbit 2 portc-ddr ; set pin C2 as input
setbit 7 portb-ddr ; set pin B7 as input ***
setbit 7 portb ; ***
write CCP1CON #00000100 ; turn on capture/compare module
; see spec sheet pg. 129 for T3CON settings:
setbit 0 T3CON ; turn on timer 3
setbit 6 T3CON ; use timer3 as clock source for capture/compare
setbit 5 T3CON
setbit 4 T3CON ; 1:8 prescale value (slowest speeed) for timer3
setptr 0
settotal_move_ave 0
setn 0
repeat ARRAY_SIZE [setarray n 0 setn (n + 1)]
end

to getvalue_SRF05
clearbit 2 PIR1 ; reset to trigger next event on C2
setn (((read TMR3H) * 256) + (read TMR3L)) ; get current timer 3 value
setbit 7 portb-ddr clearbit 7 portb-ddr ; trigger SRF05 for reading ***
waituntil [testbit 2 PIR1] ; have we got event on C2 yet?
setm (((read CCPR1H) * 256) + (read CCPR1L)) ; what was value of timer 3 at time of event on C2?
output (m - n) ; return the time it took for the event to occur
end

to getave ; here is a relatively easy way to average out values
repeat 8 [ ; get 8 readings
settotal_val (((total_val * 7) + getvalue_SRF05) / 8) ; average in the newest reading
wait 1
]
output total_val ; return average
end

to setarray :index :value ; generic code to read and write to arrays
setglobal :index + 10 :value
end

to array :index
output global :index + 10
end

to get_move_ave ; a better way to do moving average.
settemp getvalue_SRF05 ; get the current value
settotal_move_ave (total_move_ave + temp - (array ptr)) ; subtract the oldest value, and add the current value
setarray ptr temp ; record the current value
setptr (ptr + 1) % ARRAY_SIZE ; update the pointer in the circular buffer
output (leftshift total_move_ave (0 - SHIFT_SIZE)) ; return the output of the moving average
end