Smart Proximity Detector

A PICAXE-08M operates the proximity detector. It takes turns flashing two IR LEDs (IREDs) at six different frequencies. The detector between the pair of IREDs is most sensitive to 40 kHz flashes. So when the IREDs are flashed at a different frequency, its ability to detect reflections is reduced (it can't see obstacles as far away). So the PICAXE flashes an IRED and checks with the detector to see if it saw the reflection.

The PICAXE cycles through all six frequencies until there are detections on both the left and right IREDs (the only time it runs through all six frequencies is if the IR detector doesn't ever detect a reflection). The PICAXE then sends a serial message with the range to the obstacles as thety appeared to the left and right IREDs. The range of dfistance is from 1 to 6, with a value of 7 indicating there was no reflection or that the obstacles are infinity far away.

The robot reads the data from the proximity detector as it needs to and then acts on the results.

Here's the code as it is currently written.

symbol RightDetect = B0
symbol LeftDetect = B1
symbol distance = B2
symbol counter = B3
symbol RightDistance = B4
symbol LeftDistance = B5
symbol left = 1
symbol right = 4

Proximity_Detect:
RightDistance = 7
LeftDistance = 7
for counter = 1 to 6

CheckRight:
if RightDistance < 7 then CheckLeft
low right
high left
gosub Flash
RightDetect = pin3 '0 = detect, 1 = no detect
pwmout 2 off
pause 2
if RightDetect = 1 then CheckLeft
RightDistance = counter

CheckLeft:
if LeftDistance < 7 then FinishCheck
low left
high right
gosub Flash
LeftDetect = pin3 '0 = detect, 1 = no detect
pwmout 2 off
pause 2
if LeftDetect = 1 then FinishCheck
LeftDistance = counter

FinishCheck:
if LeftDistance < 7 then IsRight7
goto Repeat_Flash

IsRight7:
if RightDistance < 7 then Report

Repeat_Flash:
next

Report:
serout 0,T1200_4,(255,"L",LeftDistance,"R",RightDistance)
goto Proximity_Detect

Flash:
if counter > 1 then Check45
gosub kHz46
goto End_Flash

Check45:
if counter > 2 then Check44
gosub kHz45
goto End_Flash

Check44:
if counter > 3 then Check43
gosub kHz44
goto End_Flash

Check43:
if counter > 4 then Check42
gosub kHz43
goto End_Flash

Check42:
if counter > 5 then Check41
gosub kHz42
goto End_Flash

Check41:
if counter > 6 then Check40
gosub kHz41
goto End_Flash

Check40:
gosub kHz40
End_Flash: return

kHz40:
pwmout 2,24,50 ' 14 inches
return

kHz41:
pwmout 2,23,49 ' 10 inches
return

kHz42:
pwmout 2,23,48 ' 9 inches
return

kHz43:
pwmout 2,22,47 ' 6 inches
return

kHz44:
pwmout 2,22,45 ' 5 inches
return

kHz45:
pwmout 2,21,44 ' 3 inches
return

kHz46:
pwmout 2,21,43 ' 2 inches
return