mot_L VAR word
mot_R VAR word
Lp CON 1
Rp CON 0
hafSec con 25
ping		con	0		' free stamp pin '0' Output to activate pinger. 
compRC		con	2		' free stamp pin '2' Output to set comparator RC circuit. 
rcvr		con	1		' free stamp pin '1' Input from 40kHz receiver/comparator.
pingLen	con	200		' Duration of ping in 2-us units.
echTime VAR word
ticks VAR byte


run:
  mot_L = 925: mot_R = 949

  pulsout Lp, mot_L
  pulsout Rp, mot_R
  gosub sonar
goto run

hurt:
  for ticks = 1 to 50
  mot_L = 725: mot_R = 949
  pulsout Lp, mot_L
  pulsout Rp, mot_R
  next
goto sonar

sonar:
  high ping		' Turn pinger off initially
  high compRC			' Raise C2 to +5 volts. 
  pause 1			' Allow time for C2 to reach +5V. 
  input compRC			' Disconnect pin from C2. 
  pulsout ping,pingLen		' Send a short 40kHz pulse. 
  rctime rcvr,0,echTime		' Wait for echo; save time to echTime.
  if echTime < 150 then hurt
  if echTime > 149 then run
return