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
