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 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" 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() diff --git a/src/motiontracker_tcp.py b/src/motiontracker_tcp.py new file mode 100644 index 0000000..9566153 --- /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' + + #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 + 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: + while True: + + #Receiving from client + data = self.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[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[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])) + 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 + + #self.conn.sendall(reply) + + #came out of loop + self.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()