def __init__(self, slaveAddress):
self.logger = logging.getLogger("robot.IrSensorsController")
self.bus = smbus.SMBus(1)
self.slaveAddress = slaveAddress
self.rangeTable = RangeTable.unpickleTable()
if (self.rangeTable == 0):
self.rangeTable = RangeTable()
def __init__(self, slaveAddress):
self.logger=logging.getLogger("robot.IrSensorsController")
self.bus = smbus.SMBus(1)
self.slaveAddress = slaveAddress
self.rangeTable=RangeTable.unpickleTable()
if(self.rangeTable==0):
self.rangeTable=RangeTable()
class IR_Sensors_Controller():
'''
Constructor
'''
def __init__(self, slaveAddress):
self.logger = logging.getLogger("robot.IrSensorsController")
self.bus = smbus.SMBus(1)
self.slaveAddress = slaveAddress
self.rangeTable = RangeTable.unpickleTable()
if (self.rangeTable == 0):
self.rangeTable = RangeTable()
'''''' '''''' '''''' '''''' '''''' '''''' '''''' '''''' '''''' '''
Select the sequence of channels to read Chan
D11 D10 D9 D8 | D7 D6 D5 D4
0 0 0 0 | 0 0 0 1 Vin1
0 0 0 0 | 0 0 1 0 Vin2
0 0 0 0 | 0 1 0 0 Vin3
0 0 0 0 | 1 0 0 0 Vin4
0 0 0 1 | 0 0 0 0 Vin5
0 0 1 0 | 0 0 0 0 Vin6
0 1 0 0 | 0 0 0 0 Vin7
1 0 0 0 | 0 0 0 0 Vin8
Byte 1 = 0000+D11+D10+D9+D8
Byte 2 = D7+D6+D5+D4+1+AlertEN+Busy/Alert+Alert/BusyPolatiry
''' '''''' '''''' '''''' '''''' '''''' '''''' '''''' '''''' ''''''
'''
Configure the configurationregister. Can be used to read from
a sequence of channels automatically.
'''
def setConfigurationRegister(self, MSBs, LSBs):
chosenRegister = ConfigurationReg | multiChannels << 4
byte1 = MSBs
byte2 = 0x0F | LSBs << 4
self.bus.write_i2c_block_data(self.slaveAddress, chosenRegister,
[byte1, byte2])
'''
Read input from IR sensor
'''
def readSensorBlock(self, channel, register):
chosenRegister = register | channel << 4
try:
sensorInput = self.bus.read_i2c_block_data(self.slaveAddress,
chosenRegister, 2)
except IOError:
print 'Error in ReadSensorBlock'
return sensorInput
'''
Extract the raw distance from the 2 received bytes (12 LSB's)
'''
def extractRawDistance(self, sensorRead):
le = len(sensorRead)
if (le > 1):
tmp = (sensorRead[0] & 0b00001111) << 8 | sensorRead[1] << 0
return int(tmp)
return -1
'''
takes sensorRead as param and returns the distance in cm float
'''
def lookupCm(self, rawDistance):
if (rawDistance > 0):
return self.rangeTable.lookUpDistance(rawDistance)
return -1
'''
takes sensorRead as param and returns the alerts from a conversion
'''
def getAlerts(self, sensorRead):
if (len(sensorRead) > 1):
alert = sensorRead[0] >> 7
return alert
return -1
'''
Read average measurement from a single sensor
'''
def getAverageInCm(self, channel, amount):
average = 0
for i in range(0, amount):
tmp = self.readSensorBlock(channel, ConversionResultReg)
tmp = self.extractRawDistance(tmp)
average += tmp
time.sleep(0.10)
return self.lookupCm(int(average / amount))
'''
Read input from channels described in the channels list
Returns a list with sensor distances in cm
'''
def multiChannelReadCm(self, channels, amount):
distances = [0 for i in range(len(channels))]
for i in range(amount):
for j in range(len(distances)):
'Read from sensor'
reading = self.lookupCm(
self.extractRawDistance(
self.readSensorBlock(channels[j],
ConversionResultReg)))
'Sensor is a side sensor'
if (j == 0 or 1):
'Gap detected wait until sensor input settles'
while (reading > lastSamples[j] + 2):
lastSamples[j] = reading
reading = self.lookupCm(
self.extractRawDistance(
self.readSensorBlock(channels[j],
ConversionResultReg)))
distances[j] += reading
lastSamples[j] = reading
else:
reading = self.lookupCm(
self.extractRawDistance(
self.readSensorBlock(channels[j],
ConversionResultReg)))
distances[j] += reading
'Done reading n readings from channel'
if (amount - i == 1):
distances[j] = (distances[j] / amount)
self.logger.info("sampleAverage/" + str(distances))
#print distances
return distances
'''
Print the content of the distance list (Redundant!)
'''
def printMultiChannelReadCm(self, distances):
print("sensor readout in cm:")
for i in range(len(distances)):
print("sensor " + str(i) + "\t\t")
for i in range(len(distances)):
print(str(distances[i]))
class IR_Sensors_Controller():
'''
Constructor
'''
def __init__(self, slaveAddress):
self.logger=logging.getLogger("robot.IrSensorsController")
self.bus = smbus.SMBus(1)
self.slaveAddress = slaveAddress
self.rangeTable=RangeTable.unpickleTable()
if(self.rangeTable==0):
self.rangeTable=RangeTable()
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Select the sequence of channels to read Chan
D11 D10 D9 D8 | D7 D6 D5 D4
0 0 0 0 | 0 0 0 1 Vin1
0 0 0 0 | 0 0 1 0 Vin2
0 0 0 0 | 0 1 0 0 Vin3
0 0 0 0 | 1 0 0 0 Vin4
0 0 0 1 | 0 0 0 0 Vin5
0 0 1 0 | 0 0 0 0 Vin6
0 1 0 0 | 0 0 0 0 Vin7
1 0 0 0 | 0 0 0 0 Vin8
Byte 1 = 0000+D11+D10+D9+D8
Byte 2 = D7+D6+D5+D4+1+AlertEN+Busy/Alert+Alert/BusyPolatiry
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'''
Configure the configurationregister. Can be used to read from
a sequence of channels automatically.
'''
def setConfigurationRegister(self, MSBs, LSBs):
chosenRegister = ConfigurationReg | multiChannels << 4
byte1 = MSBs
byte2 = 0x0F | LSBs << 4
self.bus.write_i2c_block_data(self.slaveAddress, chosenRegister,[byte1, byte2])
'''
Read input from IR sensor
'''
def readSensorBlock(self, channel, register):
chosenRegister = register | channel << 4
try:
sensorInput=self.bus.read_i2c_block_data(self.slaveAddress,chosenRegister, 2)
except IOError:
print 'Error in ReadSensorBlock'
return sensorInput
'''
Extract the raw distance from the 2 received bytes (12 LSB's)
'''
def extractRawDistance(self,sensorRead):
le=len(sensorRead)
if(le>1):
tmp=(sensorRead[0] & 0b00001111) <<8 | sensorRead[1]<<0
return int(tmp)
return -1
'''
takes sensorRead as param and returns the distance in cm float
'''
def lookupCm(self,rawDistance):
if (rawDistance>0):
return self.rangeTable.lookUpDistance(rawDistance)
return -1
'''
takes sensorRead as param and returns the alerts from a conversion
'''
def getAlerts(self,sensorRead):
if(len(sensorRead)>1):
alert=sensorRead[0] >> 7
return alert
return -1
'''
Read average measurement from a single sensor
'''
def getAverageInCm(self,channel,amount):
average=0
for i in range(0,amount):
tmp = self.readSensorBlock(channel, ConversionResultReg)
tmp = self.extractRawDistance(tmp)
average+=tmp
time.sleep(0.10)
return self.lookupCm(int(average/amount))
'''
Read input from channels described in the channels list
Returns a list with sensor distances in cm
'''
def multiChannelReadCm(self,channels, amount):
distances = [0 for i in range(len(channels))]
for i in range(amount):
for j in range(len(distances)):
'Read from sensor'
reading = self.lookupCm(self.extractRawDistance(self.readSensorBlock(channels[j], ConversionResultReg)))
'Sensor is a side sensor'
if(j == 0 or 1):
'Gap detected wait until sensor input settles'
while(reading > lastSamples[j]+2):
lastSamples[j] = reading
reading = self.lookupCm(self.extractRawDistance(self.readSensorBlock(channels[j], ConversionResultReg)))
distances[j] += reading
lastSamples[j] = reading
else:
reading = self.lookupCm(self.extractRawDistance(self.readSensorBlock(channels[j], ConversionResultReg)))
distances[j] += reading
'Done reading n readings from channel'
if(amount-i==1):
distances[j]=(distances[j]/amount)
self.logger.info("sampleAverage/"+str(distances))
#print distances
return distances
'''
Print the content of the distance list (Redundant!)
'''
def printMultiChannelReadCm(self,distances):
print("sensor readout in cm:")
for i in range(len(distances)):
print("sensor "+str(i)+"\t\t")
for i in range(len(distances)):
print(str(distances[i]))
