def __init__(self, base_addr=0x68, addr=0x69): print 'Orientation Instance Initializing...' self.__base_mpu = mpu6050.MPU6050(base_addr) self.__mpu = mpu6050.MPU6050(addr) self.__base_mpu.dmpInitialize() self.__mpu.dmpInitialize() self.__base_mpu.setDMPEnabled(True) self.__mpu.setDMPEnabled(True) self.__packet_size = [0, 0] self.__packet_size[0] = self.__base_mpu.dmpGetFIFOPacketSize() self.__packet_size[1] = self.__mpu.dmpGetFIFOPacketSize() base_update_thread = Thread(target=self.__update_dmp, args=(self.__base_mpu, self.__packet_size[0], 0)) update_thread = Thread(target=self.__update_dmp, args=(self.__mpu, self.__packet_size[1], 1)) print 'Orientation Instance Initialized...' update_thread.start() self.__thread_status[1] = True base_update_thread.start() self.__thread_status[0] = True
def setup(self): # Sensor initialization self.mpu = mpu6050.MPU6050(0x69) self.mpu.dmpInitialize() self.mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison self.packetSize = self.mpu.dmpGetFIFOPacketSize() return True
def __init__(self, SSN, DRDY): threading.Thread.__init__(self) self.SSN = SSN self.DRDY = DRDY self.rm3100 = RM3100(self.SSN, self.DRDY) self.Readings = None # Sensor initialization self.mpu = mpu6050.MPU6050() self.mpu.dmpInitialize() self.mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison self.packetSize = self.mpu.dmpGetFIFOPacketSize()
def __init__(self): self.dataYaw = None self.dataPitch = None self.dataRoll = None self.mpu = None self.packetSize = None self.mpu = mpu6050.MPU6050() self.mpu.dmpInitialize() self.mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison self.packetSize = self.mpu.dmpGetFIFOPacketSize()
def run(self): mpu = mpu6050.MPU6050() mpu.dmpInitialize() mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize() while True: # Get INT_STATUS byte mpuIntStatus = mpu.getIntStatus() if mpuIntStatus >= 2: # check for DMP data ready interrupt (this should happen frequently) # get current FIFO count fifoCount = mpu.getFIFOCount() # check for overflow (this should never happen unless our code is too inefficient) if fifoCount == 1024: # reset so we can continue cleanly mpu.resetFIFO() print('FIFO overflow!') # wait for correct available data length, should be a VERY short wait fifoCount = mpu.getFIFOCount() while fifoCount < packetSize: fifoCount = mpu.getFIFOCount() result = mpu.getFIFOBytes(packetSize) q = mpu.dmpGetQuaternion(result) g = mpu.dmpGetGravity(q) self.ypr = mpu.dmpGetYawPitchRoll(q, g) self.ypr['yaw'] = self.ypr['yaw'] * 180 / math.pi self.ypr['pitch'] = self.ypr['pitch'] * 180 / math.pi self.ypr['roll'] = self.ypr['roll'] * 180 / math.pi #print(ypr['yaw'] * 180 / math.pi), #print(ypr['pitch'] * 180 / math.pi), #print(ypr['roll'] * 180 / math.pi) # track FIFO count here in case there is > 1 packet available # (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize #print "ypr = ", ypr if self.checkStop() == False: print "Thread STOP" break
def __init__(self, i2cdev=0, cb_update=None, cb_error=None, cb_info=None, cb_debug=None): #turn on power with gpio self.power = pin(pin.port.PI0) self.power.on() self.cb_update = cb_update self.cb_error = cb_error self.cb_info = cb_info self.cb_debug = cb_debug self.calibrating = True self.calibration_time = -1. self.start_time = time.time() self.yaw0 = 0 self.pitch0 = 0 self.roll0 = 0 self.ax0 = 0 self.ay0 = 0 self.az0 = 0 self.precision = 100 self.samplerate = 9 #1khz / (1 + 4) = 200 Hz [9 = 100 Hz] self.cycleActive = True mpu = mpu6050.MPU6050(address=mpu6050.MPU6050.MPU6050_DEFAULT_ADDRESS, bus=smbus.SMBus(i2cdev)) mpu.dmpInitialize() mpu.setDMPEnabled(True) self.mpu = mpu # get expected DMP packet size for later comparison self.packetSize = mpu.dmpGetFIFOPacketSize() self.readThread = threading.Thread(target=self.readcycle) self.readThread.setDaemon(1) self.readThread.start() self.info("Turned on. Calibrating.")
def main(argv): try: opts,args=getopt.getopt(argv,"h:n:s:m:",["help=","deviceNumber"]) except getopt.GetoptError: print 'usage:muti_accelerometer.py -n <deviceNumber> -s <subjectName> -m <mode>' sys.exit(2) if findElement(argv,'-n')==0: print 'usage:muti_accelerometer.py -n <deviceNumber> -s <subjectName> -m <mode>' sys.exit(2) if findElement(argv,'-s')==0: print 'usage:muti_accelerometer.py -n <deviceNumber> -s <subjectName> -m <mode>' sys.exit(2) if findElement(argv,'-m')==0: print 'usage:muti_accelerometer.py -n <deviceNumber> -s <subjectName> -m <mode>' sys.exit(2) for opt,arg in opts: if opts=='-h': print 'usage:muti_accelerometer.py -n <deviceNumber> -s <subjectName> -m <mode>' elif opt in ("-n","--deviceNumber"): deviceNum=arg elif opt in ("-s","--subjectName"): subName=arg elif opt in ("-m","--mode"): tmpm=arg mode=int(tmpm) accel=[[] for i in range(int(deviceNum))] #create dynamic list gyro=[[] for i in range(int(deviceNum))] fileName=[subName+'_sensor1.txt',subName+'_sensor2.txt',subName+'_sensor3.txt',subName+'_sensor4.txt',subName+'_sensor5.txt',subName+'_sensor6.txt',subName+'_sensor7.txt',subName+'_sensor8.txt',subName+'_sensor9.txt', subName+'_sensor10.txt',subName+'_sensor11.txt',subName+'_sensor12.txt',subName+'_sensor13.txt'] print "" print "Sample uses 0x70" print "Program Started at:"+ time.strftime("%Y-%m-%d %H:%M:%S") print "" #mode=raw_input("Please enter the mode you want") starttime = datetime.datetime.utcnow() startflag=0 tca9548 = TCA9548_Set.TCA9548_Set(addr=TCA9548_ADDRESS, bus_enable = TCA9548_CONFIG_BUS1) file0=open(fileName[0],'w') file1=open(fileName[1],'w') file2=open(fileName[2],'w') file3=open(fileName[3],'w') file4=open(fileName[4],'w') file5=open(fileName[5],'w') fileList=[file0,file1,file2,file3,file4,file5] # rotates through all 4 I2C buses and prints out what is available on each accel_tmp=[0]*int(deviceNum) count=0 flag=0 addr = ('192.168.43.221',8000) bufsize = 1024 filename = 'sensor1.txt' if mode==1: sendsock = socket(AF_INET,SOCK_STREAM) sendsock.bind(addr) sendsock.listen(5) print "waiting for client connect" conn,addr = sendsock.accept() print "server already connect client...->",addr while True: fflag=0 fileIndex=0 input_state=GPIO.input(4) #get switch state if flag==0: print "System initialize........" flag+=1 for channel in BusChannel: # print channel if startflag==0 and count>1: print "start getting data press button to stop" startflag=1 start=time.time() mpu6050 = mpu6050.MPU6050(0x68) tca9548.write_control_register(BusChannel[fileIndex]) mpu6050.dmpInitialize() mpu6050.setDMPEnable(True) mpuIntStatus=mpu6050.getIntStatus() packetSize=mpu6050.dmpGetFIFOPacketSize() if mpuIntStatus>=2: fifoCount=mpu6050.getFIFOCount() if fifoCount==1024: mpu.resetFIFO() fifoCount=mpu6050.getFIFOCount() while fifoCount<packetSize: fifoCount=mpu6050.getFIFOCount() result=mpu6050.getFIFOBytes(packetSize) #get quaternion q=mpu.dmpGetQuaternion(result) x="{0:.6f}".format(q['x']) y="{0:.6f}".format(q['y']) z="{0:.6f}".format(q['z']) w="{0:.6f}".format(q['w']) fifoCount-=packetSize #print "-----------------BUS"+str(fileIndex)+"-------------" #get gyro and accelerometer value gyro_xout = mpu6050.read_word_2c(0x43) gyro_yout = mpu6050.read_word_2c(0x45) gyro_zout = mpu6050.read_word_2c(0x47) accel_xout = mpu6050.read_word_2c(0x3b) accel_yout = mpu6050.read_word_2c(0x3d) accel_zout = mpu6050.read_word_2c(0x3f) accel_xout=accel_xout/16384.0 accel_yout=accel_yout/16384.0 accel_zout=accel_zout/16384.0 gyro_xout=gyro_xout/131.0 gyro_yout=gyro_yout/131.0 gyro_zout=gyro_zout/131.0 if mode==0 and count>1: end=time.time() realtime=end-start if realtime<0: realtime=0 fileList[fileIndex].write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" %(accel_xout,accel_yout,accel_zout,gyro_xout,gyro_yout,gyro_zout,x,y,z,w,realtime)) elif mode==1: val=math.sqrt(math.pow(int(accel_xout),2)+math.pow(int(accel_yout),2)+math.pow(int(accel_zout),2)) val=int(val) val=round(val,4) conn.send("%5s"%(str(val))) #conn.send("%2s\t%5s\t%5s\t%5s"%(str(fileIndex),str(accel_xout),str(accel_yout),str(accel_zout))) #print "accelx = %f accely = %f accelz = %f\n" %(accel_xout,accel_yout,accel_zout) ''' if fileIndex==0 or fileIndex==1 or fileIndex==2 or fileIndex==3 or fileIndex==7: mpu6050_sla=MPU6050Read.MPU6050Read(0x69,1) gyro_xout = mpu6050_sla.read_word_2c(0x43) gyro_yout = mpu6050_sla.read_word_2c(0x45) gyro_zout = mpu6050_sla.read_word_2c(0x47) accel_xout = mpu6050_sla.read_word_2c(0x3b) accel_yout = mpu6050_sla.read_word_2c(0x3d) accel_zout = mpu6050_sla.read_word_2c(0x3f) accel_xout=accel_xout/16384.0 accel_yout=accel_yout/16384.0 accel_zout=accel_zout/16384.0 gyro_xout=gyro_xout/131.0 gyro_yout=gyro_yout/131.0 gyro_zout=gyro_zout/131.0 if mode==0 and fileIndex==7: end=time.time() realtime=end-start fileList[fileIndex+5].write("%f\t%f\t%f\t%f\t%f\t%f\t%f\n" %(accel_xout,accel_yout,accel_zout,gyro_xout,gyro_yout,gyro_zout,realtime)) elif mode==0: end=time.time() realtime=end-start fileList[fileIndex+8].write("%f\t%f\t%f\t%f\t%f\t%f\t%f\n" %(accel_xout,accel_yout,accel_zout,gyro_xout,gyro_yout,gyro_zout,realtime)) if mode==1: val=math.sqrt(math.pow(int(accel_xout),2)+math.pow(int(accel_yout),2)+math.pow(int(accel_zout),2)) val=int(val) val=round(val,4) conn.send("%5s"%(str(val))) ''' #timeTmp=datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S") #timeFile.write("%s\n" %(timeTmp)) #timeArray[count]=timeTmp fileIndex+=1 ''' if fileIndex>int(deviceNum): fileIndex=0 break ''' ''' for i in range(int(deviceNum)): conn.send("%5s" %(accel_tmp[i])) ''' count+=1 '''
import time import math import mpu6050 import RPi.GPIO as GPIO # Sensor initialization mpu = mpu6050.MPU6050() mpu.dmpInitialize() mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize() gap = 5 step = 0.1 pin_left_up = 31 pin_right_up = 33 pin_left_down = 35 pin_right_down = 37 GPIO.setmode(GPIO.BOARD) GPIO.setup(pin_left_up, GPIO.OUT, initial=False) GPIO.setup(pin_right_up, GPIO.OUT, initial=False) GPIO.setup(pin_left_down, GPIO.OUT, initial=False) GPIO.setup(pin_right_down, GPIO.OUT, initial=False) motor_left_up = GPIO.PWM(pin_left_up, 50) motor_right_up = GPIO.PWM(pin_right_up, 50) motor_left_down = GPIO.PWM(pin_left_down, 50) motor_right_down = GPIO.PWM(pin_right_down, 50) dutycycle_left_up = 100.0 dutycycle_right_up = 100.0 dutycycle_left_down = 100.0
from machine import I2C, Pin import ssd1306 import mpu6050 import time # Declaration I2C # // D1 -> GPIO05 --> SDA # // D2 -> GPIO04 --> SCL i2c = I2C(scl=Pin(4), sda=Pin(5), freq=400000) # Declaration OLED SSD1306 oled = ssd1306.SSD1306_I2C(128, 64, i2c, 0x3c) # 128 x 64 pixels oled.fill(0) # efface l'ecran oled.text("Calibr. MPU6050", 0, 0) oled.show() mpu_6050 = mpu6050.MPU6050(i2c) # declaration mpu6050 oled.text("Adresse :", 0, 10) oled.text(hex(mpu_6050.adresse), 80, 10) mpu_6050.lecture_capteurs() oled.text("Temp :", 0, 20) oled.text("%2.1f" % mpu_6050.temperature, 80, 20) oled.show() time.sleep(10) if (mpu_6050.detect()): while True: oled.fill(0) # efface l'ecran mpu_6050.lecture_capteurs() oled.text("Angle X:", 0, 0) oled.text("%i" % mpu_6050.AngleX, 80, 0) oled.text("Angle Y:", 0, 10) oled.text("%i" % mpu_6050.AngleY, 80, 10)
# coding=utf-8 import math import smbus import mpu6050 import time # Sensor initialization mpu = mpu6050.MPU6050(address=mpu6050.MPU6050.MPU6050_DEFAULT_ADDRESS, bus=smbus.SMBus(0)) mpu.dmpInitialize() mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize() calibrating = True t0 = time.time() yaw0 = 0 pitch0 = 0 roll0 = 0 ax0 = 0 ay0 = 0 az0 = 0 precision = 100 def ftoip(v): return int(precision * v) def equal(l1, l2):
sensors = [2, 3, 4, 5] # acc x, y, z, gyro x, y, z offsets = { 2: [2563, 2128, 1306, 85, -18, 22], 3: [-1523, -276, 1773, 60, -13, -3], 4: [-3131, 1430, 1332, 72, 10, 26], 5: [-1944, -823, 1969, 121, -11, 20] } mpus = {} # initialize for i in sensors: mpus[i] = mpu6050.MPU6050() mpus[i].dmpInitialize() mpus[i].setDMPEnabled(True) if i in offsets: mpus[i].setXAccelOffset(offsets[i][0]) mpus[i].setYAccelOffset(offsets[i][1]) mpus[i].setZAccelOffset(offsets[i][2]) mpus[i].setXGyroOffset(offsets[i][3]) mpus[i].setYGyroOffset(offsets[i][4]) mpus[i].setZGyroOffset(offsets[i][5]) else: print('Warning: no offsets for IMU %d' % i) packetSize = mpus[i].dmpGetFIFOPacketSize()
val = (high << 8) + low return val def read_word_2c(adr): val = read_word(adr) if (val >= 0x8000): return -((65535 - val) + 1) else: return val ################################# # Sensor initialization mpu = mpu6050.MPU6050() # Instance 생성 mpu.dmpInitialize() # DMP 초기화 mpu.setDMPEnabled(True) # DMP 구동 # get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize() # 일반적으로, 42byte이다. count = 0 # Display할 시점 확인용 past = time.time() # 수행 시간 확인 # 현재 설정 확인(dmpInitialize()에서 설정됨) sample_rate = mpu.getRate() # sample rate=1K/(sample_rate+1), # 단 LPF를 사용하지 않는 경우에는 8K/(sample_rate+1) # 즉, 4 -> 200Hz, 9 -> 100Hz, 19 -> 50Hz dlp_mode = mpu.getDLPFMode() # LPF 모드 gyro_fs = mpu.getFullScaleGyroRange() # Gyro의 Full Scale
#!/usr/bin/env python from rospy import Publisher, init_node, Rate, is_shutdown, ROSInterruptException, get_rostime, Time from std_msgs.msg import Float64 from blimp_control.msg import Float64WithHeader import RPi.GPIO as GPIO import time import mpu6050 from sensor_msgs.msg import Imu import math from tf.transformations import quaternion_from_euler imu_t = mpu6050.MPU6050() imu_data = Imu() def imu(): #define publisher pub0 = Publisher('imu_data', Imu, queue_size=10) pub1 = Publisher('yaw_rate', Float64WithHeader, queue_size=10) pub2 = Publisher('yaw_rate_control', Float64, queue_size=10) init_node('imu', anonymous=True) rate = Rate(10) # 10hz print('Running IMU node.') while not is_shutdown(): # # Read IMU data. data = imu_t.get_data() imu_data.header.stamp = Time.now() imu_data.header.stamp = get_rostime() # # Get quaternion. roll = 180 * math.atan(data[3] / math.sqrt(data[4]**2 + data[5]**2)) / math.pi
from time import sleep from machine import ADC, Pin import mpu6050 wrist = mpu6050.MPU6050(22, 23) shoulder = mpu6050.MPU6050(18, 19) Thumb = ADC(Pin(39)) Index = ADC(Pin(34)) Middle = ADC(Pin(35)) Ring = ADC(Pin(32)) Little = ADC(Pin(33)) Fingers = [0, 0, 0, 0, 0] def setVal(x, val): Fingers[x] = val while True: setVal(0, Thumb.read()) setVal(1, Index.read()) setVal(2, Middle.read()) setVal(3, Ring.read()) setVal(4, Little.read()) print(Fingers + wrist.data() + shoulder.data(), end='') print(",") sleep(.25) pass
if __name__ == "__main__": if len(sys.argv) == 1: print('usage: python tcpClient.py port_number') exit(1) elif not isNumber(sys.argv[1]): print('Invalid port_number, ', sys.argv[1]) exit(2) ID = 'AAA' #connect HOST = '192.168.0.200' PORT = int(sys.argv[1]) print('Client :: Conncet to ', HOST, PORT) s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((HOST, PORT)) print('connected') msg = ID + 'attached' s.send(msg.encode(encoding='utf_8', errors='strict')) #sensor_setting sensor1 = mpu6050.MPU6050(smbus.SMBus(1), 0x68, mpu6050.MPU6050.FS_500, mpu6050.MPU6050.AFS_4g) #bus/addr sensor1.setting(100) #data = s.recv(1024) #print ('Client :: recv : ', data.decode()) while True: msg = sensor1.exam() if msg == 'Alert!!!!!!': msg = ID + msg print(msg) s.send(msg.encode(encoding='utf_8', errors='strict')) time.sleep(0.3)
#Main to test module mpu6050 import streams import mpu6050 streams.serial() try: myMpu = mpu6050.MPU6050( ) #MPU Connected using I2C0 SDA on D25 n SCL on D26, alim 3.3V print("Ready!") except Exception as e: print("Error: ", e) try: while True: # temp=myMpu.getTemp() # print('Temperature: ', temp) x = myMpu.getGyroX() y = myMpu.getGyroY() z = myMpu.getGyroZ() print("'Gyro data:", x, y, z) sleep(2000) except Exception as e: print(e)
def initialise_gyro_new(self, x, y): mpu = mpu6050.MPU6050() mpu.dmpInitialize() mpu.setDMPEnabled(True) # get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize() i = 0 # iteration count sum_yaw = 0 # summing totals to calculate the mean current_yaw = 0 print('\n'"Calculating zero-offset.") print("Please Stand By...") while True: fifoCount = mpu.getFIFOCount() # while fifoCount < packetSize: # major oscillations without this fifoCount = mpu.getFIFOCount() # result = mpu.getFIFOBytes(packetSize) q = mpu.dmpGetQuaternion(result) g = mpu.dmpGetGravity(q) ypr = mpu.dmpGetYawPitchRoll(q, g) yaw = (ypr['yaw'] * 180 / math.pi) if i < 750: i+=1 if i <= 600: # skip the first 600 iterations to give it a chance to settle/stop drifting pass if i > 600 and i <= 700: # take the average of the next 100 iterations after that (where it has settled to when stationary) sum_yaw += yaw if i == 700: avg_yaw = (sum_yaw / 100) # avg_yaw = the zero error if avg_yaw > 0: print('\n'"ALERT !!!") print("Average yaw is positive.") print("If not working check this first !") else: pass if i > 700: current_yaw = yaw - avg_yaw # === [ robot can only start moving at this point ] === # if avg_yaw < 0: # New code starts here. if current_yaw > 180: current_yaw = current_yaw - 180 current_yaw = -180 + current_yaw else: pass elif avg_yaw > 0: if current_yaw < -180: current_yaw = current_yaw + 180 current_yaw = 180 - current_yaw else: pass self.gyro_data = current_yaw fifoCount -= packetSize mpu.resetFIFO()
import ujson usb = pyb.USB_VCP() def send_data(data): global usb print(data) toto = ujson.dumps(data) usb.write(toto + "\n") a = 1 while a: try: # Set pin: scl on Y9 and sda on Y10 i2c = I2C(scl='Y9', sda='Y10', freq=400000) mpu_6050 = mpu6050.MPU6050(i2c) a = 0 except: print("init bad") if mpu_6050.detect(): data = None while True: try: # get sensors values mpu_6050.lecture_capteurs() # Details gX = mpu_6050.gyroX gY = mpu_6050.gyroY gZ = mpu_6050.gyroZ