From cafbc05664a323b5d771db2041c7cef051dc2e44 Mon Sep 17 00:00:00 2001 From: dong2 Date: Mon, 1 Mar 2021 18:59:35 +0800 Subject: [PATCH 1/7] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index baf132e..39436a8 100644 --- a/README.md +++ b/README.md @@ -170,6 +170,8 @@ Have fun! - [ ] A simple Pygame based game! - [ ] Scale down and go wireless +- [x] Add Class to read from a Serial MPU6050 device +- [x] Add Class to read from a TCP Mobile device From 4553916ac3f9dcfdcdda3ab4c5e706fb927b68fd Mon Sep 17 00:00:00 2001 From: dong2 Date: Mon, 1 Mar 2021 19:01:26 +0800 Subject: [PATCH 2/7] Create motiontracker_serial.py --- src/motiontracker_serial.py | 277 ++++++++++++++++++++++++++++++++++++ 1 file changed, 277 insertions(+) create mode 100644 src/motiontracker_serial.py diff --git a/src/motiontracker_serial.py b/src/motiontracker_serial.py new file mode 100644 index 0000000..4256172 --- /dev/null +++ b/src/motiontracker_serial.py @@ -0,0 +1,277 @@ +#coding:UTF-8 +"""Bluetooth motion tracker module. +Copyright 2017 Mark Mitterdorfer + +Class to read from a Bluetooth MPU6050 device. +Obtain acceleration, angular velocity, angle and temperature +""" +import threading +import serial + +############################################# +# Accelerometer +############################################# +ACCData=[0.0]*8 +GYROData=[0.0]*8 +AngleData=[0.0]*8 +FrameState = 0 #通过0x后面的值判断属于哪一种情况 +Bytenum = 0 #读取到这一段的第几位 +CheckSum = 0 #求和校验位 + +a = [0.0]*3 +w = [0.0]*3 +Angle = [0.0]*3 +sp = 0.0 + +def DueData(inputdata): #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里 + global FrameState #在局部修改全局变量，要进行global的定义 + global Bytenum + global CheckSum + global a + global w + global Angle + global sp + global l + for data in inputdata: #在输入的数据进行遍历 + data = ord(data) + if FrameState==0: #当未确定状态的时候，进入以下判断 + if data==0x55 and Bytenum==0: #0x55位于第一位时候，开始读取数据，增大bytenum + CheckSum=data + Bytenum=1 + continue + elif data==0x51 and Bytenum==1:#在byte不为0 且 识别到 0x51 的时候，改变frame + CheckSum+=data + FrameState=1 + Bytenum=2 + elif data==0x52 and Bytenum==1: #同理 + CheckSum+=data + FrameState=2 + Bytenum=2 + elif data==0x53 and Bytenum==1: + CheckSum+=data + FrameState=3 + Bytenum=2 + elif FrameState==1: # acc #已确定数据代表加速度 + + if Bytenum<10: # 读取8个数据 + ACCData[Bytenum-2]=data # 从0开始 + CheckSum+=data + Bytenum+=1 + else: + if data == (CheckSum&0xff): #假如校验位正确 + a = get_acc(ACCData) + CheckSum=0 #各数据归零，进行新的循环判断 + Bytenum=0 + FrameState=0 + elif FrameState==2: # gyro + + if Bytenum<10: + GYROData[Bytenum-2]=data + CheckSum+=data + Bytenum+=1 + else: + if data == (CheckSum&0xff): + w = get_gyro(GYROData) + CheckSum=0 + Bytenum=0 + FrameState=0 + elif FrameState==3: # angle + + if Bytenum<10: + AngleData[Bytenum-2]=data + CheckSum+=data + Bytenum+=1 + else: + if data == (CheckSum&0xff): + Angle = get_angle(AngleData) + + # d = a + w + Angle + # print("a(g):%10.6f %10.6f %10.6f w(deg/s):%10.6f %10.6f %10.6f Angle(deg):%10.6f %10.6f %10.6f" % d) + + #d = list(a)+list(w)+list(Angle) + #print("a(g):%10.6f %10.6f %10.6f w(deg/s):%10.6f %10.6f %10.6f Angle(deg):%10.6f %10.6f %10.6f" \ + # % (a[0],a[1],a[2],w[0],w[1],w[2],Angle[0],Angle[1],Angle[2])) + + #sp = math.sqrt(math.pow(a[0],2) + math.pow(a[1],2) + math.pow(a[2],2)) + #print("sp:%10.6f" % sp) + + CheckSum=0 + Bytenum=0 + FrameState=0 + + +def get_acc(datahex): + axl = datahex[0] + axh = datahex[1] + ayl = datahex[2] + ayh = datahex[3] + azl = datahex[4] + azh = datahex[5] + + k_acc = 16.0 + + acc_x = (axh << 8 | axl) / 32768.0 * k_acc + acc_y = (ayh << 8 | ayl) / 32768.0 * k_acc + acc_z = (azh << 8 | azl) / 32768.0 * k_acc + if acc_x >= k_acc: + acc_x -= 2 * k_acc + if acc_y >= k_acc: + acc_y -= 2 * k_acc + if acc_z >= k_acc: + acc_z-= 2 * k_acc + + return acc_x,acc_y,acc_z + + +def get_gyro(datahex): + wxl = datahex[0] + wxh = datahex[1] + wyl = datahex[2] + wyh = datahex[3] + wzl = datahex[4] + wzh = datahex[5] + k_gyro = 2000.0 + + gyro_x = (wxh << 8 | wxl) / 32768.0 * k_gyro + gyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyro + gyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyro + if gyro_x >= k_gyro: + gyro_x -= 2 * k_gyro + if gyro_y >= k_gyro: + gyro_y -= 2 * k_gyro + if gyro_z >=k_gyro: + gyro_z-= 2 * k_gyro + return gyro_x,gyro_y,gyro_z + + +def get_angle(datahex): + rxl = datahex[0] + rxh = datahex[1] + ryl = datahex[2] + ryh = datahex[3] + rzl = datahex[4] + rzh = datahex[5] + k_angle = 180.0 + + angle_x = (rxh << 8 | rxl) / 32768.0 * k_angle + angle_y = (ryh << 8 | ryl) / 32768.0 * k_angle + angle_z = (rzh << 8 | rzl) / 32768.0 * k_angle + if angle_x >= k_angle: + angle_x -= 2 * k_angle + if angle_y >= k_angle: + angle_y -= 2 * k_angle + if angle_z >=k_angle: + angle_z-= 2 * k_angle + + return angle_x,angle_y,angle_z + + +class MotionTracker(object): + """Class to track movement from MPU6050 Seila device. + """ + + def __init__(self): + """Initialization for tracker object. + + Args: + + Attributes: + sock (serial) : serial object + acc_x (float) : acceleration in X + acc_y (float) : acceleration in Y + acc_z (float) : acceleration in Z + angv_x (float) : angular velocity in X + angv_y (float) : angular velocity in Y + angv_z (float) : angular velocity in Z + ang_x (float) : angle degrees in X + ang_y (float) : angle degrees in Y + ang_z (float) : angle degrees in Z + temperature (float) : temperature in degrees celsius + __thread_read_device_data (threading.Thread) : Read input thread + """ + + self.sock = serial.Serial('/dev/ttyUSB0', '115200', timeout=0.5) + print(self.sock.is_open) + + self.acc_x = 0.0 + self.acc_y = 0.0 + self.acc_z = 0.0 + + self.angv_x = 0.0 + self.angv_y = 0.0 + self.angv_z = 0.0 + + self.ang_x = 0.0 + self.ang_y = 0.0 + self.ang_z = 0.0 + + self.temperature = 0.0 + self.__thread_read_device_data = None + + def start_read_data(self): + """Start reading from device. Wait for a second or two before + reading class attributes to allow values to 'settle' in. + Non blocking I/O performed via a private read thread. + """ + + self.__thread_read_device_data = threading.Thread(target=self.__read_device_data) + self.__thread_read_device_data.is_running = True + self.__thread_read_device_data.start() + + def stop_read_data(self): + """Stop reading from device. Join back to main thread and + close the socket. + """ + + self.__thread_read_device_data.is_running = False + self.__thread_read_device_data.join() + self.sock.close() + + def __read_device_data(self): + """Private method to read device data in 9 byte blocks. + """ + + while self.__thread_read_device_data.is_running: + + datahex = self.sock.read(33) + DueData(datahex) + + # Acceleration + + self.acc_x = a[0] + self.acc_y = a[1] + self.acc_z = a[2] + print("acc(g):%.6f %.6f %.6f" % (self.acc_x,self.acc_y,self.acc_z)) + + # Angular velocity + + self.angv_x = w[0] + self.angv_y = w[1] + self.angv_z = w[2] + print("angv(g):%.6f %.6f %.6f" % (self.angv_x,self.angv_y,self.angv_z)) + + # Angle + + self.ang_x = Angle[0] + self.ang_y = Angle[1] + self.ang_z = Angle[2] + print("ang(g):%.6f %.6f %.6f" % (self.ang_x,self.ang_y,self.ang_z)) + + +def main(): + """Test driver stub. + """ + + try: + session = MotionTracker() + session.start_read_data() + + while True: + print("ang_x:", session.ang_x, "ang_y:", session.ang_y, "ang_z:", session.ang_z) + + except KeyboardInterrupt: + session.stop_read_data() + + +if __name__ == "__main__": + main() From c54d0dc188e0d2937bd5e8c8dda079bd53d7d88b Mon Sep 17 00:00:00 2001 From: dong2 Date: Mon, 1 Mar 2021 19:02:36 +0800 Subject: [PATCH 3/7] Create motiontracker_tcp.py --- src/motiontracker_tcp.py | 149 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 149 insertions(+) create mode 100644 src/motiontracker_tcp.py diff --git a/src/motiontracker_tcp.py b/src/motiontracker_tcp.py new file mode 100644 index 0000000..f4954a8 --- /dev/null +++ b/src/motiontracker_tcp.py @@ -0,0 +1,149 @@ +"""Bluetooth motion tracker module. +Copyright 2017 Mark Mitterdorfer + +Class to read from a Bluetooth MPU6050 device. +Obtain acceleration, angular velocity, angle and temperature +""" + +import threading +import socket +import re + +class MotionTracker(object): + """Class to track movement from TCP Mobile device. + """ + + def __init__(self, bd_addr, port): + """Initialization for tracker object. + + Args: + bd_addr (str) : ip address + port (int, optional) : Port, defaults to 1 + Attributes: + bd_addr (str): ip address + port (int): Port + sock (bluetooth.bluez.BluetoothSocket) : Bluetooth socket object + acc_x (float) : acceleration in X + acc_y (float) : acceleration in Y + acc_z (float) : acceleration in Z + angv_x (float) : angular velocity in X + angv_y (float) : angular velocity in Y + angv_z (float) : angular velocity in Z + ang_x (float) : angle degrees in X + ang_y (float) : angle degrees in Y + ang_z (float) : angle degrees in Z + temperature (float) : temperature in degrees celsius + __thread_read_device_data (threading.Thread) : Read input thread + """ + self.bd_addr = bd_addr + self.port = port + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + print 'Socket created' + + #Bind socket to local host and port + try: + self.sock.bind((self.bd_addr, self.port)) + except socket.error as msg: + print 'Bind failed. Error Code : ' + str(msg[0]) + ' Message ' + msg[1] + sys.exit() + + print 'Socket bind complete' + + #Start listening on socket + self.sock.listen(10) + print 'Socket now listening' + + self.acc_x = 0.0 + self.acc_y = 0.0 + self.acc_z = 0.0 + + self.angv_x = 0.0 + self.angv_y = 0.0 + self.angv_z = 0.0 + + self.ang_x = 0.0 + self.ang_y = 0.0 + self.ang_z = 0.0 + + self.temperature = 0.0 + self.__thread_read_device_data = None + + def start_read_data(self): + """Start reading from device. Wait for a second or two before + reading class attributes to allow values to 'settle' in. + Non blocking I/O performed via a private read thread. + """ + + self.__thread_read_device_data = threading.Thread(target=self.__read_device_data) + self.__thread_read_device_data.is_running = True + self.__thread_read_device_data.start() + + def stop_read_data(self): + """Stop reading from device. Join back to main thread and + close the socket. + """ + + self.__thread_read_device_data.is_running = False + self.__thread_read_device_data.join() + self.sock.close() + + def __read_device_data(self): + """Private method to read device data in 9 byte blocks. + """ + #wait to accept a connection - blocking call + conn, addr = self.sock.accept() + print 'Connected with ' + addr[0] + ':' + str(addr[1]) + + while self.__thread_read_device_data.is_running: + while True: + + #Receiving from client + data = conn.recv(1024) + print data + # data = heard('$$$$') + type(0/1) + acc + gyro + g + mag + omt + ll = re.findall(r"\d+\.?\d*", data) + + self.acc_x = float('%.6f' % float(ll[2])) + self.acc_x = float('%.6f' % float(ll[3])) + self.acc_x = float('%.6f' % float(ll[4])) + print("acc(g):%.6f %.6f %.6f" % (self.acc_x,self.acc_y,self.acc_z)) + + self.angv_x = float('%.6f' % float(ll[5])) + self.angv_y = float('%.6f' % float(ll[6])) + self.angv_z = float('%.6f' % float(ll[7])) + print("angv(g):%.6f %.6f %.6f" % (self.angv_x,self.angv_y,self.angv_z)) + + self.ang_x = float('%.6f' % float(ll[8])) + self.ang_y = float('%.6f' % float(ll[9])) + self.ang_z = float('%.6f' % float(ll[10])) + print("ang(g):%.6f %.6f %.6f" % (self.ang_x,self.ang_y,self.ang_z)) + + reply = 'OK...' + data + if not data: + break + + #conn.sendall(reply) + + #came out of loop + conn.close() + + + +def main(): + """Test driver stub. + """ + + try: + session = MotionTracker(bd_addr="127.0.0.1", port=10000) + session.start_read_data() + + while True: + print("ang_x:", session.ang_x, "ang_y:", session.ang_y, "ang_z:", session.ang_z) + + except KeyboardInterrupt: + session.stop_read_data() + + +if __name__ == "__main__": + main() From b42be696004bf3f64d73a4f641626cbd4d3af1fe Mon Sep 17 00:00:00 2001 From: dong2 Date: Tue, 2 Mar 2021 11:46:47 +0800 Subject: [PATCH 4/7] Update motiontracker_tcp.py --- src/motiontracker_tcp.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/motiontracker_tcp.py b/src/motiontracker_tcp.py index f4954a8..f90e1a7 100644 --- a/src/motiontracker_tcp.py +++ b/src/motiontracker_tcp.py @@ -105,8 +105,8 @@ def __read_device_data(self): ll = re.findall(r"\d+\.?\d*", data) self.acc_x = float('%.6f' % float(ll[2])) - self.acc_x = float('%.6f' % float(ll[3])) - self.acc_x = float('%.6f' % float(ll[4])) + self.acc_y = float('%.6f' % float(ll[3])) + self.acc_z = float('%.6f' % float(ll[4])) print("acc(g):%.6f %.6f %.6f" % (self.acc_x,self.acc_y,self.acc_z)) self.angv_x = float('%.6f' % float(ll[5])) From 11baa754295c1859796c10774a5b988f0147eed3 Mon Sep 17 00:00:00 2001 From: dong2 Date: Tue, 2 Mar 2021 19:00:57 +0800 Subject: [PATCH 5/7] Update gl_cube.py --- examples/gl_cube.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/examples/gl_cube.py b/examples/gl_cube.py index e7d4aa7..9aba328 100644 --- a/examples/gl_cube.py +++ b/examples/gl_cube.py @@ -6,7 +6,9 @@ import sys sys.path.insert(0, "../src") -import motiontracker +#import motiontracker +#import motiontracker_serial +import motiontracker_tcp import pygame from pygame.locals import * @@ -163,7 +165,10 @@ def main(): glTranslated(0.0, 0.0, -5.0) # Start motion sensor - motion_session = motiontracker.MotionTracker(bd_addr="20:16:09:21:48:81") + #motion_session = motiontracker.MotionTracker(bd_addr="20:16:09:21:48:81") + #motion_session = motiontracker_serial.MotionTracker() + motion_session = motiontracker_tcp.MotionTracker(bd_addr='', port=10000) + motion_session.start_read_data() print("Calibrating, do not move Bluetooth module!") # Allow read values to "settle in" From f21877a105aacf3fbd989975d54fd926564df10d Mon Sep 17 00:00:00 2001 From: dong2 Date: Wed, 3 Mar 2021 10:06:07 +0800 Subject: [PATCH 6/7] Update motiontracker_tcp.py --- src/motiontracker_tcp.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/motiontracker_tcp.py b/src/motiontracker_tcp.py index f90e1a7..e99d683 100644 --- a/src/motiontracker_tcp.py +++ b/src/motiontracker_tcp.py @@ -53,6 +53,10 @@ def __init__(self, bd_addr, port): #Start listening on socket self.sock.listen(10) print 'Socket now listening' + + #wait to accept a connection - blocking call + self.conn, addr = self.sock.accept() + print 'Connected with ' + addr[0] + ':' + str(addr[1]) self.acc_x = 0.0 self.acc_y = 0.0 @@ -91,15 +95,11 @@ def stop_read_data(self): def __read_device_data(self): """Private method to read device data in 9 byte blocks. """ - #wait to accept a connection - blocking call - conn, addr = self.sock.accept() - print 'Connected with ' + addr[0] + ':' + str(addr[1]) - while self.__thread_read_device_data.is_running: while True: #Receiving from client - data = conn.recv(1024) + data = self.conn.recv(1024) print data # data = heard('$$$$') + type(0/1) + acc + gyro + g + mag + omt ll = re.findall(r"\d+\.?\d*", data) @@ -114,19 +114,19 @@ def __read_device_data(self): self.angv_z = float('%.6f' % float(ll[7])) print("angv(g):%.6f %.6f %.6f" % (self.angv_x,self.angv_y,self.angv_z)) - self.ang_x = float('%.6f' % float(ll[8])) - self.ang_y = float('%.6f' % float(ll[9])) - self.ang_z = float('%.6f' % float(ll[10])) + self.ang_x = float('%.6f' % float(ll[13])) + self.ang_y = float('%.6f' % float(ll[14])) + self.ang_z = float('%.6f' % float(ll[15])) print("ang(g):%.6f %.6f %.6f" % (self.ang_x,self.ang_y,self.ang_z)) reply = 'OK...' + data if not data: break - #conn.sendall(reply) + #self.conn.sendall(reply) #came out of loop - conn.close() + self.conn.close() From f211222d06bb1076d3b7bfaaf8d091c4d3d9a6ed Mon Sep 17 00:00:00 2001 From: dong2 Date: Wed, 3 Mar 2021 18:33:45 +0800 Subject: [PATCH 7/7] Update motiontracker_tcp.py --- src/motiontracker_tcp.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/motiontracker_tcp.py b/src/motiontracker_tcp.py index e99d683..9566153 100644 --- a/src/motiontracker_tcp.py +++ b/src/motiontracker_tcp.py @@ -104,14 +104,14 @@ def __read_device_data(self): # data = heard('$$$$') + type(0/1) + acc + gyro + g + mag + omt ll = re.findall(r"\d+\.?\d*", data) - self.acc_x = float('%.6f' % float(ll[2])) - self.acc_y = float('%.6f' % float(ll[3])) - self.acc_z = float('%.6f' % float(ll[4])) + self.acc_x = float('%.6f' % float(ll[1])) + self.acc_y = float('%.6f' % float(ll[2])) + self.acc_z = float('%.6f' % float(ll[3])) print("acc(g):%.6f %.6f %.6f" % (self.acc_x,self.acc_y,self.acc_z)) - self.angv_x = float('%.6f' % float(ll[5])) - self.angv_y = float('%.6f' % float(ll[6])) - self.angv_z = float('%.6f' % float(ll[7])) + self.angv_x = float('%.6f' % float(ll[4])) + self.angv_y = float('%.6f' % float(ll[5])) + self.angv_z = float('%.6f' % float(ll[6])) print("angv(g):%.6f %.6f %.6f" % (self.angv_x,self.angv_y,self.angv_z)) self.ang_x = float('%.6f' % float(ll[13]))