class VCNL4010:
"""Vishay VCNL4010 proximity and ambient light sensor."""
def __init__(self):
self._device = SMBus(2)
self.led_current = 20
self.frequency = FREQUENCY_390K625
self._write_u8(_VCNL4010_INTCONTROL, 0x08)
def _read_u8(self, address):
# Read an 8-bit unsigned value from the specified 8-bit address.
with SMBus(2) as self._device:
read = self._device.read_byte_data(_VCNL4010_I2CADDR_DEFAULT, address)
return read
def _write_u8(self, address, val):
# Write an 8-bit unsigned value to the specified 8-bit address.
with SMBus(2) as self._device:
self._device.write_byte_data(_VCNL4010_I2CADDR_DEFAULT, address, val)
def _read_u16BE(self, address):
with SMBus(2) as self._device:
read_block = self._device.read_i2c_block_data(_VCNL4010_I2CADDR_DEFAULT, address, 2)
return (read_block[0] << 8) | read_block[1]
@property
def proximity(self):
"""The detected proximity of an object in front of the sensor. This
is a unit-less unsigned 16-bit value (0-65535) INVERSELY proportional
to the distance of an object in front of the sensor (up to a max of
~200mm). For example a value of 10 is an object farther away than a
value of 1000. Note there is no conversion from this value to absolute
distance possible, you can only make relative comparisons.
"""
# Clear interrupt.
status = self._read_u8(_VCNL4010_INTSTAT)
status &= ~0x80
self._write_u8(_VCNL4010_INTSTAT, status)
# Grab a proximity measurement.
self._write_u8(_VCNL4010_COMMAND, _VCNL4010_MEASUREPROXIMITY)
# Wait for result, then read and return the 16-bit value.
while True:
result = self._read_u8(_VCNL4010_COMMAND)
if result & _VCNL4010_PROXIMITYREADY:
return self._read_u16BE(_VCNL4010_PROXIMITYDATA)
def setup_eon_fan():
global LEON
#os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
os.system("echo 0 > /sys/module/dwc3_msm/parameters/otg_switch")
bus = SMBus(7, force=True)
try:
bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts
bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable
bus.write_byte_data(0x21, 0x02, 0x2) # needed?
bus.write_byte_data(0x21, 0x04, 0x4) # manual override source
except IOError:
print("LEON detected")
#os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg")
os.system("echo 0 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg")
LEON = True
bus.close()
def read_arduino(slave_addr, sensor_type):
try:
I2Cbus = SMBus(BUS)
# byte = convert_bytes_to_list(bytes(str(sensor_type), "utf-8"))
byte = int(sensor_type)
# I2Cbus.write_i2c_block_data(slave_addr, MEMORY_ADDR, byte)
I2Cbus.write_byte_data(slave_addr, MEMORY_ADDR, byte)
response = I2Cbus.read_i2c_block_data(slave_addr, MEMORY_ADDR,
BYTE_LEN)
# response = I2Cbus.read_byte_data(slave_addr, MEMORY_ADDR)
I2Cbus.close()
return "ok", int(bytearray(response).decode("utf-8", "ignore"))
# return "ok", response
except:
I2Cbus.close()
print("failed to retrieve data from arduino...")
print(traceback.format_exc())
return "error", traceback.format_exc()
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):
"""Create an instance of the I2C device at the specified address on the
specified I2C bus number."""
self._address = address
self._bus = SMBus(busnum)
self._logger = logging.getLogger('Rover_log')
def writeByte(self, value):
"""Write an 8-bit value"""
value = value & 0xFF
self._bus.write_byte(self._address, register, value)
self._logger.debug("Wrote 0x%02X to register 0x%02X", value, register)
def write8(self, value, register=0):
"""Write an 8-bit value to the specified register. use register 0 if not needed"""
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 readU8(self, register=0):
"""Read an unsigned byte from the specified register. use 0 if egister is not needed"""
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 writeList(self, data, register=0):
"""Write bytes to the specified register. Use 0 if register is not needed"""
self._bus.write_i2c_block_data(self._address, register, data)
self._logger.debug("Wrote to register 0x%02X: %s", register, data)
def readList(self, length, register=0):
"""Read a length number of bytes from the specified register, use 0 if register is not needed. 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
class MPU6050:
def __init__(self,):
self.bus = SMBus(1)
self.address = 0x68
self.bus.write_byte_data(self.address, 0x6b, 0)
self.bus.write_byte_data(self.address, 26, 3)
def read_data(self,):
raw_data = self.bus.read_i2c_block_data(self.address, 0x3B, 14)
acc_x = (raw_data[0] << 8) + raw_data[1]
if acc_x >= 0x8000:
acc_x = -((65535 - acc_x) + 1)
acc_x = acc_x / 16384.0
acc_y = (raw_data[2] << 8) + raw_data[3]
if acc_y >= 0x8000:
acc_y = -((65535 - acc_y) + 1)
acc_y = acc_y / 16384.0
acc_z = (raw_data[4] << 8) + raw_data[5]
if acc_z >= 0x8000:
acc_z = -((65535 - acc_z) + 1)
acc_z = acc_z / 16384.0
z2 = acc_z * acc_z
x_rot = math.degrees(math.atan2(acc_y, math.sqrt((acc_x * acc_x) + z2)))
y_rot = -math.degrees(math.atan2(acc_x, math.sqrt((acc_y * acc_y) + z2)))
gyro_x = (raw_data[8] << 8) + raw_data[9]
if gyro_x >= 0x8000:
gyro_x = -((65535 - gyro_x) + 1)
gyro_x = gyro_x / 131.0
gyro_y = (raw_data[10] << 8) + raw_data[11]
if gyro_y >= 0x8000:
gyro_y = -((65535 - gyro_y) + 1)
gyro_y = gyro_y / 131.0
return -x_rot, y_rot, acc_z, -gyro_x, gyro_y
class MPU6050Interface:
def __init__(self):
'''
Python interface for MPU6050.
Provides python wrappers around C-level MPU6050 routines
'''
# setup c routines by loading shared object library
self.so_file = "MPU_6050_Module.so" # .so on Linux, .pyd on Windows
# open i2c bus 1
self.bus = SMBus(1)
# initialize hardware
# MPU6050.init()
self.bus.write_byte_data(MPU_ADDR, SMPLRT_DIV, 0x07)
self.bus.write_byte_data(MPU_ADDR, PWR_MGMT_1, 0x01)
self.bus.write_byte_data(MPU_ADDR, CONFIG, 0)
self.bus.write_byte_data(MPU_ADDR, GYRO_CONFIG, 24)
self.bus.write_byte_data(MPU_ADDR, INT_ENABLE, 0x01)
def read_raw_data(self, addr):
''' returns a 16-bit value from a certain address '''
high_byte = self.bus.read_byte_data(MPU_ADDR, addr)
low_byte = self.bus.read_byte_data(MPU_ADDR, addr + 1)
value = (high_byte << 8) | low_byte
return value
def get_acc_xyz(self):
''' returns accelerometer readings in g '''
Ax = self.read_raw_data(ACCEL_XOUT_H)
Ay = self.read_raw_data(ACCEL_YOUT_H)
Az = self.read_raw_data(ACCEL_ZOUT_H)
# divide by sensitivity scale factor
Ax /= 16384
Ay /= 16384
Az /= 16384
return (Ax, Ay, Az)
def get_gyr_xyz(self):
''' returns accelerometer readings in g '''
Gx = self.read_raw_data(GYRO_XOUT_H)
Gy = self.read_raw_data(GYRO_YOUT_H)
Gz = self.read_raw_data(GYRO_ZOUT_H)
# divide by sensitivity scale factor
Gx /= 131
Gy /= 131
Gz /= 131
return (Gx, Gy, Gz)
class WiiController:
def __init__(self, delay=0.05):
self.delay = delay
if rpi.RPI_REVISION == 1:
i2c_bus = 0
elif rpi.RPI_REVISION == 2:
i2c_bus = 1
elif rpi.RPI_REVISION == 3:
i2c_bus = 1
else:
raise OSError("Unable to determine Raspberry Pi revision.")
self.bus = SMBus(i2c_bus)
self.bus.write_byte_data(0x52, 0x40, 0x00)
time.sleep(0.1)
@cached(TTLCache(maxsize=1, ttl=0.0166))
def read(self):
self.bus.write_byte(0x52, 0x00)
time.sleep(self.delay)
return [(0x17 + (0x17 ^ self.bus.read_byte(0x52))) % 256
for _ in range(6)]
class Relay(object):
COMMAND_RELAY_OFF = 0x00
COMMAND_RELAY_ON = 0x01
COMMAND_SET_ADDRESS = 0x03
def __init__(self, bus_number, device_address):
self.bus = SMBus(bus_number)
self.device_address = device_address
def __del__(self):
self.bus.close()
def off(self):
"""
Turns the relay off
"""
self.bus.write_byte_data(self.device_address, self.COMMAND_RELAY_OFF, 0x01)
def on(self):
"""
Turns the relay on
"""
self.bus.write_byte_data(self.device_address, self.COMMAND_RELAY_ON, 0x01)
def set_address(self, address):
"""
Sets the I2C address of Qwiic Relay.
"""
if address < 0x07 or address > 0x78:
raise ValueError('Invalid I2C address')
self.bus.write_byte_data(self.device_address, self.COMMAND_SET_ADDRESS, address)
sleep(0.05)
class GDK101(object):
'''driver code for GDK101 gamma ray sensor'''
def __init__(self, busnum, addr):
self.bus = SMBus(busnum)
self.addr = addr
rc = self.reset()
if rc != 1: # reset failed
raise Exception('GKD101: failed to reset sensor')
def _readGKD101(self, cmd):
'''implement simple I²C interface of GDK101
- send command
- block-read two bytes
'''
self.bus.write_byte_data(self.addr, 0, cmd)
return self.bus.read_i2c_block_data(self.addr, 0, 2)
def reset(self):
d = self._readGKD101(CMD_reset)
return d[0]
def read1(self):
''' read 1 min average'''
d = self._readGKD101(CMD_readDose1)
return d[0] + d[1] / 100.
def read10(self):
''' read 10 min sliding average'''
d = self._readGKD101(CMD_readDose10)
return d[0] + d[1] / 100.
def version(self):
'''return firmware version'''
fw = self._readGKD101(CMD_firmware)
return str(fw[0] + fw[1] / 10.)
def close(self):
'''close bus'''
self.bus.close()
class I2CController:
#
# I2C
#
def __init__(self):
try:
self.i2cBus = SMBus(
1) # 0 = /dev/i2c-0 (port I2C0), 1 = /dev/i2c-1 (port I2C1)
except:
print("Error opening I2C bus")
raise I2CUnavailableError("Error opening I2C bus")
return
self.MCUI2CAddress = 0x8
self.CNCI2CAddress = 0x10
def checkI2CStatus(self):
self.i2cBus.read_byte_data(80, 0)
# Write a single register
# cmd = 0x32
# value = 0x80
# bus.write_byte_data(self.MCUI2CAddress, cmd, value)
# Write an array of registers
# values = [0xff, 0xff, 0xff, 0xff, 0xff, 0xff]
# bus.write_i2c_block_data(self.MCUI2CAddress, cmd, values)
def sendToMCU(self, cmdId, value):
self.i2cBus.write_byte_data(self.MCUI2CAddress, cmdId, value)
return
def sendToCNC(self, cmdId, value):
self.i2cBus.write_byte_data(self.CNCI2CAddress, cmdId, value)
return
def closeI2C(self):
self.i2cBus.close()
class multiplexer:
def __init__(self, bus, address):
self.bus = SMBus(bus)
self.address = address
def channel(self, channel):
""" Activate an i2c channel.
Values 0-7 indicate the channel,
any other value turns off all channels.
"""
if (channel == 0): action = 0x01
elif (channel == 1): action = 0x02
elif (channel == 2): action = 0x04
elif (channel == 3): action = 0x08
elif (channel == 4): action = 0x10
elif (channel == 5): action = 0x20
elif (channel == 6): action = 0x40
elif (channel == 7): action = 0x80
else: action = 0x00
self.bus.write_byte_data(
self.address, 0x04,
action) #0x04 is the register for switching channels
def setup_eon_fan():
if not EON:
return
os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
bus = SMBus(7, force=True)
bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts
bus.write_byte_data(0x21, 0x03,
0x1) # set drive current and global interrupt disable
bus.write_byte_data(0x21, 0x02, 0x2) # needed?
bus.write_byte_data(0x21, 0x04, 0x4) # manual override source
bus.close()
def set_eon_fan(val):
global last_eon_fan_val
if not EON:
return
if last_eon_fan_val is None or last_eon_fan_val != val:
bus = SMBus(7, force=True)
bus.write_byte_data(0x21, 0x04, 0x2)
bus.write_byte_data(0x21, 0x03, (val * 2) + 1)
bus.write_byte_data(0x21, 0x04, 0x4)
bus.close()
last_eon_fan_val = val
def set_eon_fan(val):
global LEON, last_eon_fan_val
if last_eon_fan_val is None or last_eon_fan_val != val:
bus = SMBus(7, force=True)
if LEON:
i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
bus.write_i2c_block_data(0x3d, 0, [i])
else:
bus.write_byte_data(0x21, 0x04, 0x2)
bus.write_byte_data(0x21, 0x03, (val * 2) + 1)
bus.write_byte_data(0x21, 0x04, 0x4)
bus.close()
last_eon_fan_val = val
def setup_fan():
os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
bus = SMBus(7, force=True)
try:
bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts
bus.write_byte_data(
0x21, 0x03, 0x1) # set drive current and global interrupt disable
bus.write_byte_data(0x21, 0x02, 0x2) # needed?
bus.write_byte_data(0x21, 0x04, 0x4) # manual override source
print("OP detected")
return False
except IOError:
print("LEON detected")
return True
bus.close()
class LSM6:
"""
Usage is:
with LSM6() as imu:
"""
DS33_SA0_HIGH_ADDRESS = 0b1101011
DS33_SA0_LOW_ADDRESS = 0b1101010
def __init__(self):
self.bus = SMBus(1)
self.address = self.DS33_SA0_HIGH_ADDRESS # TODO: high address is assumed, SA0 connected to supply voltage
# Enable Accelerometer high performance mode, ODR = 1000, (+-2g)
self.bus.write_byte_data(self.address, RegAddr['CTRL1_XL'], 0x80)
# Enable Gyro high performance mode, ODR = 1000, (245 dps)
self.bus.write_byte_data(self.address, RegAddr['CTRL2_G'], 0x80)
# Enable auto increment
self.bus.write_byte_data(self.address, RegAddr['CTRL3_C'], 0x04)
def read_values(self):
"""
This function reads the IMUs measurements
Returns:
The sensors measurements in the following format
(Gyro X, Gyro Y, Gyro Z, Acceleration X, Acc Y, Acc Z)
"""
value_list = self.bus.read_i2c_block_data(
self.address, RegAddr['OUTX_L_G'],
RegAddr['OUTZ_H_XL'] - RegAddr['OUTX_L_G'])
value_list = [(value_list[i], value_list[i + 1])
for i in range(0, len(value_list), 2)]
value_list = list(map(_calc_value, value_list))
for i in range(3):
value_list[i] *= 4.375 # convert to MilliG
for i in range(3, 6):
value_list[i] *= 0.61 # convert to MilliDPS
return tuple(value_list)
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, exc_traceback):
self.bus.close()
return True
def set_eon_fan(val):
global LEON, last_eon_fan_val
if last_eon_fan_val is None or last_eon_fan_val != val:
bus = SMBus(7, force=True)
if LEON:
try:
i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val]
bus.write_i2c_block_data(0x3d, 0, [i])
except IOError:
# tusb320
if val == 0:
bus.write_i2c_block_data(0x67, 0xa, [0])
else:
bus.write_i2c_block_data(0x67, 0xa, [0x20])
bus.write_i2c_block_data(0x67, 0x8, [(val - 1) << 6])
else:
bus.write_byte_data(0x21, 0x04, 0x2)
bus.write_byte_data(0x21, 0x03, (val * 2) + 1)
bus.write_byte_data(0x21, 0x04, 0x4)
bus.close()
last_eon_fan_val = val
class I2C(object):
def __init__(self, bus_number, device_address):
self.smbus = SMBus(bus_number)
self.device_address = device_address
def set_bit(self,register, index):
read_value = np.uint8(self.smbus.read_byte_data(self.device_address, register))
bit_array = np.unpackbits(read_value)
bit_array[index*-1-1] = 1
self.smbus.write_byte_data(self.device_address,register, np.packbits(bit_array))
def clear_bit(self,register, index):
read_value = np.uint8(self.smbus.read_byte_data(self.device_address, register))
bit_array = np.unpackbits(read_value)
bit_array[index*-1-1] = 0
self.smbus.write_byte_data(self.device_address,register, np.packbits(bit_array))
def read_bit(self,register, index):
value = self.smbus.read_byte_data(device_address, register)
eight_bit = format(value, '08b')
return eight_bit[index*(-1)-1]
def write_byte(self,register, value):
self.smbus.write_byte_data(self.device_address,register, int(value))
def read_byte(self, register):
return self.smbus.read_byte_data(self.device_address,register)
def read_block(self,register, number_to_read):
return self.smbus.read_i2c_block_data(self.device_address, register, number_to_read)
def byte_array_to_float32(self,array):
return np.fromiter(array[::-1], dtype=np.uint8).view('<f4')
class Robot(object):
MODE_SIMULATION = 0
MODE_CROSS_COMPILATION = 1
MODE_REMOTE_CONTROL = 2
def __init__(self):
self.time = 0
self.tof = VL53L0X.VL53L0X(i2c_bus=4, i2c_address=0x29)
self.tof.open()
self.tof.start_ranging(VL53L0X.Vl53l0xAccuracyMode.BETTER)
self.bus = SMBus(I2C_CHANNEL)
self.busFT903 = SMBus(FT903_I2C_CHANNEL)
self.devices = {}
self.previousTime = None
self.customData = ''
for name in DistanceSensor.groundNames + DistanceSensor.proximityNames:
self.devices[name] = DistanceSensor(name)
for name in LightSensor.names:
self.devices[name] = LightSensor(name)
for name in PositionSensor.names:
self.devices[name] = PositionSensor(name)
for name in Motor.names:
self.devices[name] = Motor(name)
for name in LED.names:
self.devices[name] = LED(name)
for name in Accelerometer.names:
self.devices[name] = Accelerometer(name)
for name in Gyro.names:
self.devices[name] = Gyro(name)
self.devices['tof'] = DistanceSensor('tof')
print('Starting controller.')
def step(self, duration, blocking=False):
if blocking:
now = time.time()
if self.previousTime is not None:
diff = now - (self.previousTime + 0.001 * duration)
if diff < 0:
time.sleep(-diff)
self.time += 0.001 * duration
else:
self.time += diff + 0.001 * duration
else:
self.time += 0.001 * duration
self.previousTime = time.time()
self.devices['tof'].value = self.tof.get_distance() / 1000.0
actuatorsData = []
# left motor
# 0.00628 = (2 * pi) / encoder_resolution
leftSpeed = int(self.devices['left wheel motor'].getVelocity() /
0.0068)
actuatorsData.append(leftSpeed & 0xFF)
actuatorsData.append((leftSpeed >> 8) & 0xFF)
# right motor
rightSpeed = int(self.devices['right wheel motor'].getVelocity() /
0.0068)
actuatorsData.append(rightSpeed & 0xFF)
actuatorsData.append((rightSpeed >> 8) & 0xFF)
# speaker sound
actuatorsData.append(0)
# LED1, LED3, LED5, LED7 on/off flag
actuatorsData.append((self.devices['led0'].value & 0x1)
| (self.devices['led2'].value & 0x2)
| (self.devices['led4'].value & 0x4)
| (self.devices['led6'].value & 0x8))
# LED2 R/G/B
actuatorsData.append(
int(((self.devices['led1'].value >> 16) & 0xFF) / 2.55))
actuatorsData.append(
int(((self.devices['led1'].value >> 8) & 0xFF) / 2.55))
actuatorsData.append(int((self.devices['led1'].value & 0xFF) / 2.55))
# LED4 R/G/B
actuatorsData.append(
int(((self.devices['led3'].value >> 16) & 0xFF) / 2.55))
actuatorsData.append(
int(((self.devices['led3'].value >> 8) & 0xFF) / 2.55))
actuatorsData.append(int((self.devices['led3'].value & 0xFF) / 2.55))
# LED6 R/G/B
actuatorsData.append(
int(((self.devices['led5'].value >> 16) & 0xFF) / 2.55))
actuatorsData.append(
int(((self.devices['led5'].value >> 8) & 0xFF) / 2.55))
actuatorsData.append(int((self.devices['led5'].value & 0xFF) / 2.55))
# LED8 R/G/B
actuatorsData.append(
int(((self.devices['led7'].value >> 16) & 0xFF) / 2.55))
actuatorsData.append(
int(((self.devices['led7'].value >> 8) & 0xFF) / 2.55))
actuatorsData.append(int((self.devices['led7'].value & 0xFF) / 2.55))
# Settings
actuatorsData.append(0)
# Checksum
checksum = 0
for data in actuatorsData:
checksum ^= data
actuatorsData.append(checksum)
if len(actuatorsData) != ACTUATORS_SIZE:
sys.exit('Wrond actuator data size.')
# communication with i2c bus with main board address
write = i2c_msg.write(MAIN_ADDR, actuatorsData)
read = i2c_msg.read(MAIN_ADDR, SENSORS_SIZE)
try:
self.bus.i2c_rdwr(write, read)
except:
return
sensorsData = list(read)
checksum = 0
for data in sensorsData:
checksum ^= checksum ^ data
if sensorsData[SENSORS_SIZE - 1] != checksum:
print('Wrogn receiving checksum')
return
if len(sensorsData) != SENSORS_SIZE:
sys.exit('Wrond actuator data size.')
# Read and assign DistanceSensor values
for i in range(8):
self.devices[DistanceSensor.proximityNames[i]].value = (
sensorsData[i * 2] & 0x00FF) | (
(sensorsData[i * 2 + 1] << 8) & 0xFF00)
# Read and assign LightSensor values
for i in range(8):
self.devices[LightSensor.names[i]].value = sensorsData[
i * 2 + 16] + (sensorsData[i * 2 + 17] << 8)
# Read and assign PositionSensor values
for i in range(2):
val = (sensorsData[i * 2 + 41] & 0x00FF) | (
(sensorsData[i * 2 + 42] << 8) & 0xFF00)
# Pythonic way to get signed 16bit integers :O
if val > 2**15:
val -= 2**16
# 159.23 = encoder_resolution/ (2 * pi)
self.devices[PositionSensor.names[i]].value = val / 159.23
# communication with the pi-puck extension FT903 address
mapping = [2, 1, 0]
for i in range(3):
ledName = 'pi-puck led %d' % i
if self.devices[ledName].changed:
ledValue = (
(0x01 if
((self.devices[ledName].value >> 16) & 0xFF) > 0 else 0) |
(0x02 if
((self.devices[ledName].value >> 8) & 0xFF) > 0 else 0) |
(0x04 if (self.devices[ledName].value & 0xFF) > 0 else 0))
self.busFT903.write_byte_data(FT903_ADDR, mapping[i], ledValue)
self.devices[ledName].changed = False
# communication with i2c bus with ground sensors board address
groundSensorsEnabled = False
for name in DistanceSensor.groundNames:
if self.devices[name].getSamplingPeriod() > 0:
groundSensorsEnabled = True
break
if groundSensorsEnabled:
read = i2c_msg.read(GROUND_ADDR, GROUND_SENSORS_SIZE)
try:
self.bus.i2c_rdwr(read)
except:
return
groundData = list(read)
for i in range(3):
self.devices[DistanceSensor.groundNames[i]].value = (
groundData[i * 2] << 8) + groundData[i * 2 + 1]
# communication with the i2c bus with the extension board address
# if self.devices['accelerometer'].getSamplingPeriod() > 0 or self.devices['gyro'].getSamplingPeriod() > 0:
# imuRead = i2c_msg.read(IMU_ADDR, IMU_SIZE)
# self.bus.i2c_rdwr(imuRead)
# imuData = list(imuRead)
# if len(imuData) != IMU_SIZE:
# sys.exit('Wrond IMU data size.')
# print(imuData)
def getAccelerometer(self, name):
if name in self.devices and isinstance(self.devices[name],
Accelerometer):
return self.devices[name]
print('No Accelerometer device named "%s"\n' % name)
return None
def getDistanceSensor(self, name):
if name in self.devices and isinstance(self.devices[name],
DistanceSensor):
return self.devices[name]
print('No DistanceSensor device named "%s"\n' % name)
return None
def getLED(self, name):
if name in self.devices and isinstance(self.devices[name], LED):
return self.devices[name]
print('No LED device named "%s"\n' % name)
return None
def getLightSensor(self, name):
if name in self.devices and isinstance(self.devices[name],
LightSensor):
return self.devices[name]
print('No LightSensor device named "%s"\n' % name)
return None
def getMotor(self, name):
if name in self.devices and isinstance(self.devices[name], Motor):
return self.devices[name]
print('No Motor device named "%s"\n' % name)
return None
def getPositionSensor(self, name):
if name in self.devices and isinstance(self.devices[name],
PositionSensor):
return self.devices[name]
print('No PositionSensor device named "%s"\n' % name)
return None
def getName(self):
return 'e-puck'
def getTime(self):
return self.time
def getSupervisor(self):
return False
def getSynchronization(self):
return False
def getBasicTimeStep(self):
return 32
def getNumberOfDevices(self):
return len(self.devices)
def getDeviceByIndex(self, index):
return self.devices[index]
def getMode(self):
return Robot.MODE_CROSS_COMPILATION
def getCustomData(self):
return self.customData
def setCustomData(self, data):
self.customData = data
from smbus2 import SMBus
import numpy as np
import matplotlib.pyplot as plot
# --------------------------------------------
dataMag = []
buffer = 0
bufferSize = 600
count = 0
x = []
bus = SMBus(CHANNEL)
# bus.write_byte_data(DEVICE_ADDRESS, offset, msg)
bus.write_byte_data(DEVICE_ADDRESS, SI72XX_ARAUTOINC, ARAUTOINC__ARAUTOINC_MASK)
bus.write_byte_data(DEVICE_ADDRESS, SI72XX_DSPSIGSEL, DSPSIGSEL__MAG_VAL_SEL)
bus.write_byte_data(DEVICE_ADDRESS, SI72XX_CTRL4, 0x04)
bus.write_byte_data(DEVICE_ADDRESS, SI72XX_SLTIME, 0x00)
bus.write_byte_data(DEVICE_ADDRESS, SI72XX_CTRL3, 0x02)
bus.write_byte_data(DEVICE_ADDRESS, SI72XX_POWER_CTRL, 0x00)
start = time.time()
while (count < bufferSize):
# Write a single register
#bufferL = bus.read_byte_data(DEVICE_ADDRESS, SI72XX_DSPSIGL)
#bufferM = bus.read_byte_data(DEVICE_ADDRESS, SI72XX_DSPSIGM)
#bus.write_byte_data(DEVICE_ADDRESS, SI72XX_POWER_CTRL, 0x00)
bufferData = bus.read_i2c_block_data(DEVICE_ADDRESS,SI72XX_DSPSIGM,2)
signal = (bufferData[0] & 0x7F) << 8 | bufferData[1]
class BME280:
def __init__(self):
self.bus_number = 1
self.i2c_address = 0x76
self.bus = SMBus(self.bus_number)
self.digT = []
self.digP = []
self.digH = []
self.t_fine = 0.0
def writeReg(self, reg_address, data):
self.bus.write_byte_data(self.i2c_address, reg_address, data)
def get_calib_param(self):
calib = []
for i in range(0x88, 0x88 + 24):
calib.append(self.bus.read_byte_data(self.i2c_address, i))
calib.append(self.bus.read_byte_data(self.i2c_address, 0xA1))
for i in range(0xE1, 0xE1 + 7):
calib.append(self.bus.read_byte_data(self.i2c_address, i))
self.digT.append((calib[1] << 8) | calib[0])
self.digT.append((calib[3] << 8) | calib[2])
self.digT.append((calib[5] << 8) | calib[4])
self.digP.append((calib[7] << 8) | calib[6])
self.digP.append((calib[9] << 8) | calib[8])
self.digP.append((calib[11] << 8) | calib[10])
self.digP.append((calib[13] << 8) | calib[12])
self.digP.append((calib[15] << 8) | calib[14])
self.digP.append((calib[17] << 8) | calib[16])
self.digP.append((calib[19] << 8) | calib[18])
self.digP.append((calib[21] << 8) | calib[20])
self.digP.append((calib[23] << 8) | calib[22])
self.digH.append(calib[24])
self.digH.append((calib[26] << 8) | calib[25])
self.digH.append(calib[27])
self.digH.append((calib[28] << 4) | (0x0F & calib[29]))
self.digH.append((calib[30] << 4) | ((calib[29] >> 4) & 0x0F))
self.digH.append(calib[31])
for i in range(1, 2):
if self.digT[i] & 0x8000:
self.digT[i] = (-self.digT[i] ^ 0xFFFF) + 1
for i in range(1, 8):
if self.digP[i] & 0x8000:
self.digP[i] = (-self.digP[i] ^ 0xFFFF) + 1
for i in range(0, 6):
if self.digH[i] & 0x8000:
self.digH[i] = (-self.digH[i] ^ 0xFFFF) + 1
def readData(self):
data = []
for i in range(0xF7, 0xF7 + 8):
data.append(self.bus.read_byte_data(self.i2c_address, i))
temp_raw = (data[3] << 12) | (data[4] << 4) | (data[5] >> 4)
pres_raw = (data[0] << 12) | (data[1] << 4) | (data[2] >> 4)
hum_raw = (data[6] << 8) | data[7]
return self.compensate_T(temp_raw), self.compensate_P(
pres_raw), self.compensate_H(hum_raw)
def compensate_T(self, adc_T):
global t_fine
v1 = (adc_T / 16384.0 - self.digT[0] / 1024.0) * self.digT[1]
v2 = (adc_T / 131072.0 - self.digT[0] / 8192.0) * (
adc_T / 131072.0 - self.digT[0] / 8192.0) * self.digT[2]
t_fine = v1 + v2
temperature = t_fine / 5120.0
#print "temp : %-6.2f ℃" % (temperature)
return temperature
def compensate_P(self, adc_P):
global t_fine
pressure = 0.0
v1 = (t_fine / 2.0) - 64000.0
v2 = (((v1 / 4.0) * (v1 / 4.0)) / 2048) * self.digP[5]
v2 = v2 + ((v1 * self.digP[4]) * 2.0)
v2 = (v2 / 4.0) + (self.digP[3] * 65536.0)
v1 = (((self.digP[2] * (((v1 / 4.0) * (v1 / 4.0)) / 8192)) / 8) +
((self.digP[1] * v1) / 2.0)) / 262144
v1 = ((32768 + v1) * self.digP[0]) / 32768
if v1 == 0:
return 0
pressure = ((1048576 - adc_P) - (v2 / 4096)) * 3125
if pressure < 0x80000000:
pressure = (pressure * 2.0) / v1
else:
pressure = (pressure / v1) * 2
v1 = (self.digP[8] * (((pressure / 8.0) *
(pressure / 8.0)) / 8192.0)) / 4096
v2 = ((pressure / 4.0) * self.digP[7]) / 8192.0
pressure = pressure + ((v1 + v2 + self.digP[6]) / 16.0)
#print "pressure : %7.2f hPa" % (pressure/100)
return pressure / 100
def compensate_H(self, adc_H):
global t_fine
var_h = t_fine - 76800.0
if var_h != 0:
var_h = (adc_H -
(self.digH[3] * 64.0 + self.digH[4] / 16384.0 * var_h)
) * (self.digH[1] / 65536.0 *
(1.0 + self.digH[5] / 67108864.0 * var_h *
(1.0 + self.digH[2] / 67108864.0 * var_h)))
else:
return 0
var_h = var_h * (1.0 - self.digH[0] * var_h / 524288.0)
if var_h > 100.0:
var_h = 100.0
elif var_h < 0.0:
var_h = 0.0
#print "hum : %6.2f ％" % (var_h)
return var_h
def setup(self):
osrs_t = 1 #Temperature oversampling x 1
osrs_p = 1 #Pressure oversampling x 1
osrs_h = 1 #Humidity oversampling x 1
mode = 3 #Normal mode
t_sb = 5 #Tstandby 1000ms
filter = 0 #Filter off
spi3w_en = 0 #3-wire SPI Disable
ctrl_meas_reg = (osrs_t << 5) | (osrs_p << 2) | mode
config_reg = (t_sb << 5) | (filter << 2) | spi3w_en
ctrl_hum_reg = osrs_h
self.writeReg(0xF2, ctrl_hum_reg)
self.writeReg(0xF4, ctrl_meas_reg)
self.writeReg(0xF5, config_reg)
def get_x_rotation(x, y, z):
radians = math.atan(x / dist(y, z))
return radians
def get_y_rotation(x, y, z):
radians = math.atan(y / dist(x, z))
return radians
def get_z_rotation(x, y, z):
radians = math.atan(z / dist(x, y))
return radians
bus.write_byte_data(DEV_ADDR, 0x6B, 0b00000000)
try:
while True:
x = read_data(register_accel_xout_h)
y = read_data(register_accel_yout_h)
z = read_data(register_accel_zout_h)
aX = get_x_rotation(accel_g(x), accel_g(y), accel_g(z))
aY = get_y_rotation(accel_g(x), accel_g(y), accel_g(z))
aZ = get_z_rotation(accel_g(x), accel_g(y), accel_g(z))
data = str(aX) + ' , ' + str(aY) + ' , ' + str(aZ) + '$'
print(data)
time.sleep(0.3)
except KeyboardInterrupt:
print("\nInterrupted!")
except:
print("\nClosing socket")
class AXP209(object):
def __init__(self, bus=0):
"""
Initialize the AXP209 object
:param bus: i2c bus number or a SMBus object
:type bus: Integer or SMBus object
"""
if isinstance(bus, int):
self.bus = SMBus(bus, force=True)
self.autocleanup = True
else:
self.bus = bus
self.autocleanup = False
# force ADC enable for battery voltage and current
self.adc_enable1 = 0xc3
def __enter__(self):
return self
def __exit__(self, exc_type, exc_val, exc_tb):
if self.autocleanup:
self.close()
def close(self):
self.bus.close()
@property
def gpio2_output(self):
pass
@gpio2_output.setter
def gpio2_output(self, val):
flags = GPIO012_FEATURE_SET_FLAGS()
if bool(val):
flags.gpio_function = 0b111
else:
flags.gpio_function = 0b000
self.bus.write_byte_data(AXP209_ADDRESS, GPIO2_FEATURE_SET_REG, flags.asbyte)
@property
def adc_enable1(self):
flags = ADC_ENABLE1_FLAGS()
flags.asbyte = self.bus.read_byte_data(AXP209_ADDRESS, ADC_ENABLE1_REG)
return flags
@adc_enable1.setter
def adc_enable1(self, flags):
if hasattr(flags, "asbyte"):
flags = flags.asbyte
self.bus.write_byte_data(AXP209_ADDRESS, ADC_ENABLE1_REG, flags)
@property
def power_input_status(self):
flags = POWER_INPUT_STATUS_FLAGS()
flags.asbyte = self.bus.read_byte_data(AXP209_ADDRESS, POWER_INPUT_STATUS_REG)
return flags
@property
def battery_current_direction(self):
return bool(self.power_input_status.battery_current_direction)
@property
def power_operating_mode(self):
flags = POWER_OPERATING_STATUS_FLAGS()
flags.asbyte = self.bus.read_byte_data(AXP209_ADDRESS, POWER_OPERATING_MODE_REG)
return flags
@property
def battery_exists(self):
return bool(self.power_operating_mode.battery_exists)
@property
def battery_charging(self):
return bool(self.power_operating_mode.battery_charging)
@property
def battery_voltage(self):
""" Returns voltage in mV """
msb = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_VOLTAGE_MSB_REG)
lsb = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_VOLTAGE_LSB_REG)
voltage_bin = msb << 4 | lsb & 0x0f
return voltage_bin * 1.1
@property
def battery_charge_current(self):
""" Returns current in mA """
msb = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_CHARGE_CURRENT_MSB_REG)
lsb = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_CHARGE_CURRENT_LSB_REG)
# (12 bits)
charge_bin = msb << 4 | lsb & 0x0f
# 0 mV -> 000h, 0.5 mA/bit FFFh -> 1800 mA
return charge_bin * 0.5
@property
def battery_discharge_current(self):
""" Returns current in mA """
msb = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_DISCHARGE_CURRENT_MSB_REG)
lsb = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_DISCHARGE_CURRENT_LSB_REG)
# 13bits
discharge_bin = msb << 5 | lsb & 0x1f
# 0 mV -> 000h, 0.5 mA/bit 1FFFh -> 1800 mA
return discharge_bin * 0.5
@property
def internal_temperature(self):
""" Returns temperature in celsius C """
temp_msb = self.bus.read_byte_data(AXP209_ADDRESS, INTERNAL_TEMPERATURE_MSB_REG)
temp_lsb = self.bus.read_byte_data(AXP209_ADDRESS, INTERNAL_TEMPERATURE_LSB_REG)
# MSB is 8 bits, LSB is lower 4 bits
temp = temp_msb << 4 | temp_lsb & 0x0f
# -144.7c -> 000h, 0.1c/bit FFFh -> 264.8c
return temp*0.1-144.7
@property
def battery_gauge(self):
gauge_bin = self.bus.read_byte_data(AXP209_ADDRESS, BATTERY_GAUGE_REG)
gauge = gauge_bin & 0x7f
if gauge > 100:
return -1
return gauge
class InputModule(AbstractInput):
""" A sensor support class that monitors the MH-Z16's CO2 concentration """
def __init__(self, input_dev, testing=False):
super(InputModule, self).__init__(input_dev, testing=testing, name=__name__)
self.ser = None
self.i2c = None
if not testing:
self.initialize_input()
def initialize_input(self):
if self.input_dev.interface == 'UART':
import serial
if is_device(self.input_dev.uart_location):
try:
self.ser = serial.Serial(
port=self.input_dev.uart_location,
timeout=1,
writeTimeout=5)
except serial.SerialException:
self.logger.exception('Opening serial')
else:
self.logger.error(
'Could not open "{dev}". '
'Check the device location is correct.'.format(
dev=self.input_dev.uart_location))
elif self.input_dev.interface == 'I2C':
from smbus2 import SMBus
self.i2c_address = int(str(self.input_dev.i2c_location), 16)
self.cmd_measure = [0xFF, 0x01, 0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63]
self.IOCONTROL = 0X0E << 3
self.FCR = 0X02 << 3
self.LCR = 0X03 << 3
self.DLL = 0x00 << 3
self.DLH = 0X01 << 3
self.THR = 0X00 << 3
self.RHR = 0x00 << 3
self.TXLVL = 0X08 << 3
self.RXLVL = 0X09 << 3
self.i2c = SMBus(self.input_dev.i2c_bus)
self.begin()
def get_measurement(self):
""" Gets the MH-Z16's CO2 concentration in ppmv """
self.return_dict = copy.deepcopy(measurements_dict)
co2 = None
if self.input_dev.interface == 'UART':
if not self.ser:
self.logger.error("Input not set up")
return
self.ser.flushInput()
time.sleep(1)
self.ser.write(bytearray([0xff, 0x01, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79]))
time.sleep(.01)
resp = self.ser.read(9)
if len(resp) != 0:
high = resp[2]
low = resp[3]
co2 = (high * 256) + low
elif self.input_dev.interface == 'I2C':
if not self.i2c:
self.logger.error("Input not set up")
return
self.write_register(self.FCR, 0x07)
self.send(self.cmd_measure)
try:
co2 = self.parse(self.receive())
except Exception:
co2 = None
self.value_set(0, co2)
return self.return_dict
def begin(self):
try:
self.write_register(self.IOCONTROL, 0x08)
except IOError:
pass
self.write_register(self.FCR, 0x07)
self.write_register(self.LCR, 0x83)
self.write_register(self.DLL, 0x60)
self.write_register(self.DLH, 0x00)
self.write_register(self.LCR, 0x03)
@staticmethod
def parse(response):
checksum = 0
if len(response) < 9:
return None
for i in range(0, 9):
checksum += response[i]
if response[0] == 0xFF:
if response[1] == 0x9C:
if checksum % 256 == 0xFF:
return (response[2] << 24) + (response[3] << 16) + (response[4] << 8) + response[5]
return None
def read_register(self, reg_addr):
time.sleep(0.01)
return self.i2c.read_byte_data(self.i2c_address, reg_addr)
def write_register(self, reg_addr, val):
time.sleep(0.01)
self.i2c.write_byte_data(self.i2c_address, reg_addr, val)
def send(self, command):
if self.read_register(self.TXLVL) >= len(command):
self.i2c.write_i2c_block_data(self.i2c_address, self.THR, command)
def receive(self):
n = 9
buf = []
start = time.clock()
while n > 0:
rx_level = self.read_register(self.RXLVL)
if rx_level > n:
rx_level = n
buf.extend(self.i2c.read_i2c_block_data(self.i2c_address, self.RHR, rx_level))
n = n - rx_level
if time.clock() - start > 0.2:
break
return buf
class LIS3MDL:
possible_i2c_addresses = [0x1e]
# IDs
WHO_AM_I = 0x0F
CTRL_REG1 = 0x20
CTRL_REG2 = 0x21
CTRL_REG3 = 0x22
CTRL_REG4 = 0x23
CTRL_REG5 = 0x24
STATUS_REG = 0x27
OUT_X_L = 0x28
OUT_X_H = 0x29
OUT_Y_L = 0x2A
OUT_Y_H = 0x2B
OUT_Z_L = 0x2C
OUT_Z_H = 0x2D
TEMP_OUT_L = 0x2E
TEMP_OUT_H = 0x2F
INT_CFG = 0x30
INT_SRC = 0x31
INT_THS_L = 0x32
INT_THS_H = 0x33
# Values
WHO_ID = 0x3D
HIGH_PERFORMANCE_MODE_XY = 0x70
HIGH_PERFORMANCE_MODE_Z = 0x0C
CONTINUOUS_CONVERSION_MODE = 0x00
FOUR_GAUSS_SCALE_MODE = 0x00
def __init__(self):
self.i2c_bus = SMBus(1)
self.logger = logging.getLogger()
self.address = None
if not self.find_i2c_address():
self.logger.error("Could not connect to LIS3MDL!")
else:
self.set_default_settings()
def find_i2c_address(self):
for address in self.possible_i2c_addresses:
self.logger.debug("Trying address: " + str(address))
try:
if self.i2c_bus.read_byte_data(address,
self.WHO_AM_I) == self.WHO_ID:
self.address = address
self.logger.debug("Found LIS3MDL at " + str(self.address))
return True
except:
self.logger.debug("LIS3MDL not found at " + str(address))
return False
def write_data(self, register, data):
try:
self.i2c_bus.write_byte_data(self.address, register, data)
return True
except:
self.logger.error("Could not write " + str(data) + " to " +
str(register) + " at " + str(self.address) + ".")
return False
def read_data(self, register):
try:
return self.i2c_bus.read_byte_data(self.address, register)
except:
self.logger.error("Could not read from " + str(register) + " at " +
str(self.address) + ".")
return None
def set_default_settings(self):
if self.i2c_bus is not None:
# Set X and Y to high performance
if not self.write_data(self.CTRL_REG1,
self.HIGH_PERFORMANCE_MODE_XY):
self.logger.error(
"Could not set X and Y to high performance mode!")
# Set Z to high performance
if not self.write_data(self.CTRL_REG4,
self.HIGH_PERFORMANCE_MODE_Z):
self.logger.error("Could not set Z to high performance mode!")
if not self.write_data(self.CTRL_REG2, self.FOUR_GAUSS_SCALE_MODE):
self.logger.error("Could not set to +/- 4 gauss scale mode!")
if not self.write_data(self.CTRL_REG3,
self.CONTINUOUS_CONVERSION_MODE):
self.logger.error(
"Could not set to continuous conversion mode!")
def get_magnetometer_data(self):
vector = None
mag_x = Math.two_bytes_to_number(self.read_data(self.OUT_X_H),
self.read_data(self.OUT_X_L))
mag_y = Math.two_bytes_to_number(self.read_data(self.OUT_Y_H),
self.read_data(self.OUT_Y_L))
mag_z = Math.two_bytes_to_number(self.read_data(self.OUT_Z_H),
self.read_data(self.OUT_Z_L))
vector = Vector(mag_x, mag_y, mag_z)
return vector
class SEPVE:
CURRENT_INPUT: int = 2
INPUT_GAIN_dB: int = 28
VOLUME: str = "MUTE"
BASS_dB: int = 0
MID_dB: int = 2
TREBLE_dB: int = 2
SPEAKER: str = "MUTE"
i2c_addr = 0x44
bus = None
def __init__(self, addr):
self.i2c_addr = addr
self.bus = SMBus(1)
def setInputChannelFunction(self, channel: int):
self.bus.write_byte_data(self.i2c_addr, INPUT_SELECTOR.get("SUB_ADDR"),
INPUT_SELECTOR.get(channel))
def setInputGainFunction(self, gain: int):
self.bus.write_byte_data(self.i2c_addr, INPUT_GAIN.get("SUB_ADDR"),
INPUT_GAIN.get(gain))
def setVolumeFunction(self, volume):
if volume == "MUTE":
volume = VOLUME.get("MUTE")
self.bus.write_byte_data(self.i2c_addr, VOLUME.get("SUB_ADDR"), volume)
def setBassGainFunction(self, gain: int):
self.bus.write_byte_data(self.i2c_addr, BASS_GAIN.get("SUB_ADDR"),
BASS_GAIN.get(gain))
def setMidGainFunction(self, gain: int):
self.bus.write_byte_data(self.i2c_addr, TREBLE_GAIN.get("SUB_ADDR"),
TREBLE_GAIN.get(gain))
def setTrebleGainFunction(self, gain: int):
self.bus.write_byte_data(self.i2c_addr, TREBLE_GAIN.get("SUB_ADDR"),
TREBLE_GAIN.get(gain))
def setSpeakerAttRFunction(self, att):
if att == "MUTE":
att = SPEAKER_ATT_R.get("MUTE")
self.bus.write_byte_data(self.i2c_addr, SPEAKER_ATT_R.get("SUB_ADDR"),
att)
def setSpeakerAttLFunction(self, att):
if att == "MUTE":
att = SPEAKER_ATT_L.get("MUTE")
self.bus.write_byte_data(self.i2c_addr, SPEAKER_ATT_L.get("SUB_ADDR"),
att)
#!/usr/bin/env python3
from smbus2 import SMBus
import time
import numpy as np
from RPi import GPIO
bus = SMBus(1)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(16, GPIO.OUT)
GPIO.setup(18, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)
address = 0x68
bus.write_byte_data(address, 0x1c, 0b00000000)
def pump_on():
GPIO.output(16, GPIO.HIGH)
GPIO.output(18, GPIO.LOW)
GPIO.output(22, GPIO.HIGH)
def pump_off():
GPIO.output(16, GPIO.LOW)
GPIO.output(18, GPIO.LOW)
GPIO.output(22, GPIO.LOW)
def accel():
x0 = bus.read_byte_data(address, 0x3B)
x1 = bus.read_byte_data(address, 0x3C)
y0 = bus.read_byte_data(address, 0x3D)
y1 = bus.read_byte_data(address, 0x3E)
class InputModule(AbstractInput):
"""
A sensor support class that measures the Chirp's moisture, temperature
and light
"""
def __init__(self, input_dev, testing=False):
super(InputModule, self).__init__(input_dev, testing=testing, name=__name__)
if not testing:
self.i2c_address = int(str(input_dev.i2c_location), 16)
self.i2c_bus = input_dev.i2c_bus
self.bus = SMBus(self.i2c_bus)
self.filter_average('lux', init_max=3)
def get_measurement(self):
""" Gets the light, moisture, and temperature """
self.return_dict = measurements_dict.copy()
if self.is_enabled(0):
self.value_set(0, self.filter_average('lux', measurement=self.light()))
if self.is_enabled(1):
self.value_set(1, self.moist())
if self.is_enabled(2):
self.value_set(2, self.temp() / 10.0)
return self.return_dict
def get_reg(self, reg):
# read 2 bytes from register
val = self.bus.read_word_data(self.i2c_address, reg)
# return swapped bytes (they come in wrong order)
return (val >> 8) + ((val & 0xFF) << 8)
def reset(self):
# To reset the sensor, write 6 to the device I2C address
self.bus.write_byte(self.i2c_address, 6)
def set_addr(self, new_addr):
# To change the I2C address of the sensor, write a new address
# (one byte [1..127]) to register 1; the new address will take effect after reset
self.bus.write_byte_data(self.i2c_address, 1, new_addr)
self.reset()
# self.address = new_addr
def moist(self):
# To read soil moisture, read 2 bytes from register 0
return self.get_reg(0)
def temp(self):
# To read temperature, read 2 bytes from register 5
return self.get_reg(5)
def light(self):
# To read light level, start measurement by writing 3 to the
# device I2C address, wait for 3 seconds, read 2 bytes from register 4
self.bus.write_byte(self.i2c_address, 3)
time.sleep(1.5)
lux = self.get_reg(4)
if lux == 0:
return 65535.0
else:
return(1 - (lux / 65535.0)) * 65535.0
class Controller:
def __init__(self, address=0x70, channel=1):
self.i2c_channel = channel
self.bus = SMBus(self.i2c_channel)
self.address = address
self.power_on()
for i in range(4):
self.led_enable(i)
def reset(self):
self.bus.write_byte_data(0x03, 0xA5, 0x5A)
def print_registers(self):
print("-- Registers -- ")
self.print_byte_reg(REG_MODE1)
self.print_byte_reg(REG_MODE2)
self.print_byte_reg(REG_PWM0)
self.print_byte_reg(REG_PWM1)
self.print_byte_reg(REG_PWM2)
self.print_byte_reg(REG_PWM3)
self.print_byte_reg(REG_LEDOUT)
self.print_byte_reg(REG_ALLCALL)
def _read(self, reg):
return self.bus.read_byte_data(self.address, reg)
def led_enable(self, i):
config = self._read(REG_LEDOUT)
config |= (1 << 2 * i)
config |= (1 << 2 * i + 1)
self.bus.write_byte_data(self.address, REG_LEDOUT, config)
def led_disable(self, i):
config = self._read(REG_LEDOUT)
mode = LED_MODE_OFF
config &= ~(1 << 2 * i)
config &= ~(1 << 2 * i + 1)
self.bus.write_byte_data(self.address, REG_LEDOUT, config)
def led_enabled(self, i):
config = self._read(REG_LEDOUT)
if (config >> 2 * i) & 1 == 1 and (config >> (2 * i + 1)) & 1 == 1:
return True
return False
def set_leds(self, values):
reg = PCA_AUTOINCREMENT_INDIVIDUAL | REG_PWM0
res = self.bus.write_i2c_block_data(self.address, reg, values)
def power_on(self):
config = self._read(REG_MODE1)
config &= ~(1 << SLEEP)
self.bus.write_byte_data(self.address, REG_MODE1, config)
def power_off(self):
config = self._read(REG_MODE1)
config |= (1 << SLEEP)
self.bus.write_byte_data(self.address, REG_MODE1, config)
def all_on(self):
self.set_leds([255, 255, 255, 255])
def all_off(self):
self.set_leds([0, 0, 0, 0])
def print_byte_reg(self, reg):
print("{:08b}".format(self.bus.read_byte_data(self.address, reg)))
def set_led(self, i=0, val=0):
assert val <= 255 and val >= 0
reg = PCA_AUTOINCREMENT_ALL | REG_PWM0 + i
self.bus.write_byte_data(self.address, reg, val)
def __del__(self):
self.bus.close()
class _Preamps:
""" Low level discovery and communication for the AmpliPi firmware
"""
def __init__(self, mock=False):
self.preamps = dict()
if DISABLE_HW or mock:
self.bus = None
print('Mocking preamp connection')
else:
# Setup serial connection via UART pins - set I2C addresses for preamps
# ser = serial.Serial ("/dev/ttyS0") <--- for RPi4!
ser = Serial("/dev/ttyAMA0")
ser.baudrate = 9600
addr = 0x41, 0x10, 0x0D, 0x0A
ser.write(addr)
ser.close()
# Delay to account for addresses being set
# Possibly unnecessary due to human delay
time.sleep(1)
# Setup self._bus as I2C1 from the RPi
self.bus = SMBus(1)
# Discover connected preamp boards
for p in _DEV_ADDRS:
if self.probe_preamp(p):
print('Preamp found at address {}'.format(p))
self.new_preamp(p)
else:
if p == _DEV_ADDRS[0]:
print('Error: no preamps found')
break
def new_preamp(self, index):
self.preamps[index] = [
0x0F,
0x00,
0x00,
0x3F,
0x00,
0x4F,
0x4F,
0x4F,
0x4F,
0x4F,
0x4F,
]
def write_byte_data(self, preamp_addr, reg, data):
assert preamp_addr in _DEV_ADDRS
assert type(preamp_addr) == int
assert type(reg) == int
assert type(data) == int
# dynamically update preamps (to support mock)
if preamp_addr not in self.preamps:
if self.bus is None:
self.new_preamp(preamp_addr)
else:
return None # Preamp is not connected, do nothing
if DEBUG_PREAMPS:
print("writing to 0x{:02x} @ 0x{:02x} with 0x{:02x}".format(
preamp_addr, reg, data))
self.preamps[preamp_addr][reg] = data
# TODO: need to handle volume modifying mute state in mock
if self.bus is not None:
time.sleep(0.01)
try:
self.bus.write_byte_data(preamp_addr, reg, data)
except Exception:
time.sleep(0.01)
self.bus = SMBus(1)
self.bus.write_byte_data(preamp_addr, reg, data)
def probe_preamp(self, index):
# Scan for preamps, and set source registers to be completely digital
try:
self.bus.write_byte_data(index, _REG_ADDRS['SRC_AD'], 0x0F)
return True
except Exception:
return False
def print_regs(self):
for preamp, regs in self.preamps.items():
print('preamp {}:'.format(preamp / 8))
for reg, val in enumerate(regs):
print(' {} - {:02b}'.format(reg, val))
def print(self):
for preamp_addr in self.preamps.keys():
preamp = int(preamp_addr / 8)
print('preamp {}:'.format(preamp))
src_types = self.preamps[0x08][_REG_ADDRS['SRC_AD']]
src_cfg = []
for src in range(4):
src_type = _SRC_TYPES.get((src_types >> src) & 0b01)
src_cfg += ['{}'.format(src_type)]
print(' [{}]'.format(', '.join(src_cfg)))
for zone in range(6):
self.print_zone_state(6 * (preamp - 1) + zone)
def print_zone_state(self, zone):
assert zone >= 0
preamp = (int(zone / 6) + 1) * 8
z = zone % 6
regs = self.preamps[preamp]
src_types = self.preamps[0x08][_REG_ADDRS['SRC_AD']]
src = ((regs[_REG_ADDRS['CH456_SRC']] << 8)
| regs[_REG_ADDRS['CH123_SRC']] >> 2 * z) & 0b11
src_type = _SRC_TYPES.get((src_types >> src) & 0b01)
vol = -regs[_REG_ADDRS['CH1_ATTEN'] + z]
muted = (regs[_REG_ADDRS['MUTE']] & (1 << z)) > 0
state = []
if muted:
state += ['muted']
print(' {}({}) --> zone {} vol [{}] {}'.format(
src, src_type[0], zone, utils.vol_string(vol), ', '.join(state)))
class InputModule(AbstractInput):
""" A sensor support class that monitors the MH-Z16's CO2 concentration """
def __init__(self, input_dev, testing=False):
super(InputModule, self).__init__()
self.logger = logging.getLogger("mycodo.inputs.mh_z16")
if not testing:
self.logger = logging.getLogger(
"mycodo.mh_z16_{id}".format(id=input_dev.unique_id.split('-')[0]))
self.interface = input_dev.interface
self.uart_location = input_dev.uart_location
if self.interface == 'UART':
import serial
# Check if device is valid
self.serial_device = is_device(self.uart_location)
if self.serial_device:
try:
self.ser = serial.Serial(self.serial_device, timeout=1)
except serial.SerialException:
self.logger.exception('Opening serial')
else:
self.logger.error(
'Could not open "{dev}". '
'Check the device location is correct.'.format(
dev=self.uart_location))
elif self.interface == 'I2C':
from smbus2 import SMBus
self.i2c_address = int(str(input_dev.i2c_location), 16)
self.i2c_bus = input_dev.i2c_bus
self.cmd_measure = [0xFF, 0x01, 0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63]
self.IOCONTROL = 0X0E << 3
self.FCR = 0X02 << 3
self.LCR = 0X03 << 3
self.DLL = 0x00 << 3
self.DLH = 0X01 << 3
self.THR = 0X00 << 3
self.RHR = 0x00 << 3
self.TXLVL = 0X08 << 3
self.RXLVL = 0X09 << 3
self.i2c = SMBus(self.i2c_bus)
self.begin()
def get_measurement(self):
""" Gets the MH-Z16's CO2 concentration in ppmv via UART"""
return_dict = measurements_dict.copy()
co2 = None
if self.interface == 'UART':
if not self.serial_device: # Don't measure if device isn't validated
return None
self.ser.flushInput()
time.sleep(1)
self.ser.write(bytearray([0xff, 0x01, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79]))
time.sleep(.01)
resp = self.ser.read(9)
if len(resp) != 0:
high = resp[2]
low = resp[3]
co2 = (high * 256) + low
elif self.interface == 'I2C':
self.write_register(self.FCR, 0x07)
self.send(self.cmd_measure)
try:
co2 = self.parse(self.receive())
except Exception:
co2 = None
return_dict[0]['value'] = co2
return return_dict
def begin(self):
try:
self.write_register(self.IOCONTROL, 0x08)
except IOError:
pass
self.write_register(self.FCR, 0x07)
self.write_register(self.LCR, 0x83)
self.write_register(self.DLL, 0x60)
self.write_register(self.DLH, 0x00)
self.write_register(self.LCR, 0x03)
@staticmethod
def parse(response):
checksum = 0
if len(response) < 9:
return None
for i in range(0, 9):
checksum += response[i]
if response[0] == 0xFF:
if response[1] == 0x9C:
if checksum % 256 == 0xFF:
return (response[2] << 24) + (response[3] << 16) + (response[4] << 8) + response[5]
return None
def read_register(self, reg_addr):
time.sleep(0.01)
return self.i2c.read_byte_data(self.i2c_address, reg_addr)
def write_register(self, reg_addr, val):
time.sleep(0.01)
self.i2c.write_byte_data(self.i2c_address, reg_addr, val)
def send(self, command):
if self.read_register(self.TXLVL) >= len(command):
self.i2c.write_i2c_block_data(self.i2c_address, self.THR, command)
def receive(self):
n = 9
buf = []
start = time.clock()
while n > 0:
rx_level = self.read_register(self.RXLVL)
if rx_level > n:
rx_level = n
buf.extend(self.i2c.read_i2c_block_data(self.i2c_address, self.RHR, rx_level))
n = n - rx_level
if time.clock() - start > 0.2:
break
return buf
