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)