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]))