Here is my routine to read any number of the GP2D12 IR Range Sensors. This is the code I wrote for W.A.L.T.E.R. which is based on some code I found here. I’ll answer any questions that may come up about this code here.
[code]’
’ IR Ranging Configuration
’
IR_Max con 3 ’ How many IR Rangers we have
IR_Threshld_Dist con 25 ’ Distance Threshold in cm for Turret closeness checks
IR_Threshld_Frnt con 35 ’ Front Detection threshold in cm
IR_Threshld_Tur con 35 ’ Detection threshold for Pan/Tilt Turret IR Ranger
’
’ IR Sensor Numbers
’
IR_LF con 0 ’ Left Front
IR_RF con 1 ’ Right Front
IR_TU con 2 ’ Pan/Tilt Turret
’ Future expansion
IR_LW con 4 ’ Wheel Left
IR_RW con 8 ’ Wheel Right
’
’ Bits for IR_Status checking
’
IR_LeftFront con 1
IR_RightFront con 2
IR_BothFront con 3
IR_Turret con 4
’ For future expansion
IR_LeftWheel con 8 ’ Left Side (across wheel)
IR_RightWheel con 16 ’ Right Side (across wheel)
IR_BothWheels con 24 ’ Both Sides (across wheels)
’
'---- Table Setup for IR Ranging Routine ] ----------------------------------------------------------
’
scantable bytetable 80,80,80,80,80,80,80,80,80,78, |
76,74,72,70,68,66,64,62,60,59, |
58,57,55,53,52,51,50,49,48,47, |
45,43,42,41,40,39,38,37,35,33, |
32,31,30,30,29,29,28,28,27,27, |
26,26,26,25,25,25,24,24,24,23, |
23,22,22,21,21,20,20,20,19,19, |
18,18,18,17,17,16,16,16,15,15, |
15,14,14,13,13,13,12,12,11,11, |
11,10,10,10,10,10,10,10,10,10
’ Read up to three IR Ranging sensors - based on code written by Nathan Scherdin
’ and Chuck Hellebuyck in their Range.Bas module. This routine can read up to
’ three IR Rangers without modification, controlled by the constant IR_Max.
Check_IR_Rangers:
IR_Status = 0
for index = 0 to IR_Max - 1
IR_Detected(index) = 0
' Read the sensor value
if index = IR_LF then ' Left Front
adin AX0, 2, AD_RON, scanrange
elseif index = IR_RF ' Right Front
adin AX2, 2, AD_RON, scanrange
elseif index = IR_TU ' Pan/Tilt Turret
adin AX3, 2, AD_RON, scanrange
else
if sendoutput then
serout S_OUT, I8N1_2400, "Invalid sensor number - ", DEC index, "!", 10, 13]
endif
exception Invalid_Sensor
endif
if sendoutput then
serout S_OUT, I8N1_2400, "Check_IR_Rangers: index = ", DEC index, ", IRPD_Status = ", DEC IRPD_Status, ", IR_Status = ", DEC IR_Status, 10, 13]
endif
tfloat = TOFLOAT scanrange / 5.12 ' Limit value to < 200 values
IR_Distance(index) = scantable(TOINT tfloat) ' Convert A/D to measurement
if (scanrange > 512) OR (IR_Distance(index) < (IR_Threshold(index) + IR_Offset(index))) then
IR_Detected(index) = 1
' Add bit value for the current sensor to IR_Status
' Bit 0 = 1, Sensor 0 Detect 0 0 0 0 0 0 0 1 Left Side
' Bit 1 = 1, Sensor 1 Detect 0 0 0 0 0 0 1 0 Right Side
' Bit 2 = 1, Sensor 2 Detect 0 0 0 0 0 1 0 0 Turret
' Bit 3 = 1, Sensor 3 Detect 0 0 0 0 1 0 0 0 Left Wheel
' Bit 4 = 1, Sensor 4 Detect 0 0 0 1 0 0 0 0 Right Wheel
if index = IR_LF then
IR_Status = IR_Status + 1
else
' This will have to be changed for more than 3 sensors - need proper
' power of 2 routine to make this code truely expandable.
IR_Status = IR_Status + (index * 2)
endif
if makenoise then
if index = IR_LF then ' Front Left Side
sound speaker, [dur\note5,dur\note6,dur\note7,dur\note8]
elseif index = IR_RF ' Front Right Side
sound speaker, [dur\note8,dur\note7,dur\note6,dur\note5]
elseif index = IR_TU ' Turret
sound speaker, [dur\note9,dur\note10,dur\note11,dur\note12]
endif
endif
endif
if sendoutput then
serout S_OUT, I8N1_2400, "Check_IR_Rangers: index = ", DEC index, ", Threshold = ", DEC IR_Threshold(index), ", Distance = ", DEC IR_Distance(index), " cm, Detected = ", DEC IR_Detected(index), 10, 13]
endif
next
if sendoutput then
serout S_OUT, I8N1_2400, "Check_IR_Rangers: --------------------------------------------------------------------------------------------", 10, 13]
endif
Invalid_Sensor:
return[/code]
8-Dale
