LCD_LINE_1 = 0x80 # LCD RAM address for the 1st line
LCD_LINE_2 = 0xC0 # LCD RAM address for the 2nd line
LCD_LINE_3 = 0x94 # LCD RAM address for the 3rd line
LCD_LINE_4 = 0xD4 # LCD RAM address for the 4th line
LCD_BACKLIGHT = 0x08 # On
ENABLE = 0b00000100 # Enable bit
# Timing constants
E_PULSE = 0.0005
E_DELAY = 0.0005
# Open I2C interface
# bus = smbus.SMBus(0) # Rev 1 Pi uses 0
bus = smbus.SMBus(1) # Rev 2 Pi uses 1
def lcd_init():
# Initialise display
lcd_byte(0x33, LCD_CMD) # 110011 Initialise
lcd_byte(0x32, LCD_CMD) # 110010 Initialise
lcd_byte(0x06, LCD_CMD) # 000110 Cursor move direction
lcd_byte(0x0C, LCD_CMD) # 001100 Display On,Cursor Off, Blink Off
lcd_byte(0x28, LCD_CMD) # 101000 Data length, number of lines, font size
lcd_byte(0x01, LCD_CMD) # 000001 Clear display
time.sleep(E_DELAY)
def lcd_byte(bits, mode):
# Send byte to data pins
# i2c FND를 이용한 디지털 시계
import smbus2 as smbus
import time
addr = 0x20
config_port = 0x06
output_port = 0x02
bus = smbus.SMBus(1)
bus.write_word_data(addr, config_port, 0x0000) # 16 bits
# digits = 0, 1, 2, 3, 4, 5, 6, 7, 8, 9
digits = (0xFC, 0x60, 0xDA, 0xF2, 0x66, 0xB6, 0x3E, 0xE0, 0xFE, 0xF6)
# segment number = 1, 2, 3, 4, 5, 6
seg = (0x7E, 0xBF, 0xDF, 0xEF, 0xF7, 0xFB)
def main():
while True:
# 현재 시간 string으로 가져오기
cur_time = time.ctime()
# time slicing
hour, minute, second = cur_time[11:13], cur_time[14:16], cur_time[
17:19]
strtime = hour + minute + second
# time display
for i in range(6):
num = int(strtime[i])
display_fnd(num, i)
time.sleep(0.003)
np.save('./lookup/main', calibration_function(train_data))
np.save('./train_data/main', train_data)
print('Look-up table created!')
CURRENT_SIZE = None
ZEROED = False
BIAS = 0
train_data = None
data_collected = 0
print('Calibration done!')
return 'Calibration done!'
@app.route('/calibration/show')
def show_calibration():
return {'calibration': json.dumps(train_data.tolist())}
if __name__ == '__main__':
app.config.from_object('config.default')
DEV_ADDRESS = app.config['DEV_ADDRESS']
try:
DEV_CTX = smbus2.SMBus(DEV_BUS)
except IOError as e:
print(e.message)
sys.exit(1)
app.run(host='0.0.0.0', port=7006)
def __init__(self):
self.bus = smbus.SMBus(1)
import bme280
import smbus2
from time import sleep
port = 1
address = 0x76 # Adafruit BME280 address. Other BME280s may be different
bus = smbus2.SMBus(port)
bme280.load_calibration_params(bus, address)
bme280_data = bme280.sample(bus, address)
humidity = bme280_data.humidity
pressure = bme280_data.pressure
ambient_temperature = bme280_data.temperature
print(humidity, pressure, ambient_temperature)
sleep(1)
f = open(
"/home/pi/Documents/Python-Beginner-Pi-Projects/BME280/meteoLocal.txt",
"w")
f.write(
str(int(ambient_temperature)) + "\n" + str(int(pressure)) + "\n" +
str(int(humidity)) + "\n")
f.close()
# Program to read left and right encoders on SCUTTLE.
# Left has address 40 and right has 41, and the encoders are placed on
# the motor shafts, so readings indicate movement before pulley ratio
# is considered.
# This code runs on SCUTTLE with rasPi setup. (last updated 2020.11)
# Import external libraries
import smbus2 # a Python package to communicate over i2c
import numpy as np # use numpy to build the angles array
import time # for keeping time
bus = smbus2.SMBus(1) # declare the i2c bus object
encL = 0x40 # encoder i2c address for LEFT motor
encR = 0x41 # encoder i2c address for RIGHT motor (this encoder has A1 pin pulled high)
def singleReading(
encoderSelection
): # return a reading for an encoder in degrees (motor shaft angle)
try:
twoByteReading = bus.read_i2c_block_data(
encoderSelection, 0xFE, 2
) # request data from registers 0xFE & 0xFF of the encoder. Approx 700 microseconds.
binaryPosition = (twoByteReading[0] << 6) | twoByteReading[
1] # remove unused bits 6 & 7 from byte 0xFF creating 14 bit value
degreesPosition = binaryPosition * (360 / 2**14) # convert to degrees
degreesAngle = round(degreesPosition,
1) # round to nearest 0.1 degrees
except:
print("Encoder reading failed.") # indicate a failed reading
'u': REG_OLAT_A,
'i': REG_OLAT_A,
'o': REG_OLAT_A,
'p': REG_OLAT_A,
'-': REG_OLAT_A,
'l': REG_OLAT_A,
'k': REG_OLAT_A,
'j': REG_OLAT_A,
'h': REG_OLAT_B,
'b': REG_OLAT_B,
'n': REG_OLAT_B,
'm': REG_OLAT_B,
',': REG_OLAT_B
}
bus = smbus.SMBus(CHANNEL)
word = sys.argv[1]
# ピンの入出力設定
bus.write_byte_data(ICADDR_1, REG_IODIR_A, 0x00)
bus.write_byte_data(ICADDR_1, REG_IODIR_B, 0x00)
bus.write_byte_data(ICADDR_2, REG_IODIR_A, 0x00)
bus.write_byte_data(ICADDR_2, REG_IODIR_B, 0x00)
for i in word:
print('Hitting the key: %(word)s. ' % {'word': i})
bus.write_byte_data(icaddr[i], register[i], alphabet[i])
time.sleep(1)
bus.write_byte_data(icaddr[i], register[i], 0x00)
# time.sleep(0.1)
bus_oil.write_i2c_block_data(0x68,0b10001000,[0x00])
raw=data_oil[0]<<8 | data_oil[1]
if raw >32767:
raw-=65535
vol=2.048/32767
v=raw*vol
Rf=(v/(4.13-v))*10000.0
lnRf=Rf/10000
Temp=3380/(math.log(lnRf)+3380/298.16)-273.16
return ('{:<.1f}'.format(Temp))
bus_oil=smbus2.SMBus(1)
bus_oil.write_i2c_block_data(0x68,0b10001000,[0x00])
def Oil_temp_loop():
x = Oil_temp()
var_oil.set(x)
root.after(1000, Oil_temp_loop)
#水温の取得
def water_temp():
var_water.set('{:<.1f}'.format(data['ENGTHW']))
root.after(500, water_temp)
call_back_function=CallBackFunction()
def __enter__(self):
self.__smbus = smbus2.SMBus(self.__bus)
return self
def setup(port, file, bme, pms):
bus = smbus2.SMBus(port)
calibration = bme280.load_calibration_params(bus, bme)
pms_sensor = Pms7003Sensor(pms)
logging.basicConfig(filename=file, filemode='a', format='%(message)s')
return bus, calibration, pms_sensor
class Magum:
""" Magum(gScaleRange,fsDouble,aScaleRange,noise) -> Magum
Return a new Magum object that is (optionally)
automatically initialized with the default values.
"""
_i2cBus = smbus2.SMBus(2) # open communication to I2C channel 2
_calibrated = False # check calibration
accScale = None
gyrScale = None
gyrDouble = None
# Complementary Filter Attributes
compAux = 0
_cFAngleX = 0
_cFAngleY = 0
_cFAngleZ = 0
compAux = 0
def __init__(self, gScaleRange=None, fsDouble=None, aScaleRange=None, noise=None):
self.killDrivers(1)
self._initAm(aScaleRange, noise)
self._initG(gScaleRange, fsDouble)
# accelerometer and magnetometer initialization
def _initAm(self, scaleRange=None, noise=None):
self.toStandby('a')
if noise == 1 and scaleRange in (2, 4):
regNoise = 0x04 # 0b100
elif noise in (0, None):
regNoise = 0x00
else:
print 'Error: incorrect low noise value, it can assume 1 (enabled) or 0 (diabled)'
sys.exit(1)
if scaleRange == 2:
self.setSensConf('a', 'A_XYZ_DATA_CFG', 0x00) # set range to +/- 2g
elif scaleRange == 4:
self.setSensConf('a', 'A_XYZ_DATA_CFG', 0x01) # set range to +/- 4g
elif scaleRange == 8:
self.setSensConf('a', 'A_XYZ_DATA_CFG', 0x02) # set range to +/- 8g
elif scaleRange == None:
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1, 0x21 | regNoise) # set to active mode,
# keep scaleRange at previous value, frequency = 25 Hz (same as accelerometer) in hybrid mode.
time.sleep(.300) # sleep 300 ms
else:
print 'Error: incorrect aScalRange value, read the documentation for the correct config.'
sys.exit(1)
self.accScale = scaleRange
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1,
0x00) # set to active mode, frequency = 25 Hz (same as
# accelerometer) in hybrid mode.
time.sleep(.300) # sleep 300 ms
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, M_CTRL_REG1,
0x00) # enable both accelerometer and magnetometer sensors (hybrid mode)
# gyroscope initialization
def _initG(self, scaleRange=None, fsDouble=None):
self.toStandby('g')
if fsDouble == 1:
self.gyrDouble = 2
self.setSensConf('g', 'G_CTRL_REG3', 0x01)
elif fsDouble == 0:
self.gyrDouble = 1
self.setSensConf('g', 'G_CTRL_REG3', 0x00)
else:
self.gyrDouble = 1
self.setSensConf('g', 'G_CTRL_REG3', 0x00)
if scaleRange == 2000:
self.setSensConf('g', 'G_CTRL_REG0',
0x00) # set range to +/- 2000dps (4000dps if CTRL_REG3 is set to double)
elif scaleRange == 1000:
self.setSensConf('g', 'G_CTRL_REG0',
0x01) # set range to +/- 1000dps (2000dps if CTRL_REG3 is set to double)
elif scaleRange == 500:
self.setSensConf('g', 'G_CTRL_REG0',
0x02) # set range to +/- 500dps (1000dps if CTRL_REG3 is set to double)
elif scaleRange == 250:
self.setSensConf('g', 'G_CTRL_REG0', 0x03) # set range to +/- 250dps (500dps if CTRL_REG3 is set to double)
elif scaleRange == None:
self._i2cBus.write_byte_data(I2C_G_ADDRESS, G_CTRL_REG1, 0x16) # set to active mode 25Hz
time.sleep(.300) # sleep 300 ms
else:
print 'Error: incorrect gScalRange value, read the documentation for the correct config.'
sys.exit(1)
self.gyrScale = scaleRange
self._i2cBus.write_byte_data(I2C_G_ADDRESS, G_CTRL_REG1, 0x00) # set to active mode 25Hz
time.sleep(.300) # sleep 300ms
def toStandby(self, sensor):
if sensor in ('a', 'm'):
currReg = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1) # get current configuration
if currReg % 2 == 1:
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1, currReg - 1) # set to standby_mode
if sensor in ('g'):
currReg = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_CTRL_REG1) # get old configuration
currReg = currReg >> 2
currReg = currReg << 2
self._i2cBus.write_byte_data(I2C_G_ADDRESS, G_CTRL_REG1, currReg) # set to standby_mode
time.sleep(.300) # sleep 300ms
def toActive(self, sensor):
if sensor in ('a', 'm'):
currReg = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1) # get current configuration
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1, currReg + 1) # set to active_mode
if sensor in ('g'):
currReg = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_CTRL_REG1) # get old configuration
currReg = currReg >> 2
currReg = currReg << 2
currReg = currReg + 2
self._i2cBus.write_byte_data(I2C_G_ADDRESS, G_CTRL_REG1, currReg) # set to active_mode
time.sleep(.300) # sleep 300ms
# enable/disable system drivers
def killDrivers(self, x):
proc1 = subprocess.Popen(shlex.split('lsmod'), stdout=subprocess.PIPE)
proc2 = subprocess.Popen(shlex.split('grep fxas2100x'), stdin=proc1.stdout, stdout=subprocess.PIPE,
stderr=subprocess.PIPE)
proc1.stdout.close() # Allow proc1 to receive a SIGPIPE if proc2 exits.
out1, err1 = proc2.communicate()
proc1 = subprocess.Popen(shlex.split('lsmod'), stdout=subprocess.PIPE)
proc2 = subprocess.Popen(shlex.split('grep fxos8700'), stdin=proc1.stdout, stdout=subprocess.PIPE,
stderr=subprocess.PIPE)
proc1.stdout.close() # Allow proc1 to receive a SIGPIPE if proc2 exits.
out2, err2 = proc2.communicate()
if x == 1:
if out1:
os.system('rmmod fxas2100x')
if out2:
os.system('rmmod fxos8700')
elif x == 0:
if not out1:
os.system('modprobe fxas2100x')
if not out2:
os.system('modprobe fxos8700')
else:
print "Error: wrong killDrivers(x) parameter.\n self.killDrivers(0): enable drivers \n killDrivers(1): " \
"disable drivers."
sys.exit(1)
# sensor calibration
def calibrateSens(self, samples):
acc_angle = array('i', [])
rate_gyr = array('i', [])
i = 0
sumX = 0
sumY = 0
sumZ = 0
gsumX = 0
gsumY = 0
gsumZ = 0
tarXvect = array('i', [])
tarYvect = array('i', [])
tarZvect = array('i', [])
gtarXvect = array('i', [])
gtarYvect = array('i', [])
gtarZvect = array('i', [])
gyrXangle = 0.0
gyrYangle = 0.0
gyrZangle = 0.0
accXangle = 0.0
accYangle = 0.0
accZangle = 0.0
axisOffset = array('i', [])
# sensors Calibration
raw_input("CAUTION! Sensors calibration.\nSet your udoo-neo in an horizontal position and press Enter Key...\n")
perc = -1
while i < samples:
acc_angle = self.readAData()
rate_gyr = self.readGData()
factor = self.accScale / 2
if acc_angle[0] >= 32768:
tarXvect.insert(i, int(acc_angle[0] - 65536))
else:
tarXvect.insert(i, int(acc_angle[0]))
if acc_angle[1] >= 32768:
tarYvect.insert(i, int(acc_angle[1] - 65536))
else:
tarYvect.insert(i, int(acc_angle[1]))
if acc_angle[2] >= 32768:
tarZvect.insert(i, int(acc_angle[2] - 65536 + 16384 / factor))
else:
tarZvect.insert(i, int(acc_angle[2] + 16384 / factor))
if rate_gyr[0] >= 32768:
gtarXvect.insert(i, int(rate_gyr[0] - 65536))
else:
gtarXvect.insert(i, int(rate_gyr[0]))
if rate_gyr[1] >= 32768:
gtarYvect.insert(i, int(rate_gyr[1] - 65536))
else:
gtarYvect.insert(i, int(rate_gyr[1]))
if rate_gyr[2] >= 32768:
gtarZvect.insert(i, int(rate_gyr[2] - 65536))
else:
gtarZvect.insert(i, int(rate_gyr[2]))
sumX += tarXvect[i]
sumY += tarYvect[i]
sumZ += tarZvect[i]
gsumX += gtarXvect[i]
gsumY += gtarYvect[i]
gsumZ += gtarZvect[i]
loading = int((i * 100) / samples)
if loading != perc:
print "Calibration percentage: " + str(int(loading)) + "%"
perc = loading
i += 1
print "Calibration percentage: 100%\n"
avgX = sumX / samples
avgY = sumY / samples
avgZ = sumZ / samples
gavgX = gsumX / samples
gavgY = gsumY / samples
gavgZ = gsumZ / samples
axisOffset.insert(0, avgX)
axisOffset.insert(1, avgY)
axisOffset.insert(2, avgZ)
axisOffset.insert(3, gavgX)
axisOffset.insert(4, gavgY)
axisOffset.insert(5, gavgZ)
self._calibrated = True
return axisOffset
# set sensors configurations
def setSensConf(self, sensor, reg, hexVal):
self.toStandby(sensor)
if sensor == 'a':
if reg in A_CREGS_LIST:
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, COMPLETE_REGS_DICT[reg], hexVal)
else:
_regsExample('a')
if sensor == 'm':
if reg in M_CREG_LIST:
if bool(is_hex(str(hexVal))):
self._i2cBus.write_byte_data(I2C_AM_ADDRESS, COMPLETE_REGS_DICT[reg], hexVal)
else:
_regsExample('m')
if sensor == 'g':
if reg in G_CREG_LIST:
self._i2cBus.write_byte_data(I2C_G_ADDRESS, COMPLETE_REGS_DICT[reg], hexVal)
else:
_regsExample('g')
time.sleep(.300) # sleep 300ms
self.toActive(sensor)
# read accelerometer data
def readAData(self, axisOffset=False, uM=None):
# getting x,y,z coordinate shifting first 8bit and adding
# (with the or operator) the others 8 bit to the address
xMsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OUT_X_MSB)
xLsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OUT_X_LSB)
xRaw = (xMsbRaw << 8 | xLsbRaw) # x axis
yMsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OUT_Y_MSB)
yLsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OUT_Y_LSB)
yRaw = (yMsbRaw << 8 | yLsbRaw) # y axis
zMsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OUT_Z_MSB)
zLsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OUT_Z_LSB)
zRaw = (zMsbRaw << 8 | zLsbRaw) # z axis
axisList = np.array([xRaw, yRaw, zRaw], np.float)
if axisOffset:
axisOffset = np.array(axisOffset[0:3], np.float)
axisList = axisList - axisOffset
axisList = _dataConvertion(self._i2cBus, "a", axisList, uM)
return axisList
# read magnetometer data
def readMData(self, uM=None):
axisList = array('f', [])
# getting x,y,z coordinate shifting first 8bit and adding
# (with the or operator) the others 8 bit to the address
xMsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OUT_X_MSB)
xLsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OUT_X_LSB)
xRaw = xMsbRaw << 8 | xLsbRaw # x axis
yMsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OUT_Y_MSB)
yLsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OUT_Y_LSB)
yRaw = yMsbRaw << 8 | yLsbRaw # y axis
zMsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OUT_Z_MSB)
zLsbRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OUT_Z_LSB)
zRaw = zMsbRaw << 8 | zLsbRaw # z axis
axisList.insert(0, xRaw)
axisList.insert(1, yRaw)
axisList.insert(2, zRaw)
axisList = _dataConvertion(self._i2cBus, 'm', axisList, uM)
return axisList
# read gyroscope data
def readGData(self, axisOffset=False, uM=None):
# getting x,y,z coordinate shifting first 8bit and adding
# (with the or operator) the others 8 bit to the address
xMsbRaw = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_OUT_X_MSB)
xLsbRaw = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_OUT_X_LSB)
xRaw = xMsbRaw << 8 | xLsbRaw # x axis
yMsbRaw = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_OUT_Y_MSB)
yLsbRaw = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_OUT_Y_LSB)
yRaw = yMsbRaw << 8 | yLsbRaw # y axis
zMsbRaw = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_OUT_Z_MSB)
zLsbRaw = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_OUT_Z_LSB)
zRaw = zMsbRaw << 8 | zLsbRaw # z axis
axisList = np.array([xRaw, yRaw, zRaw], np.float)
if axisOffset:
axisOffset = np.array(axisOffset[3:6], np.float)
axisList = axisList - axisOffset
axisList = _dataConvertion(self._i2cBus, "g", axisList, uM)
return axisList
def readTData(self, uM=None):
tempRaw = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_TEMP)
if tempRaw >= 128:
tempCels = float((tempRaw - 256) * 0.96)
else:
tempCels = float((tempRaw) * 0.96)
if uM in (None, 'raw'):
return tempRaw
if uM == 'C':
return tempCels
if uM == 'K':
tempKelv = float(tempCels + 273.15)
return tempKelv
if uM == 'F':
tempFahr = float(float(tempCels * 1.8) + 32)
return tempFahr
# complementary filter algorithm
def compFilter(self, DT, axisOffset, uM=None):
exTime = 0.013 # execution time
if DT < exTime:
print "Error: DT is too small to sample the accelerometer and gyroscope data.\nDT must be greater than %f." %exTime
sys.exit(1)
else:
if self._calibrated == True:
highPass = DT / (DT + exTime)
rate_gyr = array('i', [])
acc_angle = array('i', [])
cFAngleAxis = array('f', [])
rate_gyr = self.readGData()
acc_angle = self.readAData()
factor = self.accScale / 2
gFactor = float((self.gyrScale / (1000 * 32)) * self.gyrDouble)
if acc_angle[0] >= 32768:
acc_angle[0] -= 65536
if acc_angle[1] >= 32768:
acc_angle[1] -= 65536
if acc_angle[2] >= 32768:
acc_angle[2] -= 65536
if rate_gyr[0] >= 32768:
rate_gyr[0] -= 65536
if rate_gyr[1] >= 32768:
rate_gyr[1] -= 65536
if rate_gyr[2] >= 32768:
rate_gyr[2] -= 65536
x = (((acc_angle[0] - axisOffset[0]) / 4) * 0.244 * factor)
y = (((acc_angle[1] - axisOffset[1]) / 4) * 0.244 * factor)
z = (((acc_angle[2] - axisOffset[2]) / 4) * 0.244 * factor)
x2 = x * x
y2 = y * y
z2 = z * z
accXangle = math.atan(x / math.sqrt(y2 + z2)) * (180 / math.pi)
accYangle = math.atan(y / math.sqrt(x2 + z2)) * (180 / math.pi)
accZangle = math.atan(z / math.sqrt(x2 + y2)) * (180 / math.pi)
# gyrXangle = float(((rate_gyr[0] - axisOffset[3]) * gFactor) / DT)
gyrYangle = float(((rate_gyr[1] - axisOffset[4]) * gFactor) / DT)
# gyrZangle = float(((rate_gyr[2] - axisOffset[5]) * gFactor) / DT)
# modGyr = (gyrXangle * gyrXangle) + (gyrYangle * gyrYangle) + (gyrZangle * gyrZangle)
# Only for the first time we get the position or if the base doesn't move
# if self.compAux == 0 || (math.fabs(gyrXangle) <= 5 && math.fabs(gyrYangle) <= 5 && math.fabs(
# gyrZangle) <= 5):
if self.compAux == 0:
# self._cFAngleX = float(accXangle)
self._cFAngleY = float(accYangle)
# self._cFAngleZ = float(accZangle)
self.compAux = 1
else: # Then we use the Complementary Filter
# self._cFAngleX = (highPass) * (self._cFAngleX + gyrXangle * DT) + (1 - highPass) * (accXangle)
self._cFAngleY = (highPass) * (self._cFAngleY + gyrYangle * DT) + (1 - highPass) * (accYangle)
# self._cFAngleZ = (highPass) * (self._cFAngleZ + gyrZangle * DT) + (1 - highPass) * (accZangle)
if uM == ('rad'):
# cFAngleAxis.insert(0, self._cFAngleX * (math.pi / 180))
cFAngleAxis.insert(1, self._cFAngleY * (-1) * (math.pi / 180))
# cFAngleAxis.insert(2, self._cFAngleZ * (-1) * (math.pi / 180))
else: #degrees
# cFAngleAxis.insert(0, self._cFAngleX)
cFAngleAxis.insert(1, self._cFAngleY * (-1))
# cFAngleAxis.insert(2, self._cFAngleZ * (-1))
# gyrXangle = float((rate_gyr[0] - axisOffset[3]) * gFactor)
# gyrYangle = float((rate_gyr[1] - axisOffset[4]) * gFactor)
# gyrZangle = float((rate_gyr[2] - axisOffset[5]) * gFactor)
# time.sleep(DT - exTime)
return cFAngleAxis
else:
print "Error: failed calibration.\nMake sure to calibrate the sensors using calibrateSens(sensor," \
"samples)"
sys.exit(1)
# Kalman Filter
# Note: this algorithm is under development, it may not work properly like a common Kalman Filter
# If you want to improve this algorithm join us on github at https://github.com/ubalance-team/magum
def kalmanFilter(self, DT, axis, axisOffset):
exTime = 0.012 # execution time
if DT < exTime:
print "Error: DT is too small to sample the accelerometer and gyroscope data.\nDT must be greater than " \
"0.015."
sys.exit(1)
else:
if self._calibrated == True:
rate_gyr = self.readGData()
acc_angle = self.readAData()
factor = self.accScale / 2
gFactor = float((self.gyrScale / (1000 * 32)) * self.gyrDouble)
if acc_angle[0] >= 32768:
acc_angle[0] -= 65536
if acc_angle[1] >= 32768:
acc_angle[1] -= 65536
if acc_angle[2] >= 32768:
acc_angle[2] -= 65536
if rate_gyr[0] >= 32768:
rate_gyr[0] -= 65536
if rate_gyr[1] >= 32768:
rate_gyr[1] -= 65536
if rate_gyr[2] >= 32768:
rate_gyr[2] -= 65536
x = (((acc_angle[0] - axisOffset[0]) / 4) * 0.244 * factor)
y = (((acc_angle[1] - axisOffset[1]) / 4) * 0.244 * factor)
z = (((acc_angle[2] - axisOffset[2]) / 4) * 0.244 * factor)
x2 = x * x
y2 = y * y
z2 = z * z
if axis == 'x':
accAngle = math.atan(x / math.sqrt(y2 + z2)) * (180 / math.pi)
gyroRate = float((rate_gyr[0] - axisOffset[3]) * gFactor)
elif axis == 'y':
accAngle = math.atan(y / math.sqrt(x2 + z2)) * (180 / math.pi) * (-1)
gyroRate = float((rate_gyr[1] - axisOffset[4]) * gFactor)
elif axis == 'z':
accAngle = math.atan(z / math.sqrt(x2 + y2)) * (180 / math.pi) * (-1)
gyroRate = float((rate_gyr[2] - axisOffset[5]) * gFactor)
Q_angle = 0.01
Q_gyro = 0.0003
R_angle = 0.01
a_bias = 0
AP_00 = 0
AP_01 = 0
AP_10 = 0
AP_11 = 0
KFangle = 0.0
KFangle += DT * (gyroRate - a_bias)
AP_00 += - DT * (AP_10 + AP_01) + Q_angle * DT
AP_01 += - DT * AP_11
AP_10 += - DT * AP_11
AP_11 += + Q_gyro * DT
a = accAngle - KFangle
S = AP_00 + R_angle
K_0 = AP_00 / S
K_1 = AP_10 / S
KFangle += K_0 * a
a_bias += K_1 * a
AP_00 -= K_0 * AP_00
AP_01 -= K_0 * AP_01
AP_10 -= K_1 * AP_00
AP_11 -= K_1 * AP_01
time.sleep(DT - exTime)
return KFangle * float(180 / math.pi) * 0.9
else:
print "Error: failed calibration.\nMake sure to calibrate the sensors using calibrateSens(sensor," \
"samples)"
sys.exit(1)
# Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
# (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
# which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
# device orientation
def madgwickQuaternionFilter(self, aCompArray, gCompArray, mCompArray):
ax = aCompArray[0]
ay = aCompArray[1]
az = aCompArray[2]
mx = mCompArray[0]
my = mCompArray[1]
mz = mCompArray[2]
gx = gCompArray[0]
gy = gCompArray[1]
gz = gCompArray[2]
deltat = 0.001
gyroMeasError = math.pi * (5.0 / 180.0)
gyroMeasDrift = math.pi * (0.2 / 180.0)
beta = math.sqrt(3.0 / 4.0) * gyroMeasError
zeta = math.sqrt(3.0 / 4.0) * gyroMeasDrift
q = array('f', [])
q1 = 1.0
q2 = 0.0
q3 = 0.0
q4 = 0.0
norm = 0.0
hx = 0.0
hy = 0.0
_2bx = 0.0
_2bz = 0.0
s1 = 0.0
s2 = 0.0
s3 = 0.0
s4 = 0.0
qDot1 = 0.0
qDot2 = 0.0
qDot3 = 0.0
qDot4 = 0.0
# Auxiliary variables to avoid repeated arithmetic
_2q1mx = 0.0
_2q1my = 0.0
_2q1mz = 0.0
_2q2mx = 0.0
_4bx = 0.0
_4bz = 0.0
_2q1 = 2.0 * q1
_2q2 = 2.0 * q2
_2q3 = 2.0 * q3
_2q4 = 2.0 * q4
_2q1q3 = 2.0 * q1 * q3
_2q3q4 = 2.0 * q3 * q4
q1q1 = q1 * q1
q1q2 = q1 * q2
q1q3 = q1 * q3
q1q4 = q1 * q4
q2q2 = q2 * q2
q2q3 = q2 * q3
q2q4 = q2 * q4
q3q3 = q3 * q3
q3q4 = q3 * q4
q4q4 = q4 * q4
# Normalize accelerometer measurement
norm = math.sqrt(ax * ax + ay * ay + az * az)
if norm == 0.0: return # handle NaN
norm = 1.0 / norm
ax *= norm
ay *= norm
az *= norm
# Normalize magnetometer measurement
norm = math.sqrt(mx * mx + my * my + mz * mz)
if norm == 0.0: return # handle NaN
norm = 1.0 / norm
mx *= norm
my *= norm
mz *= norm
# Reference direction of Earth s magnetic field
_2q1mx = 2.0 * q1 * mx
_2q1my = 2.0 * q1 * my
_2q1mz = 2.0 * q1 * mz
_2q2mx = 2.0 * q2 * mx
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
_2bx = math.sqrt(hx * hx + hy * hy)
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
_4bx = 2.0 * _2bx
_4bz = 2.0 * _2bz
# Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0 * q2q4 - _2q1q3 - ax) + _2q2 * (2.0 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (
_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (
_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (
_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
s2 = _2q4 * (2.0 * q2q4 - _2q1q3 - ax) + _2q1 * (2.0 * q1q2 + _2q3q4 - ay) - 4.0 * q2 * (
1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + _2bz * q4 * (
_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (
_2bx * q3 + _2bz * q1) * (
_2bx * (
q2q3 - q1q4) + _2bz * (
q1q2 + q3q4) - my) + (
_2bx * q4 -
_4bz *
q2) * (
_2bx * (
q1q3 +
q2q4) +
_2bz * (
0.5 - q2q2 -
q3q3) - mz)
s3 = -_2q1 * (2.0 * q2q4 - _2q1q3 - ax) + _2q4 * (2.0 * q1q2 + _2q3q4 - ay) - 4.0 * q3 * (
1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (
_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (
_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (
_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
s4 = _2q2 * (2.0 * q2q4 - _2q1q3 - ax) + _2q3 * (2.0 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (
_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (
_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (
_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
norm = math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) # normalize step magnitude
norm = 1.0 / norm
s1 *= norm
s2 *= norm
s3 *= norm
s4 *= norm
# Compute rate of change of quaternion
qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - beta * s2
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - beta * s3
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - beta * s4
# Integrate to yield quaternion
q1 += qDot1 * deltat
q2 += qDot2 * deltat
q3 += qDot3 * deltat
q4 += qDot4 * deltat
norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) # normalize quaternion
norm = 1.0 / norm
q.insert(0, q1 * norm)
q.insert(1, q2 * norm)
q.insert(2, q3 * norm)
q.insert(3, q4 * norm)
return q
# get current sensors configurtaions
def getCurrentConf(self, sensor, screen=None):
if sensor == 'a':
config = [None] * 28
_regName = ['A_TRIG_CFG', 'A_CTRL_REG1', 'A_CTRL_REG2', 'A_CTRL_REG3', 'A_CTRL_REG4', 'A_CTRL_REG5',
'A_ASPL_COUNT', 'A_F_SETUP', 'A_XYZ_DATA_CFG', 'A_HP_FILTER_CUTOFF', 'A_PL_CFG',
'A_PL_COUNT', 'A_PL_BF_ZCOMP', 'A_PL_THS_REG', 'A_FFMT_CFG', 'A_FFMT_THS', 'A_FFMT_COUNT',
'A_VECM_CFG', 'A_VECM_THS_MSB', 'A_TRANSIENT_CFG',
'A_TRANSIENT_THS', 'A_TRANSIENT_COUNT', 'A_PULSE_CFG', 'A_PULSE_TMLT', 'A_PULSE_LTCY',
'A_OFF_X', 'A_OFF_Y', 'A_OFF_Z']
config[0] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_TRIG_CFG)
config[1] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG1)
config[2] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG2)
config[3] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG3)
config[4] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG4)
config[5] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_CTRL_REG5)
config[6] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_ASPL_COUNT)
config[7] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_F_SETUP)
config[8] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_XYZ_DATA_CFG)
config[9] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_HP_FILTER_CUTOFF)
config[10] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PL_CFG)
config[11] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PL_COUNT)
config[12] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PL_BF_ZCOMP)
config[13] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PL_THS_REG)
config[14] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_FFMT_CFG)
config[15] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_FFMT_THS)
config[16] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_FFMT_COUNT)
config[17] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_VECM_CFG)
config[18] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_VECM_THS_MSB)
config[19] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_TRANSIENT_CFG)
config[20] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_TRANSIENT_THS)
config[21] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_TRANSIENT_COUNT)
config[22] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PULSE_CFG)
config[23] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PULSE_TMLT)
config[24] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_PULSE_LTCY)
config[25] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OFF_X)
config[26] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OFF_Y)
config[27] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, A_OFF_Z)
if sensor == 'm':
config = [None] * 15
_regName = ['M_OFF_X_MSB', 'M_OFF_X_LSB', 'M_OFF_Y_MSB', 'M_OFF_Y_LSB', 'M_OFF_Z_MSB', 'M_OFF_Z_LSB',
'M_THS_CFG', 'M_THS_COUNT',
'M_CTRL_REG1', 'M_CTRL_REG2', 'M_CTRL_REG3', 'M_VECM_CFG', 'M_VECM_THS_MSB', 'M_VECM_THS_LSB',
'M_VECM_CNT']
config[0] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OFF_X_MSB)
config[1] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OFF_X_LSB)
config[2] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OFF_Y_MSB)
config[3] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OFF_Y_LSB)
config[4] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OFF_Z_MSB)
config[5] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_OFF_Z_LSB)
config[6] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_THS_CFG)
config[7] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_THS_COUNT)
config[8] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_CTRL_REG1)
config[9] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_CTRL_REG2)
config[10] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_CTRL_REG3)
config[11] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_VECM_CFG)
config[12] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_VECM_THS_MSB)
config[13] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_VECM_THS_LSB)
config[14] = self._i2cBus.read_byte_data(I2C_AM_ADDRESS, M_VECM_CNT)
if sensor == 'g':
config = [None] * 8
_regName = ['G_F_SETUP', 'G_CTRL_REG0', 'G_RT_CFG', 'G_RT_THS', 'G_RT_COUNT', 'G_CTRL_REG1', 'G_CTRL_REG2',
'G_CTRL_REG3']
config[0] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_F_SETUP)
config[1] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_CTRL_REG0)
config[2] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_RT_CFG)
config[3] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_RT_THS)
config[4] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_RT_COUNT)
config[5] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_CTRL_REG1)
config[6] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_CTRL_REG2)
config[7] = self._i2cBus.read_byte_data(I2C_G_ADDRESS, G_CTRL_REG3)
if screen == 1:
for i, reg in enumerate(_regName):
print reg + ': ' + str('0x{:02x}'.format(config[i]))
return config
"""
import yrk.settings as s, yrk.utils as utils
import smbus2 #I2C function
import time, logging, os
# On YRL040, GPIO 6 is interrupt in
# IO0-0 : IO0-7 USER 8-BIT GPIO CONNECTOR [I/O]
# IO1-0 : IO1-3 MOTOR FAULT [I]
# IO1-4 5V SWITCHED OUTPUT [O]
# IO1-5 12V SWITCHED OUTPUT [O]
# IO1-6 MOTOR FAULT LED [O]
# IO1-7 KILL SWITCH [I]
try:
i2c = smbus2.SMBus(s.YRL040_BUS)
init_okay = True
except FileNotFoundError:
logging.error("[gpio.py] : Cannot access /dev/i2c-%d" % (s.YRL040_BUS))
init_okay = False
s.BUS_ERROR = True
switched_output_5V = False
switched_output_12V = False
motor_fault_led = True #Inverted output [True=LED off]
def setup_user_gpio():
"""An initialisation function for the PCA9555 GPIO Expander for the user GPIO and motor fault detection
Sets pins IO0_0 : IO0_7 based on values from settings.py
def __init__(self, address, bus=1):
self.address = address
self.bus = smbus.SMBus(bus)
# Wake up the MPU-6050 since it starts in sleep mode
self.bus.write_byte_data(self.address, self.PWR_MGMT_1, 0x00)
def __init__(self, bus_id, debug=False):
self.isDebug = debug
if debug == False:
self.bus = smbus2.SMBus(bus_id)
from azure.iot.device.aio import IoTHubDeviceClient, ProvisioningDeviceClient
from datetime import datetime, date
import smbus2, bme280, os, asyncio, json, time
from grove.grove_moisture_sensor import GroveMoistureSensor
from dotenv import load_dotenv
from grove.grove_light_sensor_v1_2 import GroveLightSensor
# Configuration parameters
bme_pin = 1
bme_address = 0x76
moisture_pin = 2
light_pin = 0
# Create the sensors
bus = smbus2.SMBus(bme_pin)
calibration_params = bme280.load_calibration_params(bus, bme_address)
moisture_sensor = GroveMoistureSensor(moisture_pin)
light_sensor = GroveLightSensor(light_pin)
load_dotenv()
id_scope = os.getenv('ID_SCOPE')
device_id = os.getenv('DEVICE_ID')
primary_key = os.getenv('PRIMARY_KEY')
def getTemperaturePressureHumidity():
return bme280.sample(bus, bme_address, calibration_params)
import smbus2 as smbus
import math, time
import numpy as np
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68 # This is the address value read via the i2cdetect command
# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
def read_all_six():
block = bus.read_i2c_block_data(address, 0x3b, 16)
all_six = [(block[i] << 8) + block[i + 1] for i in [0, 2, 4, 8, 10, 12]]
return np.array(all_six[:3]), np.array(all_six[3:])
def read_word(adr):
high = bus.read_byte_data(address, adr)
low = bus.read_byte_data(address, adr + 1)
word = bus.read_word_data(address, adr)
block = bus.read_i2c_block_data(address, adr - 1, 16)
val = (high << 8) + low
print(high, low, word, val, block)
for i in range(99999):
#acc_xout = bus.read_word_data(address,0x3b)
#acc_yout = bus.read_word_data(address,0x3d)
def update_max_if_new_is_larger_than_max(new, max):
print("update_max_if_new_is_larger_than_max called")
if new > max:
print("max observed")
update_text(MAX_DECIBEL_FILE_PATH, 'MAX: {:.2f} dBA'.format(new))
click('update_max_decibel')
return new
else:
return max
shutdownPin = 6
resetPin = 5
i2c = smbus.SMBus(1) # 1 is bus number
addr02 = 0x3e #lcd
_command = 0x00
_data = 0x40
_clear = 0x01
_home = 0x02
display_On = 0x0f
LCD_2ndline = 0x40 + 0x80
time.sleep(1)
#LCD AQM0802/1602
def command(code):
i2c.write_byte_data(addr02, _command, code)
#time.sleep(0.1)
def __init__(self, addr, port=1):
self.addr = addr
self.bus = smbus.SMBus(port)
import smbus2
import math
import numpy as np
bus= = smbus2.SMBus(1)
DEVICE_ADDRESS = 0x1e
bus.write_byte_data(DEVICE_ADDRESS,0x20,0b11101100)
bus.write_byte_data(DEVICE_ADDRESS,0x23,0b00001100)
bus.write_byte_data(DEVICE_ADDRESS,0x24,0b00000000)
def calc():
xval_l = bus.read_byte_data(DEVICE_ADDRESS, 0x28)
xval_m = bus.read_byte_data(DEVICE_ADDRESS, 0x29)
yval_l = bus.read_byte_data(DEVICE_ADDRESS, 0x2A)
yval_m = bus.read_byte_data(DEVICE_ADDRESS, 0x2B)
zval_l = bus.read_byte_data(DEVICE_ADDRESS, 0x2C)
zval_m = bus.read_byte_data(DEVICE_ADDRESS, 0x2D)
xval = xval_m << 8 | xval_l
yval = yval_m << 8 | yval_l
zval = zval_m << 8 | zval_l
x = twos_comp(xval, 16)
y = twos_comp(yval, 16)
z = twos_comp(zval, 16)
print("X=",x)
print("Y=",y)
print("Z=",z)
heading=math.atan2(y,x)*180./np.pi
if heading<0:
heading+=360
print("heading=",heading)
def __init__(self, address=0x29, debug=True):
# Depending on if you have an old or a new Raspberry Pi, you
# may need to change the I2C bus. Older Pis use SMBus 0,
# whereas new Pis use SMBus 1. If you see an error like:
# 'Error accessing 0x29: Check your I2C address '
# change the SMBus number in the initializer below!
# setup i2c bus and SFR address
self.i2c = smbus2.SMBus(1)
self.address = address
self.debug = debug
# Module identification
self.idModel = 0x00
self.idModelRevMajor = 0x00
self.idModelRevMinor = 0x00
self.idModuleRevMajor = 0x00
self.idModuleRevMinor = 0x00
self.idDate = 0x00
self.idTime = 0x00
# Print statement updated to Python 3.X
if self.get_register(self.__VL6180X_SYSTEM_FRESH_OUT_OF_RESET) == 1:
print("ToF sensor is ready.")
self.ready = True
else:
print("ToF sensor reset failure.")
self.ready = False
# Required by datasheet
# http://www.st.com/st-web-ui/static/active/en/resource/technical/document/application_note/DM00122600.pdf
self.set_register(0x0207, 0x01)
self.set_register(0x0208, 0x01)
self.set_register(0x0096, 0x00)
self.set_register(0x0097, 0xfd)
self.set_register(0x00e3, 0x00)
self.set_register(0x00e4, 0x04)
self.set_register(0x00e5, 0x02)
self.set_register(0x00e6, 0x01)
self.set_register(0x00e7, 0x03)
self.set_register(0x00f5, 0x02)
self.set_register(0x00d9, 0x05)
self.set_register(0x00db, 0xce)
self.set_register(0x00dc, 0x03)
self.set_register(0x00dd, 0xf8)
self.set_register(0x009f, 0x00)
self.set_register(0x00a3, 0x3c)
self.set_register(0x00b7, 0x00)
self.set_register(0x00bb, 0x3c)
self.set_register(0x00b2, 0x09)
self.set_register(0x00ca, 0x09)
self.set_register(0x0198, 0x01)
self.set_register(0x01b0, 0x17)
self.set_register(0x01ad, 0x00)
self.set_register(0x00ff, 0x05)
self.set_register(0x0100, 0x05)
self.set_register(0x0199, 0x05)
self.set_register(0x01a6, 0x1b)
self.set_register(0x01ac, 0x3e)
self.set_register(0x01a7, 0x1f)
self.set_register(0x0030, 0x00)
if self.debug:
# Print statement updated to Python 3.X
print("Register settings:")
print("0x0207 - ", hex(self.get_register(0x0207)))
print("0x0208 - ", hex(self.get_register(0x0208)))
print("0x0096 - ", hex(self.get_register(0x0096)))
print("0x0097 - ", hex(self.get_register(0x0097)))
print("0x00e3 - ", hex(self.get_register(0x00e3)))
print("0x00e4 - ", hex(self.get_register(0x00e4)))
print("0x00e5 - ", hex(self.get_register(0x00e5)))
print("0x00e6 - ", hex(self.get_register(0x00e6)))
print("0x00e7 - ", hex(self.get_register(0x00e7)))
print("0x00f5 - ", hex(self.get_register(0x00f5)))
print("0x00d9 - ", hex(self.get_register(0x00d9)))
print("0x00db - ", hex(self.get_register(0x00db)))
print("0x00dc - ", hex(self.get_register(0x00dc)))
print("0x00dd - ", hex(self.get_register(0x00dd)))
print("0x009f - ", hex(self.get_register(0x009f)))
print("0x00a3 - ", hex(self.get_register(0x00a3)))
print("0x00b7 - ", hex(self.get_register(0x00b7)))
print("0x00bb - ", hex(self.get_register(0x00bb)))
print("0x00b2 - ", hex(self.get_register(0x00b2)))
print("0x00ca - ", hex(self.get_register(0x00ca)))
print("0x0198 - ", hex(self.get_register(0x0198)))
print("0x01b0 - ", hex(self.get_register(0x01b0)))
print("0x01ad - ", hex(self.get_register(0x01ad)))
print("0x00ff - ", hex(self.get_register(0x00ff)))
print("0x0100 - ", hex(self.get_register(0x0100)))
print("0x0199 - ", hex(self.get_register(0x0199)))
print("0x01a6 - ", hex(self.get_register(0x01a6)))
print("0x01ac - ", hex(self.get_register(0x01ac)))
print("0x01a7 - ", hex(self.get_register(0x01a7)))
print("0x0030 - ", hex(self.get_register(0x0030)))
def __init__(self):
self.bus = smbus.SMBus(I2C_bus_number)
self.I2C_setup(0xff)
import bme280
import smbus2
from time import sleep
I2C_PORT = 1
I2C_BME_ADDRESS = 0x77
I2C_BUS = smbus2.SMBus(I2C_PORT)
bme280.load_calibration_params(I2C_BUS, I2C_BME_ADDRESS)
def getBME():
bme280_data = bme280.sample(I2C_BUS, I2C_BME_ADDRESS)
return {
"humidity": bme280_data.humidity,
"pressure": bme280_data.pressure,
"ambient_temperature": bme280_data.temperature
}
print(getBME())
n1 = [0, 0, 1] n2 = [0, 1, 0] n3 = [0, 1, 1] n4 = [1, 0, 0] n5 = [1, 0, 1] n6 = [1, 1, 0] n7 = [1, 1, 1] # Drehzahlmesser declaration PIN_DZM_L = 19 PIN_DZM_R = 18 DZ_L = 0 DZ_R = 0 # I2C declaration BUS = smbus2.SMBus(1) ENGINE_LEFT_ADD = 0x00 ENGINE_RIGHT_ADD = 0x00 GYRO_ADD = 0x00 I2C_MOTORDRIVER_ADD = 0x00 # MOTOR-CONTROL parameter # Speed SPEED_LEFT = 0 SPEED_FORWAD = 130 SPEED_RIGHT = 0 MIN_SPEED = 10 MAX_SPEED = 100 MAX_MAX_SPEED = 150 # Direction-Optimum DESIRED_DIST_L = 20
def __init__(self, address, illuminance_offset=0.0, round=2):
self.__bus = smbus2.SMBus(1)
self.__address = address
self.__illuminance_offset = illuminance_offset
self.__round = round
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
from ctypes import *
import smbus2
VL53L0X_GOOD_ACCURACY_MODE = 0 # Good Accuracy mode
VL53L0X_BETTER_ACCURACY_MODE = 1 # Better Accuracy mode
VL53L0X_BEST_ACCURACY_MODE = 2 # Best Accuracy mode
VL53L0X_LONG_RANGE_MODE = 3 # Longe Range mode
VL53L0X_HIGH_SPEED_MODE = 4 # High Speed mode
i2cbus = smbus2.SMBus(1)
# i2c bus read callback
def i2c_read(address, reg, data_p, length):
ret_val = 0
result = []
try:
result = i2cbus.read_i2c_block_data(address, reg, length)
except IOError:
ret_val = -1
if (ret_val == 0):
for index in range(length):
data_p[index] = result[index]
def __init__(self, busnum=1, motor_address=None):
if motor_address is None:
motor_address = self.MOTOR_ADDR_L
self.bus = smbus.SMBus(busnum)
self.MOTOR_ADDRESS = motor_address
def __init__(self):
self.addr = 0x68
self.reg = 0x3f
self.pwr_mgmt = 0x6b
self.bus = smbus2.SMBus(1)
self.bus.write_byte_data(self.addr, self.pwr_mgmt, 0)
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
'''
#################################################################################################################################################
# NOTE:
# The software for this sensor is still in development and might make your GrovePi unuable as long as this sensor is connected with the GrovePi
#################################################################################################################################################
import time, sys
import RPi.GPIO as GPIO
import smbus2 as smbus
debug = 1
# use the bus that matches your raspi version
rev = GPIO.RPI_REVISION
if rev == 2 or rev == 3:
bus = smbus.SMBus(1)
else:
bus = smbus.SMBus(0)
class th02:
ADDRESS = 0x40
TH02_REG_STATUS = 0x00
TH02_REG_DATA_H = 0x01
TH02_REG_DATA_L = 0x02
TH02_REG_CONFIG = 0x03
TH02_REG_ID = 0x11
TH02_STATUS_RDY_MASK = 0x01
def __init__(self):
self.alphabet = {
'q': int('10000000', 2),
'a': int('01000000', 2),
'z': int('00100000', 2),
'w': int('00010000', 2),
's': int('00001000', 2),
'x': int('00000100', 2),
'e': int('00000010', 2),
'd': int('00000001', 2),
'c': int('00000001', 2),
'r': int('00000010', 2),
'f': int('00000100', 2),
'v': int('00001000', 2),
't': int('00010000', 2),
'g': int('00100000', 2),
'b': int('01000000', 2),
'y': int('10000000', 2),
'h': int('00000001', 2),
'n': int('00000010', 2),
'u': int('00000100', 2),
'j': int('00001000', 2),
'm': int('00010000', 2),
'i': int('00100000', 2),
'k': int('01000000', 2),
',': int('10000000', 2),
'o': int('10000000', 2),
'l': int('01000000', 2),
'p': int('00100000', 2),
'-': int('00010000', 2)
}
self.icaddr = {
'q': self.ICADDR_1,
'w': self.ICADDR_1,
'e': self.ICADDR_1,
'r': self.ICADDR_1,
't': self.ICADDR_1,
'y': self.ICADDR_1,
'a': self.ICADDR_1,
's': self.ICADDR_1,
'd': self.ICADDR_1,
'f': self.ICADDR_1,
'g': self.ICADDR_1,
'z': self.ICADDR_1,
'x': self.ICADDR_1,
'c': self.ICADDR_1,
'v': self.ICADDR_1,
'b': self.ICADDR_1,
'u': self.ICADDR_2,
'i': self.ICADDR_2,
'o': self.ICADDR_2,
'p': self.ICADDR_2,
'h': self.ICADDR_2,
'j': self.ICADDR_2,
'k': self.ICADDR_2,
'l': self.ICADDR_2,
'-': self.ICADDR_2,
'n': self.ICADDR_2,
'm': self.ICADDR_2,
',': self.ICADDR_2
}
self.register = {
'q': self.REG_OLAT_A,
'w': self.REG_OLAT_A,
'e': self.REG_OLAT_A,
'r': self.REG_OLAT_B,
't': self.REG_OLAT_B,
'y': self.REG_OLAT_B,
'a': self.REG_OLAT_A,
's': self.REG_OLAT_A,
'd': self.REG_OLAT_A,
'f': self.REG_OLAT_B,
'g': self.REG_OLAT_B,
'z': self.REG_OLAT_A,
'x': self.REG_OLAT_A,
'c': self.REG_OLAT_B,
'v': self.REG_OLAT_B,
'b': self.REG_OLAT_B,
'u': self.REG_OLAT_B,
'i': self.REG_OLAT_B,
'o': self.REG_OLAT_A,
'p': self.REG_OLAT_A,
'h': self.REG_OLAT_B,
'j': self.REG_OLAT_B,
'k': self.REG_OLAT_B,
'l': self.REG_OLAT_A,
'-': self.REG_OLAT_A,
'n': self.REG_OLAT_B,
'm': self.REG_OLAT_B,
',': self.REG_OLAT_B
}
self.bus = smbus.SMBus(self.CHANNEL)
def __init__(self):
self.bus = smbus.SMBus(1) # Rev 2 Pi uses 1
self.ONE_TIME_HIGH_RES_MODE_1 = 0x20
