class Device(object): def __init__(self, address, bus): self._bus = SMBus(bus) self._address = address def writeRaw8(self, value): value = value & 0xff self._bus.write_byte(self._address, value) def readRaw8(self): result = self._bus.read_byte(self._address) & 0xff return result def write8(self, register, value): value = value & 0xff self._bus.write_byte_data(self._address, register, value) def readU8(self, register): result = self._bus.read_byte_data(self._address, register) & 0xFF return result def readS8(self, register): result = self.readU8(register) if result > 127: result -= 256 return result def write16(self, register, value): value = value & 0xffff self._bus.write_word_data(self._address, register, value) def readU16(self, register, little_endian = True): result = self._bus.read_word_data(self._address,register) & 0xFFFF if not little_endian: result = ((result << 8) & 0xFF00) + (result >> 8) return result def readS16(self, register, little_endian = True): result = self.readU16(register, little_endian) if result > 32767: result -= 65536 return result def writeList(self, register, data): self._bus.write_i2c_block_data(self._address, register, data) def readList(self, register, length): results = self._bus.read_i2c_block_data(self._address, register, length) return results
class MTSMBus(I2CBus): """ Multi-thread compatible SMBus bus. This is just a wrapper of SMBus, serializing I/O on the bus for use in multi-threaded context and adding _i2c_ variants of block transfers. """ def __init__(self, bus_id=1, **kwargs): """ :param int bus_id: the SMBus id (see Raspberry Pi documentation) :param kwargs: parameters transmitted to :py:class:`smbus.SMBus` initializer """ I2CBus.__init__(self, **kwargs) self._bus = SMBus(bus_id) # I/O serialization lock self._lock = threading.Lock() def read_byte(self, addr): with self._lock: return self._bus.read_byte(addr) def write_byte(self, addr, data): with self._lock: self._bus.write_byte(addr, data) def read_byte_data(self, addr, reg): with self._lock: return self._bus.read_byte_data(addr, reg) def write_byte_data(self, addr, reg, data): with self._lock: self._bus.write_byte_data(addr, reg, data) def read_word_data(self, addr, reg): with self._lock: return self._bus.read_word_data(addr, reg) def write_word_data(self, addr, reg, data): with self._lock: self._bus.write_word_data(addr, reg, data) def read_block_data(self, addr, reg): with self._lock: return self._bus.read_block_data(addr, reg) def write_block_data(self, addr, reg, data): with self._lock: self._bus.write_block_data(addr, reg, data) def read_i2c_block_data(self, addr, reg, count): with self._lock: return self._bus.read_i2c_block_data(addr, reg, count) def write_i2c_block_data(self, addr, reg, data): with self._lock: self._bus.write_i2c_block_data(addr, reg, data)
class LocalhostSMBusSlave(SMBusSlave): def __init__(self, bus, address): SMBusSlave.__init__(self, bus, address) self.bus = SMBus(bus) self.address = address #return: one byte hex string without '0x': '12' def get_byte(self, offset): # print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name) ret = self.bus.read_byte_data(self.address, offset) time.sleep(self.interval) data = hex(ret) return data[2:].zfill(2) #input: offset:str, data:int def set_byte(self, offset, data): # print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name) self.bus.write_byte_data(self.address, offset, data) time.sleep(self.interval) #return: two byte hex string without '0x': '1234' def get_word(self, offset): # print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name) ret = self.bus.read_word_data(self.address, offset) data = hex(ret) time.sleep(self.interval) return data[2:].zfill(4) def set_word(self, offset, data): # print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name) if sys.byteorder == "big": data = ((data & 0xff00) >> 8) + ((data & 0x00ff) << 8) self.bus.write_word_data(self.address, offset, data) time.sleep(self.interval) #return: hex string list per byte,without '0x' :['12', '0a'] def get_block(self, offset, length="32"): # print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name) ret = self.bus.read_block_data(self.address, offset) time.sleep(self.interval) hex_ret = [hex(i)[2:].zfill(2) for i in ret] return hex_ret def set_block(self, offset, data): # print "%s %s" % (datetime.datetime.utcnow().strftime("%H:%M:%S.%f")[:-4], sys._getframe().f_code.co_name) data_arr = [(data & 0x00ff), ((data & 0xff00) >> 8)] #self.bus.write_i2c_block_data(self.address, offset, data_arr) self.bus.write_block_data(self.address, offset, data_arr) time.sleep(self.interval)
class DAC12(object): def __init__(self, dac_num, maximum=100.0): bus, self.addr = get_dac_addr(dac_num) self.bus = SMBus(bus) self.coeff = 4096/float(maximum) def conv(self, valeur): return int(ceil(valeur * self.coeff)) & 0xFFFF def set_eeprom(self, valeur): self.bus.write_word_data(self.addr, WRITE_DAC_AND_EEPROM, self.conv(valeur)) def set(self, valeur): self.bus.write_word_data(self.addr, WRITE_DAC, self.conv(valeur))
class DAC12(object): def __init__(self, dac_num, maximum=100.0): bus, self.addr = get_dac_addr(dac_num) self.bus = SMBus(bus) self.coeff = 4096 / float(maximum) def conv(self, valeur): return int(ceil(valeur * self.coeff)) & 0xFFFF def set_eeprom(self, valeur): self.bus.write_word_data(self.addr, WRITE_DAC_AND_EEPROM, self.conv(valeur)) def set(self, valeur): self.bus.write_word_data(self.addr, WRITE_DAC, self.conv(valeur))
def main(): ''' Main program function ''' # Define registers values from datasheet IODIRA = 0x00 # IO direction A - 1= input 0 = output IODIRB = 0x01 # IO direction B - 1= input 0 = output IPOLA = 0x02 # Input polarity A IPOLB = 0x03 # Input polarity B GPINTENA = 0x04 # Interrupt-onchange A GPINTENB = 0x05 # Interrupt-onchange B DEFVALA = 0x06 # Default value for port A DEFVALB = 0x07 # Default value for port B INTCONA = 0x08 # Interrupt control register for port A INTCONB = 0x09 # Interrupt control register for port B IOCON = 0x0A # Configuration register GPPUA = 0x0C # Pull-up resistors for port A GPPUB = 0x0D # Pull-up resistors for port B INTFA = 0x0E # Interrupt condition for port A INTFB = 0x0F # Interrupt condition for port B INTCAPA = 0x10 # Interrupt capture for port A INTCAPB = 0x11 # Interrupt capture for port B GPIOA = 0x12 # Data port A GPIOB = 0x13 # Data port B OLATA = 0x14 # Output latches A OLATB = 0x15 # Output latches B i2cbus = SMBus(1) # Create a new I2C bus i2caddress = 0x20 # Address of MCP23017 device i2cbus.write_byte_data(i2caddress, IOCON, 0x02) # Update configuration register i2cbus.write_word_data( i2caddress, IODIRA, 0xFF00) # Set Port A as outputs and Port B as inputs while (True): portb = i2cbus.read_byte_data(i2caddress, GPIOB) # Read the value of Port B print(portb) # print the value of Port B i2cbus.write_byte_data(i2caddress, GPIOA, 0x01) # Set pin 1 to on time.sleep(0.5) # Wait 500ms i2cbus.write_byte_data(i2caddress, GPIOA, 0x00) # Set pin 1 to off time.sleep(0.5) # Wait 500ms
def setUVLimit(self, uvLimit): if (uvLimit > 32): uvWarnLimit = float(uvLimit) + 2 uvFaultLimit = float(uvLimit) else: uvWarnLimit = 34.0 uvFaultLimit = 32.0 bus = SMBus(1) #bus.write_byte_data(self.address, 0x10, int(0b00000000)) bus.write_word_data(self.address, 0x59, int(uvFaultLimit)) #This causes an error bus.write_word_data( self.address, 0x58, int(uvWarnLimit) ) #The error shows up here or if this is commented out it shows up on under the getTempurature method bus.close()
class EPuck: """Class for interfacing with a generic e-puck robot.""" def __init__(self, i2c_bus: Optional[int] = None, i2c_address: Optional[int] = None): if i2c_bus is not None: self._bus = SMBus(i2c_bus) else: try: self._bus = SMBus(_EPUCK_I2C_CHANNEL) except FileNotFoundError: self._bus = SMBus(_EPUCK_LEGACY_I2C_CHANNEL) if i2c_address is not None: self._i2c_address = i2c_address else: self._i2c_address = _EPUCK_I2C_ADDRESS GPIO.setmode(GPIO.BOARD) GPIO.setup(_EPUCK_RESET_PIN, GPIO.OUT, initial=GPIO.HIGH) def __del__(self): GPIO.cleanup(_EPUCK_RESET_PIN) def _write_data_8(self, address, data): self._bus.write_byte_data(self._i2c_address, address, data) def _write_data_16(self, address, data): self._bus.write_word_data(self._i2c_address, address, data) def _read_data_8(self, address): return self._bus.read_byte_data(self._i2c_address, address) def _read_data_16(self, address): return self._bus.read_word_data(self._i2c_address, address) @staticmethod def reset_robot(): GPIO.output(_EPUCK_RESET_PIN, GPIO.LOW) sleep(0.1) GPIO.output(_EPUCK_RESET_PIN, GPIO.HIGH)
class MCP9804Sensor(object): """Represents an MCP9820 i2c sensor connected to a GPIO pin""" def __init__(self, connections, logger, params, sensors, actuators): """Sets the sensor pin to pud and publishes its current value""" self.lastpoll = time.time() #Initialise to some time ago (make sure it publishes) self.lastPublish = time.time() - 1000 self.logger = logger self.sensorMode = params("Mode") # Note the int(xx, 16) here as this is Hex (base 16) in the Config self.addr = int(params("Address"), 16) # 0x1f self.bus = SMBus(int(params("Bus"))) # 1 self.precisionTemp = int(params("PrecisionTemp")) self.useF = False try: if (params("Scale") == 'F'): self.useF = True except ConfigParser.NoOptionError: pass #Use 1 reading as default self.ARRAY_SIZE = 1 if (self.sensorMode == "Advanced"): #Array size of last readings self.ARRAY_SIZE = 5 #Initialize Array self.arrTemperature = [None] * self.ARRAY_SIZE self.temperature = None self.forcePublishInterval = 60 self.destination = params("Destination") self.poll = float(params("Poll")) self.publish = connections self.logger.info( "----------Configuring MCP9804 Sensor: bus='{0}' address='{1}' poll='{2}' destination='{3}' Initial values: Temp='{4}'" .format(self.bus, self.addr, self.poll, self.destination, self.temperature)) self.publishState() def wakeup(self): try: self.bus.write_word_data(self.addr, 1, 0x0000) except: self.logger.warn("Wakeup failed..") def shutdown(self): try: self.bus.write_word_data(self.addr, 1, 0x0001) except: self.logger.warn("Shutdown failed..") def convertTemp(self, value): return value if self.useF == False else value * 9 / 5.0 + 32 def checkState(self): """Detects and publishes any state change""" hasChanged = False valueTemp = self.readSensor(True) if (valueTemp is None): self.logger.warn( "Last reading isn't valid. preserving old reading T='{0}'". format(self.temperature)) return #Verify reading of temperature if (valueTemp != self.temperature): self.temperature = valueTemp hasChanged = True if (hasChanged or (time.time() - self.lastPublish) > self.forcePublishInterval): self.publishState() def publishStateImpl(self, data, destination): for conn in self.publish: conn.publish(data, destination) def publishState(self): """Publishes the current state""" didPublish = False if (self.temperature is not None): didPublish = True strTemp = str(round(self.temperature, self.precisionTemp)) self.logger.debug("Publish temperature '{0}' to '{1}'".format( strTemp, self.destination + "/temperature")) self.publishStateImpl(strTemp, self.destination + "/temperature") if (didPublish): self.lastPublish = time.time() def isReadingValid(self, value, acceptableMin, acceptableMax): if (value is None): return False if ((value >= acceptableMin) and (value <= acceptableMax)): return True return False def readSensor(self, shutdown=True): resultTemp = None valueTemp = float(-99) try: if shutdown: self.wakeup() time.sleep(0.36) msb, lsb = self.bus.read_i2c_block_data(self.addr, 5, 2) if shutdown: self.shutdown() tcrit = msb >> 7 & 1 tupper = msb >> 6 & 1 tlower = msb >> 5 & 1 temperature = (msb & 0xf) * 16 + lsb / 16.0 if msb >> 4 & 1: temperature = 256 - temperature valueTemp = float(temperature) except IOError as ex: self.logger.debug("ERROR - {0} - {1}".format( ex.errno, ex.strerror)) finally: if shutdown: try: self.shutdown() except Exception as e: self.logger.debug("ERROR - {0} {1}".format( e.errno, e.strerror)) #print("Raw reading T:{0}".format(valueTemp )) # Is temperature reading valid? if (self.isReadingValid(valueTemp, -40.0, 125.0)): valueTemp = self.convertTemp(valueTemp) self.arrTemperature.append(round(valueTemp, 2)) else: #Reading out of bounds return resultTemp if (len(self.arrTemperature) > self.ARRAY_SIZE): del self.arrTemperature[0] if (self.sensorMode == "Advanced"): sumTemp = sum(filter(None, self.arrTemperature)) noTempReadings = len(filter(None, self.arrTemperature)) if (noTempReadings > 0): resultTemp = float( str(round(sumTemp / noTempReadings, self.precisionTemp))) #print("readValueAdvanced: Result - Temp:'{0}'".format(resultTemp)) self.logger.info("readValue: Temp:'{0}'".format(resultTemp)) #Simple mode -> Just return last reading else: resultTemp = float(str(round(valueTemp, self.precissionTemp))) self.logger.info("readValue: Temp:'{0}'".format(resultTemp)) #print("readValueSimple: Result - Temp:'{0}'".format(resultTemp)) return resultTemp
class PanTilt: """PanTilt HAT Driver Communicates with PanTilt HAT over i2c to control pan, tilt and light functions """ REG_CONFIG = 0x00 REG_SERVO1 = 0x01 REG_SERVO2 = 0x03 REG_WS2812 = 0x05 REG_UPDATE = 0x4E UPDATE_WAIT = 0.03 NUM_LEDS = 24 def __init__( self, enable_lights=True, idle_timeout=2, # Idle timeout in seconds light_mode=WS2812, light_type=RGB, servo1_min=575, servo1_max=2325, servo2_min=575, servo2_max=2325, address=0x15, i2c_bus=None): self._is_setup = False self._idle_timeout = idle_timeout self._servo1_timeout = None self._servo2_timeout = None self._i2c_retries = 10 self._i2c_retry_time = 0.01 self._enable_servo1 = False self._enable_servo2 = False self._enable_lights = enable_lights self._light_on = 0 self._servo_min = [servo1_min, servo2_min] self._servo_max = [servo1_max, servo2_max] self._light_mode = light_mode self._light_type = light_type self._i2c_address = address self._i2c = i2c_bus def setup(self): if self._is_setup: return True if self._i2c is None: try: from smbus import SMBus self._i2c = SMBus(1) except ImportError: if version_info[0] < 3: raise ImportError( "This library requires python-smbus\nInstall with: sudo apt-get install python-smbus" ) elif version_info[0] == 3: raise ImportError( "This library requires python3-smbus\nInstall with: sudo apt-get install python3-smbus" ) self.clear() self._set_config() atexit.register(self._atexit) self._is_setup = True def _atexit(self): if self._servo1_timeout is not None: self._servo1_timeout.cancel() if self._servo2_timeout is not None: self._servo2_timeout.cancel() self._enable_servo1 = False self._enable_servo2 = False self._set_config() def idle_timeout(self, value): """Set the idle timeout for the servos Configure the time, in seconds, after which the servos will be automatically disabled. :param value: Timeout in seconds """ self._idle_timeout = value def _set_config(self): """Generate config value for PanTilt HAT and write to device.""" config = 0 config |= self._enable_servo1 config |= self._enable_servo2 << 1 config |= self._enable_lights << 2 config |= self._light_mode << 3 config |= self._light_on << 4 self._i2c_write_byte(self.REG_CONFIG, config) def _check_int_range(self, value, value_min, value_max): """Check the type and bounds check an expected int value.""" if type(value) is not int: raise TypeError("Value should be an integer") if value < value_min or value > value_max: raise ValueError( "Value {value} should be between {min} and {max}".format( value=value, min=value_min, max=value_max)) def _check_range(self, value, value_min, value_max): """Check the type and bounds check an expected int value.""" if value < value_min or value > value_max: raise ValueError( "Value {value} should be between {min} and {max}".format( value=value, min=value_min, max=value_max)) def _servo_us_to_degrees(self, us, us_min, us_max): """Converts pulse time in microseconds to degrees :param us: Pulse time in microseconds :param us_min: Minimum possible pulse time in microseconds :param us_max: Maximum possible pulse time in microseconds """ self._check_range(us, us_min, us_max) servo_range = us_max - us_min angle = (float(us - us_min) / float(servo_range)) * 180.0 return int(round(angle, 0)) - 90 def _servo_degrees_to_us(self, angle, us_min, us_max): """Converts degrees into a servo pulse time in microseconds :param angle: Angle in degrees from -90 to 90 """ self._check_range(angle, -90, 90) angle += 90 servo_range = us_max - us_min us = (servo_range / 180.0) * angle return us_min + int(us) def _servo_range(self, servo_index): """Get the min and max range values for a servo""" return (self._servo_min[servo_index], self._servo_max[servo_index]) def _i2c_write_block(self, reg, data): if type(data) is list: for x in range(self._i2c_retries): try: self._i2c.write_i2c_block_data(self._i2c_address, reg, data) return except IOError: time.sleep(self._i2c_retry_time) continue raise IOError("Failed to write block") else: raise ValueError("Value must be a list") def _i2c_write_word(self, reg, data): if type(data) is int: for x in range(self._i2c_retries): try: self._i2c.write_word_data(self._i2c_address, reg, data) return except IOError: time.sleep(self._i2c_retry_time) continue raise IOError("Failed to write word") def _i2c_write_byte(self, reg, data): if type(data) is int: for x in range(self._i2c_retries): try: self._i2c.write_byte_data(self._i2c_address, reg, data) return except IOError: time.sleep(self._i2c_retry_time) continue raise IOError("Failed to write byte") def _i2c_read_byte(self, reg): for x in range(self._i2c_retries): try: return self._i2c.read_byte_data(self._i2c_address, reg) except IOError: time.sleep(self._i2c_retry_time) continue raise IOError("Failed to read byte") def _i2c_read_word(self, reg): for x in range(self._i2c_retries): try: return self._i2c.read_word_data(self._i2c_address, reg) except IOError: time.sleep(self._i2c_retry_time) continue raise IOError("Failed to read byte") def clear(self): """Clear the buffer.""" self._pixels = [0] * self.NUM_LEDS * 3 self._pixels += [1] def light_mode(self, mode): """Set the light mode for attached lights. PanTiltHAT can drive either WS2812 or SK6812 pixels, or provide a PWM dimming signal for regular LEDs. * PWM - PWM-dimmable LEDs * WS2812 - 24 WS2812 or 18 SK6812 pixels """ self.setup() self._light_mode = mode self._set_config() def light_type(self, set_type): """Set the light type for attached lights. Set the type of lighting strip connected: * RGB - WS2812 pixels with RGB pixel order * RGB - WS2812 pixels with GRB pixel order * RGBW - SK6812 pixels with RGBW pixel order * GRBW - SK6812 pixels with GRBW pixel order """ self._light_type = set_type def num_pixels(self): """Returns the supported number of pixels depending on light mode. RGBW or GRBW support 18 pixels RGB supports 24 pixels """ if self._light_type in [RGBW, GRBW]: return 18 return 24 def brightness(self, brightness): """Set the brightness of the connected LED ring. This only applies if light_mode has been set to PWM. It will be ignored otherwise. :param brightness: Brightness from 0 to 255 """ self.setup() self._check_int_range(brightness, 0, 255) if self._light_mode == PWM: # The brightness value is taken from the first register of the WS2812 chain self._i2c_write_byte(self.REG_WS2812, brightness) def set_all(self, red, green, blue, white=None): """Set all pixels in the buffer. :param red: Amount of red, from 0 to 255 :param green: Amount of green, from 0 to 255 :param blue: Amount of blue, from 0 to 255 :param white: Optional amount of white for RGBW and GRBW strips """ for index in range(self.num_pixels()): self.set_pixel(index, red, green, blue, white) def set_pixel_rgbw(self, index, red, green, blue, white): """Set a single pixel in the buffer for GRBW lighting stick :param index: Index of pixel from 0 to 17 :param red: Amount of red, from 0 to 255 :param green: Amount of green, from 0 to 255 :param blue: Amount of blue, from 0 to 255 :param white: Amount of white, from 0 to 255 """ self.set_pixel(index, red, green, blue, white) def set_pixel(self, index, red, green, blue, white=None): """Set a single pixel in the buffer. :param index: Index of pixel from 0 to 23 :param red: Amount of red, from 0 to 255 :param green: Amount of green, from 0 to 255 :param blue: Amount of blue, from 0 to 255 :param white: Optional amount of white for RGBW and GRBW strips """ self._check_int_range(index, 0, self.num_pixels() - 1) for color in [red, green, blue]: self._check_int_range(color, 0, 255) if white is not None: self._check_int_range(white, 0, 255) if self._light_type in [RGBW, GRBW]: index *= 4 if self._light_type == RGBW: self._pixels[index] = red self._pixels[index + 1] = green self._pixels[index + 2] = blue if self._light_type == GRBW: self._pixels[index] = green self._pixels[index + 1] = red self._pixels[index + 2] = blue if white is not None: self._pixels[index + 3] = white else: index *= 3 if self._light_type == RGB: self._pixels[index] = red self._pixels[index + 1] = green self._pixels[index + 2] = blue if self._light_type == GRB: self._pixels[index] = green self._pixels[index + 1] = red self._pixels[index + 2] = blue def show(self): """Display the buffer on the connected WS2812 strip.""" self.setup() self._i2c_write_block(self.REG_WS2812, self._pixels[:32]) self._i2c_write_block(self.REG_WS2812 + 32, self._pixels[32:64]) self._i2c_write_block(self.REG_WS2812 + 64, self._pixels[64:]) self._i2c_write_byte(self.REG_UPDATE, 1) def servo_enable(self, index, state): """Enable or disable a servo. Disabling a servo turns off the drive signal. It's good practise to do this if you don't want the Pan/Tilt to point in a certain direction and instead want to save power. :param index: Servo index: either 1 or 2 :param state: Servo state: True = on, False = off """ self.setup() if index not in [1, 2]: raise ValueError("Servo index must be 1 or 2") if state not in [True, False]: raise ValueError("State must be True/False") if index == 1: self._enable_servo1 = state else: self._enable_servo2 = state self._set_config() def servo_pulse_min(self, index, value): """Set the minimum high pulse for a servo in microseconds. :param value: Value in microseconds """ if index not in [1, 2]: raise ValueError("Servo index must be 1 or 2") self._servo_min[index - 1] = value def servo_pulse_max(self, index, value): """Set the maximum high pulse for a servo in microseconds. :param value: Value in microseconds """ if index not in [1, 2]: raise ValueError("Servo index must be 1 or 2") self._servo_max[index - 1] = value def get_servo_one(self): """Get position of servo 1 in degrees.""" self.setup() us_min, us_max = self._servo_range(0) us = self._i2c_read_word(self.REG_SERVO1) return self._servo_us_to_degrees(us, us_min, us_max) def get_servo_two(self): """Get position of servo 2 in degrees.""" self.setup() us_min, us_max = self._servo_range(1) us = self._i2c_read_word(self.REG_SERVO2) return self._servo_us_to_degrees(us, us_min, us_max) def servo_one(self, angle): """Set position of servo 1 in degrees. :param angle: Angle in degrees from -90 to 90 """ self.setup() if not self._enable_servo1: self._enable_servo1 = True self._set_config() us_min, us_max = self._servo_range(0) us = self._servo_degrees_to_us(angle, us_min, us_max) self._i2c_write_word(self.REG_SERVO1, us) if self._idle_timeout > 0: if self._servo1_timeout is not None: self._servo1_timeout.cancel() self._servo1_timeout = Timer(self._idle_timeout, self._servo1_stop) self._servo1_timeout.daemon = True self._servo1_timeout.start() def _servo1_stop(self): self._servo1_timeout = None self._enable_servo1 = False self._set_config() def servo_two(self, angle): """Set position of servo 2 in degrees. :param angle: Angle in degrees from -90 to 90 """ self.setup() if not self._enable_servo2: self._enable_servo2 = True self._set_config() us_min, us_max = self._servo_range(1) us = self._servo_degrees_to_us(angle, us_min, us_max) self._i2c_write_word(self.REG_SERVO2, us) if self._idle_timeout > 0: if self._servo2_timeout is not None: self._servo2_timeout.cancel() self._servo2_timeout = Timer(self._idle_timeout, self._servo2_stop) self._servo2_timeout.daemon = True self._servo2_timeout.start() def _servo2_stop(self): self._servo2_timeout = None self._enable_servo2 = False self._set_config() pan = servo_one tilt = servo_two get_pan = get_servo_one get_tilt = get_servo_two
def _writeWordPMBus(self, cmd, word, pecByte=True): bus = SMBus(self.busID) bus.pec = pecByte bus.write_word_data(self.address, cmd, word) bus.close()
class I2C(object): MASTER = 0 SLAVE = 1 RETRY = 5 def __init__(self, *args, **kargs): self._bus = 1 self._smbus = SMBus(self._bus) def auto_reset(func): def wrapper(*args, **kw): try: return func(*args, **kw) except OSError: # soft_reset() time.sleep(1) return func(*args, **kw) return wrapper @auto_reset def _i2c_write_byte(self, addr, data): # self._debug("_i2c_write_byte: [0x{:02X}] [0x{:02X}]".format(addr, data)) return self._smbus.write_byte(addr, data) @auto_reset def _i2c_write_byte_data(self, addr, reg, data): # self._debug("_i2c_write_byte_data: [0x{:02X}] [0x{:02X}] [0x{:02X}]".format(addr, reg, data)) return self._smbus.write_byte_data(addr, reg, data) @auto_reset def _i2c_write_word_data(self, addr, reg, data): # self._debug("_i2c_write_word_data: [0x{:02X}] [0x{:02X}] [0x{:04X}]".format(addr, reg, data)) return self._smbus.write_word_data(addr, reg, data) @auto_reset def _i2c_write_i2c_block_data(self, addr, reg, data): # self._debug("_i2c_write_i2c_block_data: [0x{:02X}] [0x{:02X}] {}".format(addr, reg, data)) return self._smbus.write_i2c_block_data(addr, reg, data) @auto_reset def _i2c_read_byte(self, addr): # self._debug("_i2c_read_byte: [0x{:02X}]".format(addr)) return self._smbus.read_byte(addr) @auto_reset def _i2c_read_i2c_block_data(self, addr, reg, num): # self._debug("_i2c_read_i2c_block_data: [0x{:02X}] [0x{:02X}] [{}]".format(addr, reg, num)) return self._smbus.read_i2c_block_data(addr, reg, num) def is_ready(self, addr): addresses = self.scan() if addr in addresses: return True else: return False def scan(self): cmd = "i2cdetect -y %s" % self._bus _, output = self.run_command(cmd) outputs = output.split('\n')[1:] # self._debug("outputs") addresses = [] for tmp_addresses in outputs: tmp_addresses = tmp_addresses.split(':')[1] tmp_addresses = tmp_addresses.strip().split(' ') for address in tmp_addresses: if address != '--': addresses.append(address) # self._debug("Conneceted i2c device: %s"%addresses) return addresses def send(self, send, addr, timeout=0): if isinstance(send, bytearray): data_all = list(send) elif isinstance(send, int): data_all = [] d = "{:X}".format(send) d = "{}{}".format("0" if len(d) % 2 == 1 else "", d) # print(d) for i in range(len(d) - 2, -1, -2): tmp = int(d[i:i + 2], 16) # print(tmp) data_all.append(tmp) data_all.reverse() elif isinstance(send, list): data_all = send else: raise ValueError( "send data must be int, list, or bytearray, not {}".format( type(send))) if len(data_all) == 1: data = data_all[0] self._i2c_write_byte(addr, data) elif len(data_all) == 2: reg = data_all[0] data = data_all[1] self._i2c_write_byte_data(addr, reg, data) elif len(data_all) == 3: reg = data_all[0] data = (data_all[2] << 8) + data_all[1] self._i2c_write_word_data(addr, reg, data) else: reg = data_all[0] data = list(data_all[1:]) self._i2c_write_i2c_block_data(addr, reg, data) def recv(self, recv, addr=0x00, timeout=0): if isinstance(recv, int): result = bytearray(recv) elif isinstance(recv, bytearray): result = recv else: return False for i in range(len(result)): result[i] = self._i2c_read_byte(addr) return result def mem_write(self, data, addr, memaddr, timeout=5000, addr_size=8): #memaddr match to chn if isinstance(data, bytearray): data_all = list(data) elif isinstance(data, int): data_all = [] for i in range(0, 100): d = data >> (8 * i) & 0xFF if d == 0: break else: data_all.append(d) data_all.reverse() self._i2c_write_i2c_block_data(addr, memaddr, data_all) def mem_read(self, data, addr, memaddr, timeout=5000, addr_size=8): if isinstance(data, int): num = data elif isinstance(data, bytearray): num = len(data) else: return False result = bytearray(num) result = self._i2c_read_i2c_block_data(addr, memaddr, num) return result def test(): a_list = [0x2d, 0x64, 0x0] b = I2C() b.send(a_list, 0x14)
from smbus import SMBus b = SMBus(1) # 1 indicates /dev/i2c-1 dev_addr1 = 0x10 dev_addr2 = 0x11 general_purpose_register = 0x03 dac_pin_config_register = 0x05 dac_out0_register = 0x10 # set I/O0 of dev 0 to dac output b.write_word_data(dev_addr1, dac_pin_config_register, 0x0001) # set I/O0 to 1000/4095 -> 0x3e8 b.write_word_data(dev_addr1, dac_out0_register, 0x83e8) print("{:x}".format(b.read_word_data(dev_addr1, general_purpose_register)))
class MotorDriverTB6612FNG: """ A control class for the Grove - Motor Driver(TB6612FNG). """ _addr = None _buffer = None _i2c_bus = None def __init__(self, address=TB6612FNGCodes.GMD_I2C_ADDRESS): """ Initializes the Motor Driver module with a specific or default address. Creates a connection to the I2C bus on Raspberry. By default the address for this module is 0x14. :rtype: MotorDriverTB6612FNG class instance. """ self._i2c_bus = SMBus(1) self._addr = address self.standby() def standby(self): """ Enter standby mode. Normally you don't need to call this, except that you have called notStandby() before. :return: nothing. """ self._i2c_bus.write_word_data(self._addr, TB6612FNGCodes.GMD_CMD_STANDBY, 0) def not_standby(self): """ Exit standby mode. Motor driver does't do any action at this mode. :return: nothing. """ self._i2c_bus.write_word_data(self._addr, TB6612FNGCodes.GMD_CMD_NOT_STANDBY, 0) def set_i2c_addr(self, addr: int): """ Set new address for this module. :param addr: The new address for motor driver. (0x01~0x7f) :return: nothing. """ if addr == 0x00: return elif addr >= 0x80: return self._i2c_bus.write_word_data(self._addr, TB6612FNGCodes.GMD_CMD_SET_ADDR, addr) self._addr = addr def dc_motor_run(self, chl: int, speed: int): """ Output a specific amount of voltage to a specific motor. Voltage is between -255 and 255. The negative voltage means motor will go counter clockwise. :param chl: Selection which motor to run. MOTOR_CHA or MOTOR_CHB. :param speed: Speed from -255 to 255 to run this motor. (8bit voltage). Note that there is always a starting speed(a starting voltage) for motor. If the input voltage is 5V, the starting speed should larger than 100 or smaller than -100. :return: nothing. """ self._buffer = [0] * 3 if speed > 255: speed = 255 elif speed < -255: speed = -255 if speed >= 0: self._buffer[0] = int(TB6612FNGCodes.GMD_CMD_CW) self._buffer[2] = int(speed) else: self._buffer[0] = int(TB6612FNGCodes.GMD_CMD_CCW) self._buffer[2] = int(-speed) self._buffer[1] = int(chl) self._i2c_bus.write_i2c_block_data(self._addr, self._buffer[0], self._buffer[1:]) def dc_motor_break(self, chl: int): """ Brake, stop the motor immediately. :param chl: MOTOR_CHA or MOTOR_CHB. :return: nothing. """ self._i2c_bus.write_word_data(self._addr, TB6612FNGCodes.GMD_CMD_BRAKE, chl) def dc_motor_stop(self, chl: int): """ Stop the motor slowly. :param chl: MOTOR_CHA or MOTOR_CHB. :return: nothing. """ self._i2c_bus.write_word_data(self._addr, TB6612FNGCodes.GMD_CMD_STOP, chl) def stepper_run(self, mode: int, steps: int, rpm: int): """ Drive a stepper motor. :param mode: 4 driver mode: FULL_STEP,WAVE_DRIVE, HALF_STEP, MICRO_STEPPING, for more information: https://en.wikipedia.org/wiki/Stepper_motor#/media/File:Drive.png :param steps: The number of steps to run, range from -32768 to 32767. When steps = 0, the stepper stops. When steps > 0, the stepper runs clockwise. When steps < 0, the stepper runs anticlockwise. :param rpm: Revolutions per minute, the speed of a stepper, range from 1 to 300. Note that high rpm will lead to step lose, so rpm should not be larger than 150. :return: nothing. """ cw = 0 self._buffer = [0] * 6 if steps > 0: cw = 1 elif steps == 0: self.stepper_stop() elif steps == -32768: steps = 32767 else: steps = -steps if rpm < 1: rpm = 1 elif rpm > 300: rpm = 300 ms_per_step = 3000.0 / rpm self._buffer[0] = mode self._buffer[1] = int(cw) self._buffer[2] = steps self._buffer[3] = int(steps >> 8) self._buffer[4] = int(ms_per_step) self._buffer[5] = (int(ms_per_step) >> 8) self._i2c_bus.write_i2c_block_data(self._addr, TB6612FNGCodes.GMD_CMD_STEPPER_RUN, self._buffer) def stepper_stop(self): """ Stop a stepper motor. :return: nothing. """ self._i2c_bus.write_word_data(self._addr, TB6612FNGCodes.GMD_CMD_STEPPER_STOP, 0) def stepper_keep_run(self, mode: int, rpm: int, is_cw: int): """ Keep a stepper motor running. Keeps moving(direction same as the last move, default to clockwise). :param mode: 4 driver mode: FULL_STEP,WAVE_DRIVE, HALF_STEP, MICRO_STEPPING, for more information: https://en.wikipedia.org/wiki/Stepper_motor#/media/File:Drive.png :param rpm: Revolutions per minute, the speed of a stepper, range from 1 to 300. Note that high rpm will lead to step lose, so rpm should not be larger than 150. :param is_cw: Set the running direction, true for clockwise and false for anti-clockwise. :return: nothing. """ cw = 5 if is_cw else 4 self._buffer = [0] * 4 if rpm < 1: rpm = 1 elif rpm > 300: rpm = 300 ms_per_step = 3000.0 / rpm self._buffer[0] = int(mode) self._buffer[1] = int(cw) self._buffer[2] = int(ms_per_step) self._buffer[3] = (int(ms_per_step) >> 8) self._i2c_bus.write_i2c_block_data( self._addr, TB6612FNGCodes.GMD_CMD_STEPPER_KEEP_RUN, self._buffer)
class LinuxI2cBus: """A Linux I²C device, which is itself an I²C bus. Should not be instantiated directly; use `LinuxI2c.find_devices` instead. This type mimics the `smbus.SMBus` read/write/close APIs. However, `open` does not take any parameters, and not all APIs are available. """ # note: this is not a liquidctl BaseBus, as that would cause # find_liquidctl_devices to try to directly instantiate it def __init__(self, i2c_dev): if not i2c_dev.name.startswith('i2c-'): raise ValueError('not a bus or unsupported adapter') self._i2c_dev = i2c_dev self._smbus = None self._number = int(i2c_dev.name[4:]) def find_devices(self, drivers, **kwargs): """Probe drivers and find compatible devices in this bus.""" for drv in drivers: yield from drv.probe(self, **kwargs) def open(self): """Open the I²C bus.""" if not self._smbus: try: self._smbus = SMBus(self._number) except FileNotFoundError: if Path('/sys/class/i2c-dev').exists(): raise raise OSError('kernel module i2c-dev not loaded') from None def read_byte(self, address): """Read a single byte from a device.""" value = self._smbus.read_byte(address) _LOGGER.debug('read byte @ 0x%02x:0x%02x', address, value) return value def read_byte_data(self, address, register): """Read a single byte from a designated register.""" value = self._smbus.read_byte_data(address, register) _LOGGER.debug('read byte data @ 0x%02x:0x%02x 0x%02x', address, register, value) return value def read_word_data(self, address, register): """Read a single 2-byte word from a given register.""" value = self._smbus.read_word_data(address, register) _LOGGER.debug('read word data @ 0x%02x:0x%02x 0x%02x', address, register, value) return value def read_block_data(self, address, register): """Read a block of up to 32 bytes from a given register.""" data = self._smbus.read_block_data(address, register) _LOGGER.debug('read block data @ 0x%02x:0x%02x: %r', address, register, LazyHexRepr(data)) return data def write_byte(self, address, value): """Write a single byte to a device.""" _LOGGER.debug('writing byte @ 0x%02x: 0x%02x', address, value) return self._smbus.write_byte(address, value) def write_byte_data(self, address, register, value): """Write a single byte to a designated register.""" _LOGGER.debug('writing byte data @ 0x%02x:0x%02x 0x%02x', address, register, value) return self._smbus.write_byte_data(address, register, value) def write_word_data(self, address, register, value): """Write a single 2-byte word to a designated register.""" _LOGGER.debug('writing word data @ 0x%02x:0x%02x 0x%02x', address, register, value) return self._smbus.write_word_data(address, register, value) def write_block_data(self, address, register, data): """Write a block of byte data to a given register.""" _LOGGER.debug('writing block data @ 0x%02x:0x%02x %r', address, register, LazyHexRepr(data)) return self._smbus.write_block_data(address, register, data) def close(self): """Close the I²C connection.""" if self._smbus: self._smbus.close() self._smbus = None def load_eeprom(self, address): """Return EEPROM name and data in `address`, or None if N/A.""" # uses kernel facilities to avoid directly reading from the EEPROM # or managing its pages, also avoiding the need for unsafe=smbus dev = f'{self._number}-{address:04x}' try: name = self._i2c_dev.joinpath(dev, 'name').read_text().strip() # work around a bug in the kernel by reading the ee1004 eeprom # in small enough chunks # related: #416 ("DDR4 support broken on Linux 5.16.3") # related: https://lore.kernel.org/lkml/[email protected]/ eeprom_path = self._i2c_dev.joinpath(dev, 'eeprom') eeprom_size = eeprom_path.stat().st_size _LOGGER.debug('path=%s, size=%s', eeprom_path, eeprom_size) with eeprom_path.open(mode='rb', buffering=0) as f: eeprom = bytearray() while len(eeprom) < eeprom_size: b = f.read(128) if not b: break eeprom.extend(b) return LinuxEeprom(name, eeprom) except FileNotFoundError: return None @property def name(self): return self._i2c_dev.name @property def description(self): return self._try_sysfs_read('name') @property def parent_vendor(self): return self._try_sysfs_read_hex('device/vendor') @property def parent_device(self): return self._try_sysfs_read_hex('device/device') @property def parent_subsystem_vendor(self): return self._try_sysfs_read_hex('device/subsystem_vendor') @property def parent_subsystem_device(self): return self._try_sysfs_read_hex('device/subsystem_device') @property def parent_driver(self): try: return Path(os.readlink(self._i2c_dev.joinpath('device/driver'))).name except FileNotFoundError: return None def __str__(self): if self.description: return f'{self.name}: {self.description}' return self.name def __repr__(self): def hexid(maybe): if maybe is not None: return f'{maybe:#06x}' return 'None' return f'{self.__class__.__name__}: name: {self.name!r}, description:' \ f' {self.description!r}, parent_vendor: {hexid(self.parent_vendor)},' \ f' parent_device: {hexid(self.parent_device)}, parent_subsystem_vendor:' \ f' {hexid(self.parent_subsystem_vendor)},' \ f' parent_subsystem_device: {hexid(self.parent_subsystem_device)},' \ f' parent_driver: {self.parent_driver!r}' def _try_sysfs_read(self, *sub, default=None): try: return self._i2c_dev.joinpath(*sub).read_text().rstrip() except FileNotFoundError: return default def _try_sysfs_read_hex(self, *sub, default=None): try: return int(self._i2c_dev.joinpath(*sub).read_text(), base=16) except FileNotFoundError: return default
class I2CDevice(object): """ Class for communicating with an I2C device. Allows reading and writing 8-bit, 16-bit, and byte array values to registers on the device. It can handle signed, unsigned and endianness. :var uint address: Assigned I2C address. :var uint8 busid: Assigned IC2 bus identifier. :param uint address: I2C address. :param uint busid: IC2 bus identifier. :param class i2c_class: Class implementing the I2C reading interface. If None, smbus.SMBus will be used. """ def __init__(self, busnum, address, i2c_class=None): self._busnum = busnum self._address = address if i2c_class is None: from smbus import SMBus self._bus = SMBus(busnum) else: self._bus = i2c_class(busnum) self._logger = logging.getLogger( '/dev/i2c-{}/{:#x}'.format(busnum, address) ) def _debug(self): self._logger.setLevel(logging.DEBUG) self._logger.addHandler(logging.StreamHandler()) @property def busnum(self): return self._busnum @property def address(self): return self._address def write(self, value): """ Write the specified 8-bit value to the device base address. """ assert bound_bits(value, 8) self._bus.write_byte(self._address, value) self._logger.debug( 'Wrote value {:#x}'.format(value) ) def register_write_u8(self, register, value): """ Write an 8-bit value to the specified 8-bit register. """ assert bound_bits(register, 8) assert bound_bits(value, 8) self._bus.write_byte_data(self._address, register, value) self._logger.debug( 'Wrote to register {:#x} value {:#x}'.format(register, value) ) def register_write_u16(self, register, value): assert bound_bits(register, 8) assert bound_bits(value, 16) self._bus.write_word_data(self._address, register, value) self._logger.debug( 'Wrote to register pair {:#x}, {:#x} value {:#x} '.format( register, register + 1, value ) ) def read(self): """ Read the device base address and return a 8-bit value. """ result = self._bus.read_byte(self._address) & 0xFF self._logger.debug( 'Read value {:#x}'.format(result) ) return result def register_read_u8(self, register): """ Read the specified 8-bit register and return a 8-bit value. """ assert bound_bits(register, 8) result = self._bus.read_byte_data(self._address, register) & 0xFF self._logger.debug( 'Read from register {:#x} returns {:#x}'.format(register, result) ) return result def register_read_s8(self, register): """ Read the specified 8-bit register and return a signed 7-bit value. """ result = self.register_read_u8(register) if result > 127: result -= 256 self._logger.debug('... as signed: {:#x}'.format(result)) return result def register_read_u16(self, register, little_endian=True): """ Read the specified 8-bit register and return a 16-bit value with the specified endianness. Default is little endian, or least significant byte first. """ assert bound_bits(register, 8) result = self._bus.read_word_data(self._address, register) & 0xFFFF self._logger.debug( 'Read from register pair {:#x}, {:#x} value {:#x} '.format( register, register + 1, result ) ) # Swap bytes if using big endian because read_word_data assumes little # endian on ARM (little endian) systems. if not little_endian: result = ((result << 8) & 0xFF00) + (result >> 8) self._logger.debug('... as big endian: {:#x}'.format(result)) return result def register_read_s16(self, register, little_endian=True): """ Read the specified 8-bit register and return a signed 15-bit value with the specified endianness. Default is little endian, or least significant byte first. """ result = self.register_read_u16(register, little_endian) if result > 32767: result -= 65536 self._logger.debug('... as signed: {:#x}'.format(result)) return result def register_read_u16le(self, register): """ Same as register_read_u16 with endianness set to little endian. """ return self.register_read_u16(register, little_endian=True) def register_read_u16be(self, register): """ Same as register_read_u16 with endianness set to big endian. """ return self.register_read_u16(register, little_endian=False) def register_read_s16le(self, register): """ Same as register_read_s16 with endianness set to little endian. """ return self.register_read_s16(register, little_endian=True) def register_read_s16be(self, register): """ Same as register_read_s16 with endianness set to big endian. """ return self.register_read_s16(register, little_endian=False)
class I2CBus: # The device ids supported # Note: Under linux, the enumeration of the i2c devices is not guaranteed to match the BBB device id, i.e., i2c0 may # not map to /dev/i2c-0. Here is a link on this topic: https://datko.net/2013/11/03/bbb_i2c # What is important from a software perspective is that pins and the device id match since we don't use the linux # device name. That is, we need to use an id, 0, 1, 2, which corresponds to a specific device connected on specific # pins. # Note: I2C device 0 is not enabled by default. There is a way to enable it (see above) but there are also possible # conflicts with existing capes. DEV_I2C_0 = 0 DEV_I2C_1 = 1 DEV_I2C_2 = 2 # The following formatting is taken from struct and maps the character designations to number of bytes in the type __TYPE_SIZES = {'d': 8, # double - 8 bytes 'f': 4, # float - 4 bytes 'L': 4, # uint32 - 4 bytes 'l': 4, # int32 - 4 bytes 'H': 2, # uint16 - 2 bytes 'h': 2, # int16 - 2 bytes 'B': 1, # uint8 - 1 byte 'b': 1 # int8 - 1 byte } def __init__(self, device): try: self._smbus = SMBus(device) except RuntimeError: raise I2CBusError("Unable to open SMBus using {}".format(device)) def _read_multiple_bytes(self, address, offset, num_bytes): return self._smbus.read_i2c_block_data(address, offset, num_bytes) def _write_multiple_bytes(self, address, offset, byte_values): self._smbus.write_i2c_block_data(address, offset, list(byte_values)) def WriteUint8(self, address, offset, value): self._smbus.write_byte_data(address, offset, value) def WriteUint16(self, address, offset, value): self._smbus.write_word_data(address, offset, value) def WriteInt16(self, address, offset, value): self._smbus.write_word_data(address, offset, value) def WriteUint32(self, address, offset, value): bytes = bytearray(struct.pack('L', value)) self._write_multiple_bytes(address, offset, bytes) def WriteFloat(self, address, offset, value): bytes = bytearray(struct.pack('f',value)) self._write_multiple_bytes(address, offset, bytes) def WriteArray(self, address, offset, values, type, endian=sys.byteorder): # Convert each value to its byte representation and place into a bytearray before writing to the bus # Note: struct.pack returns a string representation of the value. For a 1-byte value, it is a # string representation of 1 byte, for a 2 or 4 byte value, it is a 2-byte of 4-byte representation # Therefore, it is necessary to concatentate the strings before converting to a bytearray if endian == 'little': format = '<'+type else: format = '>'+type byte_values = '' for value in values: byte_values += struct.pack(format, value) self._write_multiple_bytes(address, offset, bytearray(byte_values)) def ReadUint8(self, address, offset): return self._smbus.read_byte_data(address, offset) def ReadUint16(self, address, offset): return self._smbus.read_word_data(address, offset) def ReadInt16(self, address, offset): return self._smbus.read_word_data(address, offset) def ReadUint32(self, address, offset): bytes = self._read_multiple_bytes(address, offset, I2CBus.__TYPE_SIZES['L']) return struct.unpack('L', str(bytearray(bytes)))[0] def ReadInt32(self, address, offset): bytes = self._read_multiple_bytes(address, offset, I2CBus.__TYPE_SIZES['l']) return struct.unpack('l', str(bytearray(bytes)))[0] def ReadFloat(self, address, offset): values = self._read_multiple_bytes(address, offset, I2CBus.__TYPE_SIZES['f']) return struct.unpack('f', str(bytearray(values)))[0] def ReadArray(self, address, offset, num_values, type, endian=sys.byteorder): # Create a format specifier based on the number of values requested. # All of the values will be read as the same type, e.g., all floats, all long, etc # The format specifies the number of float values to convert format = '%s%s' % (num_values, type) # Calculate number of bytes to read # - num_values is the number of values to read # - num_bytes is num_values * size of each value num_bytes = num_values * I2CBus.__TYPE_SIZES[type] # It turns out that reading i2c block data is not supported on all Raspberry Pi's (probably a OS/driver difference) # The Pi 2 running Jessie doesn't support i2c (i2cget with no arguments shows no 'i' option) # The Pi 3 running Jessie does support i2c (i2cget with no argument shows 'i' option) # So, we need to support both options bytes = self._read_multiple_bytes(address, offset, num_bytes) # Match the endianess of the request. Default is platform endianess # struct provides a format specifier for endianess if endian == 'little': format = '<'+format else: format = '>'+format return list(struct.unpack(format, str(bytearray(bytes))))
class bq34z100g1(object): def __init__(self, address=0x55, bus=1): self._address = address self._bus = SMBus(bus) @staticmethod def log(msg): print(msg) @staticmethod def ftol(byte: int): liste = [] liste.append(1) if byte & 1 > 0 else liste.append(0) liste.append(1) if byte & 2 > 0 else liste.append(0) liste.append(1) if byte & 4 > 0 else liste.append(0) liste.append(1) if byte & 8 > 0 else liste.append(0) liste.append(1) if byte & 16 > 0 else liste.append(0) liste.append(1) if byte & 32 > 0 else liste.append(0) liste.append(1) if byte & 64 > 0 else liste.append(0) liste.append(1) if byte & 128 > 0 else liste.append(0) return liste def openConfig(self): self._bus.write_word_data(self._address, 0x00, 0x0414) self._bus.write_word_data(self._address, 0x00, 0x3672) self._bus.write_byte_data(self._address, 0x61, 0x00) def setConfig(self): self.openConfig() self._bus.write_byte_data(self._address, 0x3e, 0x30) self._bus.write_byte_data(self._address, 0x3f, 0x00) # self._bus.write_byte_data(self._address, 0x4a, 0x00) # self._bus.write_byte_data(self._address, 0x4b, 0x00) def _writeValue(self, cmd, text): try: self._bus.write_word_data(self._address, cmd, text) except: bq34z100g1.log("Couldn't write to i2c bus") def _readValue(self, register: int, length: int = 2) -> int: try: if length == 2: return self._bus.read_word_data(self._address, register) if length == 1: return self._bus.read_byte_data(self._address, register) else: print("is not supported by now") # do again two times? return -1 except: bq34z100g1.log("Could not read i2c bus") return -1 def _readSignedValue(self, register: int, length: int = 2) -> int: value = self._readValue(register, length) if length == 2: if value <= 32767: return value else: return value - 65535 if length == 1: if value <= 128: return value else: return value - 256 def get_temperature(self): # 0x0c """returns Temperature in °C""" return round((self._readValue(0x0C) * 0.1) - 273.15, 2) def get_internal_temperature(self): # 0x2a """return internal Temperature in °C""" return round((self._readValue(0x2A) * 0.1) - 273.15, 2) def get_voltage(self): # 0x08,0x09 """return Voltage in mV""" return self._readValue(0x08) def get_current(self): # 0x10,0x11 """returns Current in mA""" return self._readSignedValue(0x10) def get_power(self): # 0x26,0x27 """returns current Power Usage""" return self._readSignedValue(0x26) def get_capacity(self): # 0x04,0x05 """returns Capacity in mAh""" return self._readValue(0x04) def get_full_capacity(self): # 0x06,0x07 """returns Capacity when full in mAh""" return self._readValue(0x06) def get_design_capacity(self): # 0x0c,0x0d """returns Design Capacity in mAh""" return self._readValue(0x3C) def get_cycle_count(self): # 0x2c """returns the amount of Cycles the Battery has run""" return self._readValue(0x2C, 1) def get_state_of_charge(self): # 0x02 """return State of Charge in %""" return self._readValue(0x02, 1) def get_flagsa(self): #0x0e, 0x0f return str(str(self.ftol(int(self._readValue(0x0e, 1)))) + "\n" + str(self.ftol(int(self._readValue(0x0f, 1))))) def get_flagsb(self): #0x12, 0x13 return str(str(self.ftol(int(self._readValue(0x12, 1)))) + "\n" + str(self.ftol(int(self._readValue(0x13, 1))))) def get_ctrl_statusa(self): return self.ftol(int(self._readValue(0x00, 1))) def get_ctrl_statusb(self): return self.ftol(int(self._readValue(0x01, 1))) def get_max_error(self): return self._readValue(0x03, 1) def get_avg_time_to_empty(self): return self._readValue(0x18) def get_avg_time_to_full(self): # 0x1a,0x1b return self._readValue(0x1A) def get_state_of_health(self): # 0x2e,0x2f return self._readValue(0x2E) def get_qmax_time(self): return self._readValue(0x74) def get_learned_status(self): return self.ftol(int(self._readValue(0x63,1)))
class Device(object): """Class for communicating with an I2C device using the adafruit-pureio pure python smbus library, or other smbus compatible I2C interface. Allows reading and writing 8-bit, 16-bit, and byte array values to registers on the device.""" def __init__(self, address, busnum, i2c_interface=None): """Create an instance of the I2C device at the specified address on the specified I2C bus number.""" self._address = address if i2c_interface is None: # Use pure python I2C interface if none is specified. # import Adafruit_PureIO.smbus self._bus = SMBus(busnum) else: # Otherwise use the provided class to create an smbus interface. self._bus = i2c_interface(busnum) self._logger = logging.getLogger('Adafruit_I2C.Device.Bus.{0}.Address.{1:#0X}' \ .format(busnum, address)) def writeRaw8(self, value): """Write an 8-bit value on the bus (without register).""" value = value & 0xFF self._bus.write_byte(self._address, value) self._logger.debug("Wrote 0x%02X", value) def write8(self, register, value): """Write an 8-bit value to the specified register.""" value = value & 0xFF self._bus.write_byte_data(self._address, register, value) self._logger.debug("Wrote 0x%02X to register 0x%02X", value, register) def write16(self, register, value): """Write a 16-bit value to the specified register.""" value = value & 0xFFFF self._bus.write_word_data(self._address, register, value) self._logger.debug("Wrote 0x%04X to register pair 0x%02X, 0x%02X", value, register, register+1) def writeList(self, register, data): """Write bytes to the specified register.""" self._bus.write_i2c_block_data(self._address, register, data) self._logger.debug("Wrote to register 0x%02X: %s", register, data) def readList(self, register, length): """Read a length number of bytes from the specified register. Results will be returned as a bytearray.""" results = self._bus.read_i2c_block_data(self._address, register, length) self._logger.debug("Read the following from register 0x%02X: %s", register, results) return results def readRaw8(self): """Read an 8-bit value on the bus (without register).""" result = self._bus.read_byte(self._address) & 0xFF self._logger.debug("Read 0x%02X", result) return result def readU8(self, register): """Read an unsigned byte from the specified register.""" result = self._bus.read_byte_data(self._address, register) & 0xFF self._logger.debug("Read 0x%02X from register 0x%02X", result, register) return result def readS8(self, register): """Read a signed byte from the specified register.""" result = self.readU8(register) if result > 127: result -= 256 return result def readU16(self, register, little_endian=True): """Read an unsigned 16-bit value from the specified register, with the specified endianness (default little endian, or least significant byte first).""" result = self._bus.read_word_data(self._address,register) & 0xFFFF self._logger.debug("Read 0x%04X from register pair 0x%02X, 0x%02X", result, register, register+1) # Swap bytes if using big endian because read_word_data assumes little # endian on ARM (little endian) systems. if not little_endian: result = ((result << 8) & 0xFF00) + (result >> 8) return result def readS16(self, register, little_endian=True): """Read a signed 16-bit value from the specified register, with the specified endianness (default little endian, or least significant byte first).""" result = self.readU16(register, little_endian) if result > 32767: result -= 65536 return result def readU16LE(self, register): """Read an unsigned 16-bit value from the specified register, in little endian byte order.""" return self.readU16(register, little_endian=True) def readU16BE(self, register): """Read an unsigned 16-bit value from the specified register, in big endian byte order.""" return self.readU16(register, little_endian=False) def readS16LE(self, register): """Read a signed 16-bit value from the specified register, in little endian byte order.""" return self.readS16(register, little_endian=True) def readS16BE(self, register): """Read a signed 16-bit value from the specified register, in big endian byte order.""" return self.readS16(register, little_endian=False)
class PiPuckMotorServer(object): """ROS Node to expose topics relating to the Pi-puck motors.""" def __init__(self): """Initialise node.""" self._bus = SMBus(I2C_CHANNEL) rospy.on_shutdown(self.close_bus) self._steps_right_pub = rospy.Publisher('motors/steps_right', UInt16, queue_size=10) self._steps_left_pub = rospy.Publisher('motors/steps_left', UInt16, queue_size=10) self._real_steps_right_pub = rospy.Publisher('motors/real_steps_right', Int64, queue_size=10) self._real_steps_left_pub = rospy.Publisher('motors/real_steps_left', Int64, queue_size=10) self._odometry_pub = rospy.Publisher('motors/odometry', Odometry, queue_size=10) rospy.init_node("motors") rospy.Subscriber("motors/speed_right", Float32, self.callback_right) rospy.Subscriber("motors/speed_left", Float32, self.callback_left) self._rate = rospy.Rate(rospy.get_param('~rate', 10)) tf_prefix_key = rospy.search_param("tf_prefix") if tf_prefix_key: tf_prefix = rospy.get_param(tf_prefix_key, None) else: tf_prefix = None if tf_prefix is not None and not tf_prefix.endswith("/"): tf_prefix += "/" self._reference_frame = REFERENCE_FRAME_ID if tf_prefix: self._reference_frame = tf_prefix + self._reference_frame self._left_overflows = 0 self._right_overflows = 0 self._left_steps_previous = 0 self._right_steps_previous = 0 self._real_left_steps_previous = 0 self._real_right_steps_previous = 0 self._left_motor_speed = 0 self._right_motor_speed = 0 self._estimate_x = 0 self._estimate_y = 0 self._estimate_theta = 0 self._last_measurement_time = rospy.get_time() @staticmethod def convert_speed(speed): """Convert input speed data into a speed value for the stepper motors.""" speed = float(speed) if speed > 1.0: speed = 1.0 elif speed < -1.0: speed = -1.0 return int(speed * MAX_SPEED) def callback_left(self, data): """Handle requests for speed change of left motor.""" self._left_motor_speed = data.data self._bus.write_word_data( EPUCK_I2C_ADDR, LEFT_MOTOR_SPEED, PiPuckMotorServer.convert_speed(self._left_motor_speed)) def callback_right(self, data): """Handle request for speed change of right motor.""" self._right_motor_speed = data.data self._bus.write_word_data( EPUCK_I2C_ADDR, RIGHT_MOTOR_SPEED, PiPuckMotorServer.convert_speed(self._right_motor_speed)) def close_bus(self): """Close the I2C bus after the ROS Node is shutdown.""" self._bus.write_word_data(EPUCK_I2C_ADDR, LEFT_MOTOR_SPEED, 0) self._bus.write_word_data(EPUCK_I2C_ADDR, RIGHT_MOTOR_SPEED, 0) self._bus.close() @staticmethod def euler_to_quaternion(yaw, pitch, roll): """Convert euler angles of pitch, roll, and yaw to a quaternion. Based on code from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles. We don't use tf.transforms here to reduce dependence on tf and increase performance on this simple task. """ c_y = cos(yaw * 0.5) s_y = sin(yaw * 0.5) c_p = cos(pitch * 0.5) s_p = sin(pitch * 0.5) c_r = cos(roll * 0.5) s_r = sin(roll * 0.5) q_w = c_y * c_p * c_r + s_y * s_p * s_r q_x = c_y * c_p * s_r - s_y * s_p * c_r q_y = s_y * c_p * s_r + c_y * s_p * c_r q_z = s_y * c_p * c_r - c_y * s_p * s_r return Quaternion(x=q_x, y=q_y, z=q_z, w=q_w) def run(self): """ROS Node server.""" initial_left_steps = int( self._bus.read_word_data(EPUCK_I2C_ADDR, LEFT_MOTOR_STEPS)) initial_right_steps = int( self._bus.read_word_data(EPUCK_I2C_ADDR, RIGHT_MOTOR_STEPS)) # Initial steps are set to counter the fact that the node may have previously run and the # e-puck firmware will not necessarily have been reset. self._left_steps_previous = initial_left_steps self._right_steps_previous = initial_right_steps self._real_left_steps_previous = initial_left_steps self._real_right_steps_previous = initial_right_steps while not rospy.is_shutdown(): # Get current steps left_steps = int( self._bus.read_word_data(EPUCK_I2C_ADDR, LEFT_MOTOR_STEPS)) right_steps = int( self._bus.read_word_data(EPUCK_I2C_ADDR, RIGHT_MOTOR_STEPS)) # Get step measurement time measurement_time = rospy.get_time() # Check for overflows and underflows if left_steps + (MAX_MOTOR_STEPS_RAW / 2.0) < self._left_steps_previous: self._left_overflows += 1 elif left_steps > self._left_steps_previous + ( MAX_MOTOR_STEPS_RAW / 2.0): self._left_overflows -= 1 if right_steps + (MAX_MOTOR_STEPS_RAW / 2.0) < self._right_steps_previous: self._right_overflows += 1 elif right_steps > self._right_steps_previous + ( MAX_MOTOR_STEPS_RAW / 2.0): self._right_overflows -= 1 # Calculate the real steps left and right accounting for overflows real_left_steps = left_steps + self._left_overflows * MAX_MOTOR_STEPS_RAW real_right_steps = right_steps + self._right_overflows * MAX_MOTOR_STEPS_RAW # Publish raw and real steps self._steps_right_pub.publish(right_steps) self._steps_left_pub.publish(left_steps) self._real_steps_right_pub.publish(real_right_steps) self._real_steps_left_pub.publish(real_left_steps) # Calculate step changes delta_left = (real_left_steps - self._real_left_steps_previous) * MOTOR_STEP_DISTANCE delta_right = (real_right_steps - self._real_right_steps_previous ) * MOTOR_STEP_DISTANCE #Calculate delta steps and turn delta_theta = (delta_right - delta_left) / WHEEL_DISTANCE delta_steps = (delta_right + delta_left) / 2.0 # Update the estimate for x, y, and rotation self._estimate_x += delta_steps * cos(self._estimate_theta + delta_theta / 2.0) self._estimate_y += delta_steps * sin(self._estimate_theta + delta_theta / 2.0) self._estimate_theta += delta_theta # Update previous steps to current self._left_steps_previous = left_steps self._right_steps_previous = right_steps self._real_left_steps_previous = real_left_steps self._real_right_steps_previous = real_right_steps # Send odometry odometry_message = Odometry() odometry_message.pose.pose.orientation = PiPuckMotorServer.euler_to_quaternion( self._estimate_theta, 0, 0) odometry_message.pose.pose.position.x = self._estimate_x odometry_message.pose.pose.position.y = self._estimate_y odometry_message.twist.twist.linear.x = delta_steps / ( measurement_time - self._last_measurement_time) odometry_message.twist.twist.angular.z = delta_theta / ( measurement_time - self._last_measurement_time) odometry_message.header.frame_id = self._reference_frame self._odometry_pub.publish(odometry_message) self._last_measurement_time = measurement_time self._rate.sleep()
class I2C(_Basic_class): MASTER = 0 SLAVE = 1 RETRY = 5 def __init__(self, *args, **kargs): # *args表示位置参数(形式参数),可无,; **kargs表示默认值参数,可无。 super().__init__() self._bus = 1 self._smbus = SMBus(self._bus) def _i2c_write_byte(self, addr, data): # i2C 写系列函数 self._debug("_i2c_write_byte: [0x{:02X}] [0x{:02X}]".format( addr, data)) return self._smbus.write_byte(addr, data) def _i2c_write_byte_data(self, addr, reg, data): self._debug( "_i2c_write_byte_data: [0x{:02X}] [0x{:02X}] [0x{:02X}]".format( addr, reg, data)) return self._smbus.write_byte_data(addr, reg, data) def _i2c_write_word_data(self, addr, reg, data): self._debug( "_i2c_write_word_data: [0x{:02X}] [0x{:02X}] [0x{:04X}]".format( addr, reg, data)) return self._smbus.write_word_data(addr, reg, data) def _i2c_write_i2c_block_data(self, addr, reg, data): self._debug( "_i2c_write_i2c_block_data: [0x{:02X}] [0x{:02X}] {}".format( addr, reg, data)) return self._smbus.write_i2c_block_data(addr, reg, data) def _i2c_read_byte(self, addr): # i2C 读系列函数 self._debug("_i2c_read_byte: [0x{:02X}]".format(addr)) return self._smbus.read_byte(addr) def _i2c_read_i2c_block_data(self, addr, reg, num): self._debug( "_i2c_read_i2c_block_data: [0x{:02X}] [0x{:02X}] [{}]".format( addr, reg, num)) return self._smbus.read_i2c_block_data(addr, reg, num) def is_ready(self, addr): addresses = self.scan() if addr in addresses: return True else: return False def scan(self): # 查看有哪些i2c设备 cmd = "i2cdetect -y %s" % self._bus _, output = self.run_command( cmd) # 调用basic中的方法,在linux中运行cmd指令,并返回运行后的内容 outputs = output.split('\n')[1:] # 以回车符为分隔符,分割第二行之后的所有行 self._debug("outputs") addresses = [] for tmp_addresses in outputs: if tmp_addresses == "": continue tmp_addresses = tmp_addresses.split(':')[1] tmp_addresses = tmp_addresses.strip().split( ' ') # strip函数是删除字符串两端的字符,split函数是分隔符 for address in tmp_addresses: if address != '--': addresses.append(int(address, 16)) self._debug("Conneceted i2c device: %s" % addresses) # append以列表的方式添加address到addresses中 return addresses def send(self, send, addr, timeout=0): # 发送数据,addr为从机地址,send为数据 if isinstance(send, bytearray): data_all = list(send) elif isinstance(send, int): data_all = [] d = "{:X}".format(send) d = "{}{}".format( "0" if len(d) % 2 == 1 else "", d ) # format是将()中的内容对应填入{}中,()中的第一个参数是一个三目运算符,if条件成立则为“0”,不成立则为“”(空的意思),第二个参数是d,此行代码意思为,当字符串为奇数位时,在字符串最强面添加‘0’,否则,不添加, 方便以下函数的应用 # print(d) for i in range(len(d) - 2, -1, -2): # 从字符串最后开始取,每次取2位 tmp = int(d[i:i + 2], 16) # 将两位字符转化为16进制 # print(tmp) data_all.append(tmp) # 添加到data_all数组中 data_all.reverse() elif isinstance(send, list): data_all = send else: raise ValueError( "send data must be int, list, or bytearray, not {}".format( type(send))) if len(data_all) == 1: # 如果data_all只有一组数 data = data_all[0] self._i2c_write_byte(addr, data) elif len(data_all) == 2: # 如果data_all只有两组数 reg = data_all[0] data = data_all[1] self._i2c_write_byte_data(addr, reg, data) elif len(data_all) == 3: # 如果data_all只有三组数 reg = data_all[0] data = (data_all[2] << 8) + data_all[1] self._i2c_write_word_data(addr, reg, data) else: reg = data_all[0] data = list(data_all[1:]) self._i2c_write_i2c_block_data(addr, reg, data) def recv(self, recv, addr=0x00, timeout=0): # 接收数据 if isinstance(recv, int): # 将recv转化为二进制数 result = bytearray(recv) elif isinstance(recv, bytearray): result = recv else: return False for i in range(len(result)): result[i] = self._i2c_read_byte(addr) return result def mem_write(self, data, addr, memaddr, timeout=5000, addr_size=8): #memaddr match to chn if isinstance(data, bytearray): data_all = list(data) elif isinstance(data, list): data_all = data elif isinstance(data, int): data_all = [] data = "%x" % data if len(data) % 2 == 1: data = "0" + data # print(data) for i in range(0, len(data), 2): # print(data[i:i+2]) data_all.append(int(data[i:i + 2], 16)) else: raise ValueError( "memery write require arguement of bytearray, list, int less than 0xFF" ) # print(data_all) self._i2c_write_i2c_block_data(addr, memaddr, data_all) def mem_read(self, data, addr, memaddr, timeout=5000, addr_size=8): # 读取数据 if isinstance(data, int): num = data elif isinstance(data, bytearray): num = len(data) else: return False result = bytearray(self._i2c_read_i2c_block_data(addr, memaddr, num)) return result def readfrom_mem_into(self, addr, memaddr, buf): buf = self.mem_read(len(buf), addr, memaddr) return buf def writeto_mem(self, addr, memaddr, data): self.mem_write(data, addr, memaddr) # i2c = I2C() # i2c.scan() # i2c.mem_write(0xff53773, 20, 20)
class ads1015: def __init__(self, addr=0x48): self.i2c = SMBus(1) self.addr = addr self.constants() self.mux = self.get(self.MUX) self.gain = self.get(self.GAIN) self.mode = self.get(self.MODE) self.data_rate = self.get(self.DATA_RATE) self.comp_mode = self.get(self.COMP_MODE) self.comp_pol = self.get(self.COMP_POL) self.comp_lat = self.get(self.COMP_LAT) self.comp_que = self.get(self.COMP_QUE) def constants(self): # Register adresses self.CONV_REG = 0b00 self.CONFIG_REG = 0b01 self.LO_TRESH_REG = 0b10 self.HI_TRESH_REG = 0b11 # Input MUX self.MUX = (12, 3, 'mux') self.MUX_DIFF_01 = 0b000 self.MUX_DIFF_03 = 0b001 self.MUX_DIFF_13 = 0b010 self.MUX_DIFF_23 = 0b011 self.MUX_0 = 0b100 self.MUX_1 = 0b101 self.MUX_2 = 0b110 self.MUX_3 = 0b111 # Gain self.GAIN = (9, 3, 'gain') self.GAIN_0 = 0b000 # +/-6.144V self.GAIN_1 = 0b001 # +/-4.096V self.GAIN_2 = 0b010 # +/-2.048V self.GAIN_3 = 0b011 # +/-1.024V self.GAIN_4 = 0b100 # +/-0.512V self.GAIN_5 = 0b101 # +/-0.256V self.VRANGE = [6.144, 4.096, 2.048, 1.024, 0.512, 0.256] # Conversion mode self.MODE = (8, 1, 'mode') self.MODE_CONTINUOUS = 0b0 self.MODE_SINGLE_SHOT = 0b1 # Data rate self.DATA_RATE = (5, 3, 'data_rate') self.DATA_RATE_128SPS = 0b000 self.DATA_RATE_250SPS = 0b001 self.DATA_RATE_490SPS = 0b010 self.DATA_RATE_920SPS = 0b011 self.DATA_RATE_1600SPS = 0b100 self.DATA_RATE_2400SPS = 0b101 self.DATA_RATE_3300SPS = 0b110 # Comperator mode self.COMP_MODE = (4, 1, 'comp_mode') self.COMP_MODE_NORMAL = 0b0 self.COMP_MODE_WINDOW = 0b1 # Comperator polarity self.COMP_POL = (3, 1, 'comp_pol') self.COMP_POL_NORMAL = 0b0 self.COMP_POL_INVERTED = 0b1 # Comperator latching self.COMP_LAT = (2, 1, 'comp_lat') self.COMP_LAT_OFF = 0b0 self.COMP_LAT_ON = 0b1 # Comperator queue self.COMP_QUE = (0, 2, 'comp_que') self.COMP_QUE_1 = 0b00 self.COMP_QUE_2 = 0b01 self.COMP_QUE_4 = 0b10 self.COMP_QUE_OFF = 0b11 def bitwrite16(self, word, value, bitshift, bitwidth): return ((word & (0xFFFF - ((2**bitwidth - 1) << bitshift))) | (value << bitshift)) def byteswap(self, word): return ((word >> 8) & 0x00FF) + ((word << 8) & 0xFF00) def get(self, parameter): config = self.byteswap( self.i2c.read_word_data(self.addr, self.CONFIG_REG)) return (config >> parameter[0]) & (2**parameter[1] - 1) def set(self, parameter, value): config = self.byteswap( self.i2c.read_word_data(self.addr, self.CONFIG_REG)) config = self.bitwrite16( config, value, bitshift=parameter[0], bitwidth=parameter[1]) & 0x7FFF self.i2c.write_word_data(self.addr, self.CONFIG_REG, self.byteswap(config)) setattr(self, parameter[2], value) def convert(self): config = self.byteswap( self.i2c.read_word_data(self.addr, self.CONFIG_REG)) config = config | 0x8000 self.i2c.write_word_data(self.addr, self.CONFIG_REG, self.byteswap(config)) def isconverting(self): config = self.byteswap( self.i2c.read_word_data(self.addr, self.CONFIG_REG)) return ((config & 0x8000) == 0) def read(self, mux=None): if mux in [0, 1, 2, 3, 4, 5, 6, 7]: self.set(self.MUX, mux) if self.mode == self.MODE_SINGLE_SHOT: self.convert() while self.isconverting(): pass return self.byteswap(self.i2c.read_word_data(self.addr, self.CONV_REG)) def voltage(self, mux=None): return self.read(mux) * self.VRANGE[self.gain] / 0x7FFF def set_input(self, pin=None): if pin in [0, 1, 2, 3]: self.set(self.MUX, pin + 4) def set_gain(self, gain=None): if gain in [0, 1, 2, 3, 4, 5]: self.set(self.GAIN, gain)
class I2CInterface(object): """ I2C interface class. It writes VU Meter stereo data into the I2C port defined in the configuration file. The I2C addresses for the left and right channel should be defined in config.txt. Default values 0x21, 0x20. The property output.size defines the number of bits used for the I2C VU Meter. The default value is 10. For the smaller/larger numbers the bit_patterns array should be modified accordingly. """ def __init__(self, config, data_source): """ Initializer """ self.data_source = data_source self.port = config[I2C_INTERFACE][PORT] self.left_channel_address = config[I2C_INTERFACE][LEFT_CHANNEL_ADDRESS] self.right_channel_address = config[I2C_INTERFACE][RIGHT_CHANNEL_ADDRESS] self.output_size = config[I2C_INTERFACE][OUTPUT_SIZE] self.update_period = config[I2C_INTERFACE][UPDATE_PERIOD] self.step = int(100/self.output_size) if "win" in sys.platform: self.i2c_interface = DummySMBus() else: from smbus import SMBus self.i2c_interface = SMBus(self.port) self.i2c_interface.write_byte_data(self.left_channel_address, 0x00, 0x00) self.i2c_interface.write_byte_data(self.left_channel_address, 0x01, 0x00) self.i2c_interface.write_byte_data(self.right_channel_address, 0x00, 0x00) self.i2c_interface.write_byte_data(self.right_channel_address, 0x01, 0x00) self.bit_patterns = {} self.bit_patterns[10] = 0b10000000 self.bit_patterns[20] = 0b11000000 self.bit_patterns[30] = 0b11100000 self.bit_patterns[40] = 0b11110000 self.bit_patterns[50] = 0b11111000 self.bit_patterns[60] = 0b11111100 self.bit_patterns[70] = 0b11111110 self.bit_patterns[80] = 0b11111111 self.bit_patterns[90] = 0b111111111 self.bit_patterns[100] = 0b1111111111 f = "{:0" + str(self.output_size) + "b}" self.logging_template = "I2C left: " + f + " right: " + f def start_writing(self): """ Start writing thread """ self.running = True thread = Thread(target = self.write_data) thread.start() def write_data(self): """ Method of the writing thread """ while self.running: v = self.data_source.get_value() left = self.get_bits(v[0]) right = self.get_bits(v[1]) logging.debug(self.logging_template.format(left, right)) self.i2c_interface.write_word_data(self.left_channel_address, 0x12, left) self.i2c_interface.write_word_data(self.right_channel_address, 0x12, right) time.sleep(self.update_period) def stop_writing(self): """ Stop writing thread and nullify values in I2C """ self.running = False time.sleep(self.update_period) self.i2c_interface.write_word_data(self.left_channel_address, 0x12, 0) self.i2c_interface.write_word_data(self.right_channel_address, 0x12, 0) def get_bits(self, n): """ Return bit pattern for the defined value in range 0-100 :param n: data value :return: bit pattern """ if n == 0: return 0 else: k = int(math.ceil(n / self.step)) * self.step v = self.bit_patterns[k] return v
class Device(object): """Class for communicating with an I2C device using the smbus library. Allows reading and writing 8-bit, 16-bit, and byte array values to registers on the device.""" def __init__(self, address, busnum): """Create an instance of the I2C device at the specified address on the specified I2C bus number.""" self._address = address self._bus = SMBus(busnum) def writeRaw8(self, value): """Write an 8-bit value on the bus (without register).""" value = value & 0xFF self._bus.write_byte(self._address, value) def write8(self, register, value): """Write an 8-bit value to the specified register.""" value = value & 0xFF self._bus.write_byte_data(self._address, register, value) def write16(self, register, value): """Write a 16-bit value to the specified register.""" value = value & 0xFFFF self._bus.write_word_data(self._address, register, value) def writeList(self, register, data): """Write bytes to the specified register.""" self._bus.write_i2c_block_data(self._address, register, data) def readList(self, register, length): """Read a length number of bytes from the specified register. Results will be returned as a bytearray.""" results = self._bus.read_i2c_block_data(self._address, register, length) return results def readRaw8(self): """Read an 8-bit value on the bus (without register).""" result = self._bus.read_byte(self._address) & 0xFF return result def readU8(self, register): """Read an unsigned byte from the specified register.""" result = self._bus.read_byte_data(self._address, register) & 0xFF return result def readS8(self, register): """Read a signed byte from the specified register.""" result = self.readU8(register) if result > 127: result -= 256 return result def readU16(self, register, little_endian=True): """Read an unsigned 16-bit value from the specified register, with the specified endianness (default little endian, or least significant byte first).""" result = self._bus.read_word_data(self._address, register) & 0xFFFF # Swap bytes if using big endian because read_word_data assumes little # endian on ARM (little endian) systems. if not little_endian: result = ((result << 8) & 0xFF00) + (result >> 8) return result def readS16(self, register, little_endian=True): """Read a signed 16-bit value from the specified register, with the specified endianness (default little endian, or least significant byte first).""" result = self.readU16(register, little_endian) if result > 32767: result -= 65536 return result def readU16LE(self, register): """Read an unsigned 16-bit value from the specified register, in little endian byte order.""" return self.readU16(register, little_endian=True) def readU16BE(self, register): """Read an unsigned 16-bit value from the specified register, in big endian byte order.""" return self.readU16(register, little_endian=False) def readS16LE(self, register): """Read a signed 16-bit value from the specified register, in little endian byte order.""" return self.readS16(register, little_endian=True) def readS16BE(self, register): """Read a signed 16-bit value from the specified register, in big endian byte order.""" return self.readS16(register, little_endian=False)
class LinuxI2cBus: """A Linux I²C device, which is itself an I²C bus. Should not be instantiated directly; use `LinuxI2c.find_devices` instead. This type mimics the `smbus.SMBus` read/write/close APIs. However, `open` does not take any parameters, and not all APIs are available. """ # note: this is not a liquidctl BaseBus, as that would cause # find_liquidctl_devices to try to directly instantiate it def __init__(self, i2c_dev): self._i2c_dev = i2c_dev self._smbus = None try: assert i2c_dev.name.startswith('i2c-') self._number = int(i2c_dev.name[4:]) except: raise ValueError(f'cannot infer bus number') def find_devices(self, drivers, **kwargs): """Probe drivers and find compatible devices in this bus.""" for drv in drivers: yield from drv.probe(self, **kwargs) def open(self): """Open the I²C bus.""" if not self._smbus: try: self._smbus = SMBus(self._number) except FileNotFoundError: if Path('/sys/class/i2c-dev').exists(): raise raise OSError('kernel module i2c-dev not loaded') from None def read_byte(self, address): """Read a single byte from a device.""" value = self._smbus.read_byte(address) _LOGGER.debug('read byte @ 0x%02x: 0x%02x', address, value) return value def read_byte_data(self, address, register): """Read a single byte from a designated register.""" value = self._smbus.read_byte_data(address, register) _LOGGER.debug('read byte data @ 0x%02x:0x%02x: 0x%02x', address, register, value) return value def read_word_data(self, address, register): """Read a single 2-byte word from a given register.""" value = self._smbus.read_word_data(address, register) _LOGGER.debug('read word data @ 0x%02x:0x%02x: 0x%04x', address, register, value) return value def read_block_data(self, address, register): """Read a block of up to 32 bytes from a given register.""" data = self._smbus.read_block_data(address, register) _LOGGER.debug('read block data @ 0x%02x:0x%02x: %r', address, register, LazyHexRepr(data)) return data def write_byte(self, address, value): """Write a single byte to a device.""" _LOGGER.debug('writing byte @ 0x%02x: 0x%02x', address, value) return self._smbus.write_byte(address, value) def write_byte_data(self, address, register, value): """Write a single byte to a designated register.""" _LOGGER.debug('writing byte data @ 0x%02x:0x%02x: 0x%02x', address, register, value) return self._smbus.write_byte_data(address, register, value) def write_word_data(self, address, register, value): """Write a single 2-byte word to a designated register.""" _LOGGER.debug('writing word data @ 0x%02x:0x%02x: 0x%04x', address, register, value) return self._smbus.write_word_data(address, register, value) def write_block_data(self, address, register, data): """Write a block of byte data to a given register.""" _LOGGER.debug('writing block data @ 0x%02x:0x%02x: %r', address, register, LazyHexRepr(data)) return self._smbus.write_block_data(address, register, data) def close(self): """Close the I²C connection.""" if self._smbus: self._smbus.close() self._smbus = None def load_eeprom(self, address): """Return EEPROM name and data in `address`, or None if N/A.""" # uses kernel facilities to avoid directly reading from the EEPROM # or managing its pages, also avoiding the need for unsafe=smbus dev = f'{self._number}-{address:04x}' try: name = self._i2c_dev.joinpath(dev, 'name').read_text().strip() eeprom = self._i2c_dev.joinpath(dev, 'eeprom').read_bytes() return LinuxEeprom(name, eeprom) except Exception as err: return None @property def name(self): return self._i2c_dev.name @property def description(self): return self._try_sysfs_read('name') @property def parent_vendor(self): return self._try_sysfs_read_hex('device/vendor') @property def parent_device(self): return self._try_sysfs_read_hex('device/device') @property def parent_subsystem_vendor(self): return self._try_sysfs_read_hex('device/subsystem_vendor') @property def parent_subsystem_device(self): return self._try_sysfs_read_hex('device/subsystem_device') @property def parent_driver(self): try: return Path( os.readlink(self._i2c_dev.joinpath('device/driver'))).name except FileNotFoundError: return None def __str__(self): if self.description: return f'{self.name}: {self.description}' return self.name def __repr__(self): def hexid(maybe): if maybe is not None: return f'{maybe:#06x}' return 'None' return f'{self.__class__.__name__}: name: {self.name!r}, ' \ f'description: {self.description!r}, ' \ f'parent_vendor: {hexid(self.parent_vendor)}, ' \ f'parent_device: {hexid(self.parent_device)}, ' \ f'parent_subsystem_vendor: {hexid(self.parent_subsystem_vendor)}, ' \ f'parent_subsystem_device: {hexid(self.parent_subsystem_device)}, ' \ f'parent_driver: {self.parent_driver!r}' def _try_sysfs_read(self, *sub, default=None): try: return self._i2c_dev.joinpath(*sub).read_text().rstrip() except FileNotFoundError: return default def _try_sysfs_read_hex(self, *sub, default=None): try: return int(self._i2c_dev.joinpath(*sub).read_text(), base=16) except FileNotFoundError: return default
class SHT31D: def __init__(self, i2cAddr): self.address = i2cAddr self.bus = SMBus(1) time.sleep(0.05) def readTemperatureAndHumidity(self): self.writeCommand(SHT31DConstant.SHT31_MEAS_HIGHREP) time.sleep(0.015) TempMSB, TempLSB, TempCRC, HumMSB, HumLSB, HumCRC = self.readData( SHT31DConstant.SHT31_MEAS_HIGHREP) STemp = TempMSB * 16 * 16 + TempLSB SHum = HumMSB * 16 * 16 + HumLSB # print TempCRC if TempCRC != self.CRC8([TempMSB, TempLSB]): TC = "nan" else: TC = -45 + 175 * STemp / float(2**16 - 1) # print HumCRC if HumCRC != self.CRC8([HumMSB, HumLSB]): RH = "nan" else: RH = 100 * SHum / float(2**16 - 1) return (TC, RH) # return def readTemperature(self): (TC, RH) = self.readTemperatureAndHumidity() return TC def readHumidity(self): (TC, RH) = self.readTemperatureAndHumidity() return RH def clearStatus(self): self.writeCommand(SHT31DConstant.SHT31_CLEARSTATUS) def readStatus(self): self.writeCommand(SHT31DConstant.SHT31_READSTATUS) StatusMSB, StatusLSB, StatusCRC = self.readData( SHT31DConstant.SHT31_READSTATUS, 3) if StatusCRC != self.CRC8([StatusMSB, StatusLSB]): return 'nan' else: return StatusMSB << 8 | StatusLSB def heaterEnable(self): self.writeCommand(SHT31DConstant.SHT31_HEATEREN) def heaterDisable(self): self.writeCommand(SHT31DConstant.SHT31_HEATERDIS) def softReset(self): self.writeCommand(SHT31DConstant.SHT31_SOFTRESET) def writeCommand(self, cmd): self.bus.write_word_data(self.address, cmd >> 8, cmd & 0xFF) def readData(self, cmd, numBytes=6): return self.bus.read_i2c_block_data(self.address, cmd, numBytes) def CRC8(self, buffer): """ Polynomial 0x31 (x8 + x5 +x4 +1) """ polynomial = 0x31 crc = 0xFF index = 0 # print buffer for index in range(0, len(buffer)): crc ^= int(buffer[index]) for i in range(8, 0, -1): if crc & 0x80: crc = (crc << 1) ^ polynomial else: crc = (crc << 1) # print crc & 0xFF return crc & 0xFF