quaternion_BMX055.py

00001 """
00002 Created on      Feb. 7th, 2020
00003 @author: Kenji Arai
00004 
00005 Refrence
00006     http://kieranwynn.github.io/pyquaternion/
00007 
00008     https://kumadasu.com/series/quaternion-for-embedded/
00009 
00010     https://wgld.org/d/webgl/w031.html
00011     http://marupeke296.com/DXG_No10_Quaternion.html
00012     http://edom18.hateblo.jp/entry/2018/06/25/084023
00013 """
00014 
00015 from pyquaternion import Quaternion
00016 from time import sleep
00017 import serial
00018 import serial.tools.list_ports
00019 import tkinter as tk
00020 from tkinter import *
00021 from tkinter import messagebox
00022 import numpy as np
00023 from matplotlib import pyplot as plt
00024 from mpl_toolkits.mplot3d import Axes3D
00025 from enum import Enum
00026 
00027 from check_ser_port import connect_dtlggr
00028 
00029 D_LBL_MODE_IMU = 'Operating Mode:   Quaternion '
00030 D_BTN_MODE_IMU = ' IMU / Quarternion mode '
00031 D_LBL_MODE_ACC = 'Operating Mode:   Accelerometer '
00032 D_BTN_MODE_ACC = ' ACC / Accelerometer mode '
00033 D_SPACE = '         '
00034 D_LETTER = 'UTF-8'
00035 
00036 class mode(Enum):
00037     ACC = 1
00038     IMU = 2
00039     ALL = 3
00040     RST = 4
00041 
00042 class op_mode:
00043     opeartion_mode = mode.IMU
00044 
00045     def set_mode(ser,mode):
00046         if mode == mode.IMU:
00047             op_mode.opeartion_mode = mode.IMU
00048             ser.write(bytes('2',D_LETTER))
00049             sleep(0.02)
00050             ser.write(bytes('2',D_LETTER))
00051         elif mode == mode.ACC:
00052             op_mode.opeartion_mode = mode.ACC
00053             ser.write(bytes('1',D_LETTER))
00054             sleep(0.02)
00055             ser.write(bytes('1',D_LETTER))
00056         else:
00057             ser.write(bytes('R',D_LETTER))
00058             sleep(0.02)
00059             ser.write(bytes('R',D_LETTER))
00060 
00061     def get_mode():
00062         return op_mode.opeartion_mode
00063 
00064 def chart_initialize(root):
00065     root.title("BMX055 Quarternion")
00066     root.resizable(False, False)
00067     frame0 = tk.Frame(root)
00068     frame1 = tk.Frame(root)
00069     text_widget0 = tk.Text(frame0,height=1,width=35,fg='darkblue',bg='gray94',
00070                            relief='flat',font=('Arial', '12'))
00071     text_widget0.grid(row=0,column=0)
00072 
00073     def acc_action():
00074         op_mode.set_mode(ser,mode.ACC)
00075         text_widget0.delete('1.0','end')
00076         text_widget0.insert('end',D_SPACE)
00077         text_widget0.insert('end',D_LBL_MODE_ACC)
00078 
00079     def imu_action():
00080         op_mode.set_mode(ser,mode.IMU)
00081         text_widget0.delete('1.0','end')
00082         text_widget0.insert('end',D_SPACE)
00083         text_widget0.insert('end',D_LBL_MODE_IMU)
00084 
00085     def reset_action():
00086         op_mode.set_mode(ser,mode.RST)
00087         text_widget0.delete('1.0','end')
00088         text_widget0.insert('end',D_SPACE)
00089         text_widget0.insert('end',D_LBL_MODE_IMU)
00090 
00091     def quit_action():
00092         root.destroy()
00093 
00094     button0 = tk.Button(frame1,text=D_BTN_MODE_ACC,command=acc_action)
00095     button0.pack(anchor=tk.S,fill=tk.BOTH)
00096     button1 = tk.Button(frame1,text=D_BTN_MODE_IMU,command=imu_action)
00097     button1.pack(anchor=tk.S,fill=tk.BOTH)
00098     button2 = tk.Button(frame1,text='RESET',command=reset_action)
00099     button2.pack(anchor=tk.S,fill=tk.BOTH)
00100     button3 = tk.Button(frame1,text='EXIT',command=quit_action)
00101     button3.pack(anchor=tk.S,fill=tk.BOTH)
00102     frame0.pack(fill='both')
00103     frame1.pack(fill='both')
00104 
00105     text_widget0.delete('1.0','end')
00106     text_widget0.insert('end',D_SPACE)
00107     text_widget0.insert('end',D_LBL_MODE_IMU)
00108     
00109 
00110 def graph_initialize():
00111     global fig, lines, startpoints, endpoints
00112 
00113     fig = plt.figure(figsize=(8,6))
00114     ax = fig.add_axes([0, 0, 1, 1], projection='3d')
00115     ax.set_xlabel('X(Red)',size=14, color='red')
00116     ax.set_ylabel('Y(Green)',size=14, color='green')
00117     ax.set_zlabel('Z(Blue)',size=14, color='blue')
00118     colors = ['red', 'green', 'blue']
00119     lines = sum([ax.plot([], [], linewidth = 5, c=c) for c in colors], [])
00120     startpoints = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
00121     endpoints = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
00122     ax.set_xticks([-1.0, -0.5, 0.0, 0.5, 1.0])
00123     ax.set_yticks([-1.0, -0.5, 0.0, 0.5, 1.0])
00124     ax.set_zticks([-1.0, -0.5, 0.0, 0.5, 1.0])
00125     ax.set_xlim((-1, 1))
00126     ax.set_ylim((-1, 1))
00127     ax.set_zlim((-1, 1))
00128     ax.quiver(-1,0,0,1,0,0,length=2,arrow_length_ratio=0,
00129               linestyles='dashed',color='red')
00130     ax.quiver(0,-1,0,0,1,0,length=2,arrow_length_ratio=0,
00131               linestyles='dashed',color='green')
00132     ax.quiver(0,0,-1,0,0,1,length=2,arrow_length_ratio=0,
00133               linestyles='dashed',color='blue')
00134     ax.view_init(30, 10)
00135 
00136 def graph_update(serData,mode):
00137     if  mode == mode.ACC:
00138         n = 0
00139         for line, start, end in zip(lines, startpoints, endpoints):
00140             start = [0.,0.,0.]
00141             end =[0., 0., 0.]
00142             end[n] = float(serData[n])
00143             n += 1
00144             line.set_data([start[0], end[0]], [start[1], end[1]])
00145             line.set_3d_properties([start[2], end[2]])
00146     elif mode == mode.IMU:
00147         q = Quaternion(serData)
00148         for line, start, end in zip(lines, startpoints, endpoints):
00149             start = q.rotate(start)
00150             end = q.rotate(end)
00151             line.set_data([start[0], end[0]], [start[1], end[1]])
00152             line.set_3d_properties([start[2], end[2]])
00153 
00154     fig.canvas.draw()
00155 
00156 def show_BMX055_serial():
00157 
00158     def every_second():
00159         serData = ser.readline().strip().split(b',')
00160         dt = len(serData)
00161         #print(serData)
00162         mode = op_mode.get_mode()
00163         if  mode == mode.ACC and dt != 3:
00164             op_mode.set_mode(ser,mode.ACC)
00165             print('ERROR @ read ser read')
00166         elif  mode == mode.IMU and dt != 4:
00167             op_mode.set_mode(ser,mode.IMU)
00168             print('ERROR @ read ser read')
00169         elif mode == mode.ACC or mode == mode.IMU:
00170             graph_update(serData,mode)
00171         else:
00172             print(serData)
00173 
00174         root.after(1, every_second)
00175 
00176     root = tk.Tk()
00177     com_port=connect_dtlggr()
00178     if com_port == None:
00179         print("Serial port is not found!")
00180         root.withdraw()
00181         ready = messagebox.showwarning('showwaning', 'USB is not ready!!')
00182         print(ready)
00183         root.destroy()
00184         return
00185 
00186     global ser
00187     print('checking serial')
00188     #ser = serial.Serial(com_port,921600,timeout=None)
00189     ser = serial.Serial(com_port,115200,timeout=None)
00190     op_mode.set_mode(ser,mode.IMU)
00191     chart_initialize(root)
00192     graph_initialize()
00193     every_second()
00194     root.mainloop()
00195 
00196     print('Push EXIT button')
00197     ser.close()
00198     plt.clf()
00199     plt.close()
00200 
00201 if __name__ == '__main__':
00202     show_BMX055_serial()
00203     print("Exit")