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
