' =========================================================================
'   File....... SENSOR_FUSION.BS2
'   Purpose.... Implement weighted mean fusion of distance data from two
'               rangefinder sensors of different accuracy/quality
'   Author..... Bryan Bergeron
'   Reference.. See Parallax data sheets for GP2D12 and ADC0831
'   Hardware... BS2p, US and IR rangefinder sensors
'   E-mail..... bergeronbryan@yahoo.com
'   Version.... 20 MAR 2006
'
'   {$STAMP BS2p}
'   {$PBASIC 2.5}
' =========================================================================
'
' ====[ Constants ]========================================================
Weight1         CON     2               ' Weight of sensor 1
Weight2         CON     1               ' Weight of sensor 2
Max_Sensor1     CON     368             ' Maximum in-range value for sensor 1
Max_Sensor2     CON     99              ' Maximum in-range value for sensor 2
Max_Fused       CON     369             ' Depends on environment
True            CON     1               '
False           CON     0               '
' ====[ Variables ]=========================================================
Sensor1         VAR     Word            ' Assign sensor 1 data
Sensor2         VAR     Word            ' Assign sensor 2 data
Fused           VAR     Word            ' Fused sensor data value
InRange1        VAR     Byte            ' True if sensor 1 data in range
InRange2        VAR     Byte            ' True if sensor 2 data in range
'
' ====[ Initialization ]====================================================

Sensor1 = 90                            ' Replace with sensor assignment
Sensor2 = 0                             ' Change values for testing
'
' ====[ Main ]==============================================================
Main:
GOSUB FuseData
DEBUG "FUSED SENDOR DATA: ", DEC Fused, CR
END
'
' ====[ Subroutine ]========================================================
' Fused = [[sensor1 x weight1] + [sensor2 x weight2]]/[weight1 + weight2]
'
FuseData:
InRange1 = False: InRange2 = False     ' Default both sensors out of range
Fused = Max_Fused                      ' Default beyond range of both sensors
'
IF (Sensor1 <= Max_Sensor1) THEN InRange1 = True
IF (Sensor2 <= Max_Sensor2) THEN InRange2 = True
'
' If both sensors show out of range, then return Max_Fused
IF ((NOT(InRange1 = True)) AND (NOT(InRange2 = True))) THEN Done
'
' Perform weighted mean calculation
Fused = (Sensor1*Weight1*InRange1) + (Sensor2*Weight2*InRange2)
Fused = Fused/((Weight1*InRange1) + (Weight2*InRange2))
Done:
RETURN
' =========================================================================