def accelVibeInTimeInterval(timeInterval=5, accelThreshold=1):
'''
This function will run through the accelerometer values over the time
interval indicated in timeInterval. If at any point during the timeInterval
the value exceeds the threshold value it will return a true else, it will return
false
timeInterval = time interval over which accelerometer value is checked in seconds
accelThreshold = acceleration over 1g which the accelerometer must exceed to be
considered on. Value in m/s^2
'''
sensor1 = mpu6050(0x68)
sensor2 = mpu6050(0x69)
sensor1On = False
sensor2On = False
accelList1 = []
accelList2 = []
for loopTimer in range(timeInterval * 10):
absAccel1, absAccel2 = getAccelerometerValues(sensor1, sensor2)
accelList1.append(absAccel1)
accelList2.append(absAccel2)
sleep(0.1)
if max(accelList1) > accelThreshold:
sensor1On = True
if max(accelList2) > accelThreshold:
sensor2On = True
return sensor1On, sensor2On
def __init__(self):
self.gestures = {"arm_down.txt": "0", "arm_straight.txt": "1", "arm_across.txt": "2", "elbow_table.txt": "3"}
self.sensor1 = mpu6050(0x69)
self.sensor2 = mpu6050(0x68)
self.svm_model = None
"""
def play():
p = pyaudio.PyAudio()
sensor = mpu6050(0x68)
vol = 1.0 # range [0.0, 1.0]
fs = 4000 # sampling rate, Hz, must be integer
duration = 5 # in seconds, may be float
#for i in samples:
# print i,"\n"
# play. May repeat with different volume values (if done interactively)
while(1):
print "\nstarting"
data = sensor.get_accel_data()
r_acc = pow((pow(data["x"],2) + pow(data["y"],2) + pow(data["z"],2)),0.5)
# print r_acc
x_angle = math.acos(data["x"]/r_acc)
y_angle = math.acos(data["y"]/r_acc)
z_angle = math.acos(data["z"]/r_acc)
vol = volume(x_angle,y_angle,z_angle)/100
print vol
ll = octave(x_angle,y_angle,z_angle)
print "octave number : " , ll
f=frequency(read_distance(),ll)
print f
# generate samples, note conversion to float32 array
samples = (np.sin(2*np.pi*np.arange(fs*duration)*f/fs)).astype(np.float32)
# for paFloat32 sample values must be in range [-1.0, 1.0]
stream = p.open(format=pyaudio.paFloat32,channels=1,rate=fs,output=True)
stream.write(vol*samples)
stream.stop_stream()
stream.close()
def init(self):
threading.Thread.__init__(self)
self.sensor = mpu6050(0x68)
self.kalmanX = Kalman_py()
self.kalmanY = Kalman_py()
sleep(0.1)
self.acc = self.sensor.get_accel_data()
self.gyro = self.sensor.get_gyro_data()
self.temp = self.sensor.get_temp()
self.kalAngleX = 0.0
self.kalAngleY = 0.0
self.roll = math.degrees(math.atan2(self.acc['y'], self.acc['z']))
self.pitch = math.degrees(
math.atan(-self.acc['x'] /
math.sqrt(self.acc['y']**2 + self.acc['z']**2)))
self.kalmanX.setAngle(self.roll)
self.kalmanY.setAngle(self.pitch)
self.gyroXangle = self.roll
self.gyroYangle = self.pitch
self.compAngleX = self.roll
self.compAngleY = self.pitch
self.micros = lambda: int(time())
self.timer = self.micros()
self.t = self.micros()
def __init__(self, mpuadd, qmcadd, bmeadd, pwmadd, pwmfrequency):
self.mpuaddress = mpuadd
self.qmcaddress = qmcadd
self.bmeaddress = bmeadd
self.pwmaddress = pwmadd
self.qmcpollingrate = py_qmc5883l.ODR_50HZ
self.i2cport = 1
#connect to IC's
#adafruit pca9685
self.pwm = Adafruit_PCA9685.PCA9685(address=self.pwmaddress)
self.pwm.set_pwm_freq(pwmfrequency)
self.pwm.set_pwm(0, 0, 360)
self.pwm.set_pwm(1, 0, 360)
self.pwm.set_pwm(2, 0, 360)
self.pwm.set_pwm(3, 0, 360)
self.mpuaccelrange = mpu6050.ACCEL_RANGE_4G
#bme280
self.bus = smbus2.SMBus(self.i2cport)
self.bmecalibration_params = bme280.load_calibration_params(self.bus, self.bmeaddress)
#mpu6050
self.mpu6050 = mpu6050(self.mpuaddress)
#QMC5883L-TL
self.qmc5833lcalibrationdata = [[1.0303, 0.0255, -227.7989],
[0.0255, 1.0214, 1016.4415],
[0.0, 0.0, 1.0]]
self.qmc5883L = py_qmc5883l.QMC5883L(output_range=py_qmc5883l.RNG_2G, output_data_rate=self.qmcpollingrate)
#init kalman filtered mpu
self.kalmanfilter = AngleMeterAlpha.AngleMeterAlpha()
self.kalmanfilter.MPU_Init()
self.kalmanfilter.measure()
def __init__(self, speed=1):
self.motors = Motors()
self.speed = speed
self.calibrate_gyro = (-4.8,1.35,-1.15)
self.mpu = mpu6050(0x68)
self.rotation = {}
self.orientation = {}
def __init__(self):
self.ACCEL_SAMPLE_SIZE = 32
self.GYRO_SAMPLE_SIZE = 32
self.SENSOR_TOLERANCE = 1
self.SENSOR_TARGET_POS = 0
self.SERVO_0_PIN = 23
self.SERVO_1_PIN = 24
self.SERVO_MIN = 800
self.SERVO_MAX = 2500
self.SERVO_TIME_DELAY_MIN = 1
self.SERVO_TIME_DELAY_MAX = 1.5
self.ACTION_REWARD_FACTOR = 100
self.ACTION_REWARD_MAX = 150
self.TIME_PEN_FACTOR = 0
self.servo = pigpio.pi()
self.servo.set_servo_pulsewidth(self.SERVO_0_PIN, 1650)
self.servo.set_servo_pulsewidth(self.SERVO_1_PIN, 1650)
self.sensor = mpu6050(0x68)
self.time_start = 0
self.time_stop = 0
def read_data():
sensor = mpu6050(0x68)
ac = sensor.get_accel_data()
gs = sensor.get_gyro_data()
print ac
print gs
return ac, gs
def main():
global client, sensor
# register the signal handler
signal.signal(signal.SIGINT, signal_handler)
status = "default"
# parse the command line arguments
try:
opts, args = getopt.getopt(sys.argv[1:], "i:")
except getopt.GetoptError:
print('GET OPT ERROR')
for opt, arg in opts:
if opt == '-i':
status = arg
sensor = mpu6050(0x68)
try:
options = {
"org": "sat52l",
"id": "therpi",
"auth-method": "apikey",
"auth-key": "a-sat52l-fdaspx5lja",
"auth-token": "dC8TO5j?ol8jUgVlJe"
}
client = ibmiotf.application.Client(options)
client.connect()
print("Connected to IBM Cloud!!")
except ibmiotf.ConnectionException as e:
print(e)
print("starting the loop")
loop()
def main():
global client, mpu, dht11
# register the signal handler
signal.signal(signal.SIGINT, signal_handler)
mpu = mpu6050(0x68)
dht11 = Adafruit_DHT.DHT11
try:
options = {
"org": "ml5pjc",
"id": "rpi",
"auth-method": "apikey",
"auth-key": "a-ml5pjc-tfl4ng9v5z",
"auth-token": "X0-327z9TUSLql6?dC"
}
client = ibmiotf.application.Client(options)
client.connect()
print("Connected to IBM Cloud!!")
except ibmiotf.ConnectionException as e:
print(e)
print("starting the loop")
loop()
def steadyProcessing(self):
global MPU_connection, sensor
if MPU_connection:
try:
valueGet = sensor.get_accel_data()
except:
try:
sensor = mpu6050(0x68)
MPU_connection = 1
print('mpu6050 connected')
except:
MPU_connection = 0
print('mpu6050 disconnected')
else:
return
xGet = kfX.kalman(valueGet['x'])
yGet = kfY.kalman(valueGet['y'])
xDebug = xGet - self.xMiddle
yDebug = yGet - self.yMiddle
self.pitchValue = rangeCtrl((minHeight - middleHeight),
(maxHeight - middleHeight),
self.pitchValue + xDebug * self.valueP)
self.rollValue = rangeCtrl((minHeight - middleHeight),
(maxHeight - middleHeight),
self.rollValue - yDebug * self.valueP)
# print('debug:', xDebug)
# print('pitch:', self.pitchValue)
try:
pitchRoll(self.pitchValue, self.rollValue)
except:
pass
time.sleep(self.mpuDelay)
def __init__(self, config):
from mpu6050 import mpu6050
self.sensor = mpu6050(config.getint('address'))
self.fusionPose = np.array([0.,0.,0.])
self.calib_counter = 0
self.calib_offset = np.array([0.,0.,0.])
def __init__(self):
self.Kp = 100
self.Ki = 0.002
self.halfT = 0.001
self.q0 = 1
self.q1 = 0
self.q2 = 0
self.q3 = 0
self.exInt = 0
self.eyInt = 0
self.ezInt = 0
self.pitch = 0
self.roll =0
self.yaw = 0
self.sensor = mpu6050(address=0x68)
self.sensor.set_accel_range(mpu6050.ACCEL_RANGE_2G)
self.sensor.set_gyro_range(mpu6050.GYRO_RANGE_250DEG)
self.kalman_filter_AX = Kalman_filter(0.001,0.1)
self.kalman_filter_AY = Kalman_filter(0.001,0.1)
self.kalman_filter_AZ = Kalman_filter(0.001,0.1)
self.kalman_filter_GX = Kalman_filter(0.001,0.1)
self.kalman_filter_GY = Kalman_filter(0.001,0.1)
self.kalman_filter_GZ = Kalman_filter(0.001,0.1)
self.Error_value_accel_data,self.Error_value_gyro_data=self.average_filter()
def steady():
global sensor
if steadyMode:
if MPU_connection:
try:
accelerometer_data = sensor.get_accel_data()
X = accelerometer_data['x']
X = kalman_filter_X.kalman(X)
Y = accelerometer_data['y']
Y = kalman_filter_Y.kalman(Y)
#X_error = X_pid.GenOut(X_steady-X)
#Y_error = Y_pid.GenOut(Y_steady-Y)
X_error = X_steady - X
Y_error = Y_steady - Y
if abs(X_error) > mpu_tor or abs(Y_error) > mpu_tor:
status_GenOut(0, X_error * P, Y_error * P)
direct_M_move()
# print('X:%f'%X_error)
# print('Y:%f'%Y_error)
except:
time.sleep(0.1)
sensor = mpu6050(0x68)
pass
def __init__(self, i2cAddress, busnum=1):
self.busy = False
self.initialized = False
self.i2cAddress = int(i2cAddress)
self.busnum = int(busnum)
self.ax = 0
self.ay = 0
self.az = 0
self.gx = 0
self.gy = 0
self.gz = 0
try:
self.mpu = mpu6050(i2cAddress, bus=busnum)
self.mpu.set_gyro_range(mpu6050.GYRO_RANGE_250DEG)
self.mpu.bus.write_byte_data(i2cAddress, self.SIGNAL_PATH_RESET,
0x07)
self.mpu.bus.write_byte_data(i2cAddress, self.I2C_SLV0_ADDR,
0x80 | 0x40)
# self.mpu.bus.write_byte_data(i2cAddress, self.ACCEL_CONFIG, 0x01)
self.mpu.bus.write_byte_data(i2cAddress, self.MOT_THR, 2) # 0x14
self.mpu.bus.write_byte_data(i2cAddress, self.MOT_DUR, 1)
self.mpu.bus.write_byte_data(i2cAddress, self.MOT_DETECT_CTRL,
0x15)
self.mpu.bus.write_byte_data(i2cAddress, self.INT_ENABLE, 0x40)
self.mpu.set_accel_range(mpu6050.ACCEL_RANGE_2G)
self.initialized = True
except Exception as e:
print(e)
self.initialized = False
def create_sensor(mockmode):
if not mockmode:
from mpu6050 import mpu6050
return mpu6050(0x68)
else:
from mock_mpu6050 import MockMPU6050
return MockMPU6050()
def mpu6050_data_generator(dt, stopped):
from mpu6050 import mpu6050
sensor = mpu6050(0x68)
# try to poll the sensor until it configures properly
started_at = time.time()
last_err = None
while time.time() < started_at + 1:
try:
sensor.get_all_data()
except IOError as err:
last_err = err
time.sleep(0.02)
else:
break
else:
raise IOError('MPU6050 reading fails: {}'.format(str(last_err)))
while not stopped.isSet():
start = time.time()
accel = sensor.get_accel_data()
gyro = sensor.get_gyro_data()
temp = sensor.get_temp()
item = accel['x'], accel['y'], accel['z'], gyro['x'], gyro['y'], gyro[
'z'], temp
yield item
duration = time.time() - start
if dt > duration:
time.sleep(dt - duration)
def __init__(self, address=device_address, bus=i2c_bus):
self.mpu = mpu6050(address, bus)
time.sleep(1)
accel = self.mpu.get_accel_rawdata()
#print (accel)
if self.RestrictPitch:
roll = math.atan2(accel["y"], accel["z"]) * radToDeg
pitch = math.atan(
-accel["x"] / math.sqrt((accel["y"]**2) +
(accel["z"]**2))) * radToDeg
else:
roll = math.atan(
accel["y"] / math.sqrt((accel["x"]**2) +
(accel["z"]**2))) * radToDeg
pitch = math.atan2(-accel["x"], accel["z"]) * radToDeg
self.kalmanX.setAngle(roll)
self.kalmanY.setAngle(pitch)
self.gyroXAngle = roll
self.gyroYAngle = pitch
self.compAngleX = roll
self.compAngleY = pitch
self.timer = time.time()
def __init__(self):
# ROS Parameters:
self.ori_cov = float(rospy.get_param('~ori_cov', '0.0025') ) # Orientation covariance
self.vel_cov = float(rospy.get_param('~vel_cov', '0.02') ) # Angular velocity covariance
self.acc_cov = float(rospy.get_param('~acc_cov', '0.04') ) # Linear acceleration covariance
self.imu_i2c = rospy.get_param('~imu_i2c', '0x68') # I2C device No
self.imu_link = rospy.get_param('~imu_link', 'imu_link') # imu_link name
self.pub_freq = float( rospy.get_param('~pub_freq', '50') ) # hz of imu pub
# I2C Communication:
try:
self.sensor = mpu6050(0x68)
rospy.loginfo("Flusing first 50 data readings ...")
for x in range(0, 50):
gyro_data = self.sensor.get_gyro_data()
time.sleep(0.01)
except:
rospy.logerr("Can not receive data from the I2C device: "+ self.imu_i2c +
". Did you specify the correct No. ?")
sys.exit(0)
rospy.loginfo("Communication success !")
# ROS handler
self.pub = rospy.Publisher('imu_data', Imu, queue_size=1)
self.timer_pub = rospy.Timer(rospy.Duration(1.0/self.pub_freq), self.timerCB)
def __init__(self, addr=0x68, poll_delay=0.0166):
from mpu6050 import mpu6050
self.sensor = mpu6050(addr)
self.accel = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.gyro = { 'x' : 0., 'y' : 0., 'z' : 0. }
self.temp = 0.
self.poll_delay = poll_delay
self.on = True
def __init__(self):
self.__gyro_offset = self.__zero()
self.__accel_offset = self.__zero()
self.__gyro = self.__zero()
self.__accel = self.__zero()
self.__step = 0
self.__sensor = mpu6050(self.ADDRESS)
thread.start_new_thread(self.update, ())
def __init__(self):
'''Initialize parameters - the game board, moves stack and winner'''
self.board = [ '-' for i in range(0,9) ]
self.mpu6050 = []
self.mpu6050 = [ mpu6050(0x68, i) for i in range(1,2)]
self.mpu6050[0].offset= self.mpu6050[0].calculate_offset(5,5,5,5)
def init(self, address=0x68):
success = False
try:
self.accelerometer = mpu6050(address)
success = True
except Exception as e:
logging.debug('MPU6050 - not present: {0}'.format(e))
return success
def sensor_setup(all_settings):
ds = DS18B20()
if all_settings['mpu6050_settings']['i2c_addr'] == 68:
mpu_addr = 0x68
mpu = mpu6050(mpu_addr)
pump = relays(all_settings['pump_settings'])
heater = relays(all_settings['heater_settings'])
return ds, mpu, pump, heater
def __init__(self):
super().__init__('mpu6050_publisher')
self.accel_publisher_ = self.create_publisher(Accel, '/mpu6050/accel',
10)
self.gyro_publisher_ = self.create_publisher(Gyro, '/mpu6050/gyro', 10)
self.sensor = mpu6050(Device_Address)
self.accel_timer_ = self.create_timer(0.1, self.publish_accel)
self.gyro_timer_ = self.create_timer(0.1, self.publish_gyro)
def __init__(self, address): self.sensor = mpu6050(address) # configure the gyro sensor # let it here be hardcoded because maybe we'll change the sensor # in the future and then we won't be able to use the same configs # self.sensor.set_gyro_range(mpu6050.GYRO_RANGE_250DEG) self.sensor.set_gyro_range(mpu6050.GYRO_RANGE_2000DEG) # self.sensor.set_accel_range(mpu6050.ACCEL_RANGE_2G) self.sensor.set_accel_range(mpu6050.ACCEL_RANGE_8G)
def initTurbineVibeSensor():
global accelerometer
try:
accelerometer = mpu6050(0x68)
print("Turbine vibration sensor is connected")
except:
print(
"The turbine appears to be disconnected. Please check the connection."
)
def __init__(self,
address=0x68,
rate=10): # the default publishing frequency is 10 Hz
self.mpu = mpu6050(address)
rospy.init_node('mpu6050')
self.pub = rospy.Publisher('imu/data_raw', Imu, queue_size=1)
self.r = rospy.Rate(rate)
self.imu = Imu()
self.main()
def mpu6050read():
sensor = mpu6050(0x68)
while True:
k = sensor.get_accel_data()
print('x= %f, y= %f, z= %f\r' % (k['x'], k['y'], k['z']), end='')
time.sleep(0.1)
pass
def get_accel():
sensor = mpu6050(0x68)
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
for i in ["x", "y", "z"]:
accel_data[i] = str(accel_data[i])
gyro_data[i] = str(gyro_data[i])
return accel_data, gyro_data
def __init__(self):
rospy.init_node('imu')
self.publish_data=True
self.sensor = mpu6050(0x68)
self.imu_data=imu_data()
self.imu_event=imu_event()
self.checking_acc=False
self.acc_counter=0
self.acc_thres=0
self.rate=100
# Init publisher
self.pub_data = rospy.Publisher('imu/data', imu_data,queue_size=100)
self.pub_event = rospy.Publisher('imu/event', imu_event,queue_size=100)
# Init services
s_en = rospy.Service('publish_imu_data', Empty, self.enable_publishing)
s_dis = rospy.Service('disable_publish_imu_data', Empty, self.disable_publishing)
def __init__(self):
'''Initialize parameters - the game board, moves stack and winner'''
self.board = [ '-' for i in range(0,9) ]
self.lastmoves = []
self.winner = None
self.mpu6050 = []
self.mpu6050 = [ mpu6050(0x68, i) for i in range(1,5)]
#for i in range(0,3):
self.mpu6050[0].offset= self.mpu6050[0].calculate_offset(5,5,5,5)
self.mpu6050[1].offset= self.mpu6050[1].calculate_offset(5,5,5,5)
self.mpu6050[2].offset= self.mpu6050[2].calculate_offset(5,5,5,5)
self.mpu6050[3].offset= self.mpu6050[3].calculate_offset(5,5,5,5)
#self.mpu6050[4].offset= self.mpu6050[4].calculate_offset(5,25,25,25)
#self.mpu6050[5].offset= self.mpu6050[5].calculate_offset(5,25,25,25)
#self.mpu6050[6].offset= self.mpu6050[6].calculate_offset(5,25,25,25)
print("Test")
#!/usr/bin/python from mpu6050 import mpu6050 #from pylab import * import time import matplotlib.pyplot as plt # gyro settings gyro=mpu6050() gyro.setFilter(0) plt.ion() data = [] dataGyro = [] value = gyro.getAccelX() filteredData = [value] value = gyro.getGyroY() filteredDataGyro = [value] angle = value angleData = [] for i in range(18000000): value = gyro.getAccelX() * 90.0 accel = value data.append(value) filteredData.append(0.9 * filteredData[-1] + 0.1 * value) value = gyro.getGyroY() * -1.0 dataGyro.append(value) filteredDataGyro.append(0.9 * filteredDataGyro[-1] + 0.1 * value)
