IMU | Ep.1: Preparing an experiment to test linear positions (ft. MPU6050, GY-BNO055)

Eric · Eric · commit cb1424a9ab59 · 2021-07-07T14:38:55.000-07:00
diff --git a/ESP32_GY-BNO055/ESP32_GY-BNO055.ino b/ESP32_GY-BNO055/ESP32_GY-BNO055.ino
@@ -0,0 +1,72 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <TFT_eSPI.h>
+#define I2C_SDA 21
+#define I2C_SCL 22
+
+TwoWire I2CBNO = TwoWire(0);
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x29, &I2CBNO);
+TFT_eSPI tft = TFT_eSPI();
+
+void setup(){
+  Serial.begin(115200);
+  displayInit();
+
+  I2CBNO.begin(I2C_SDA, I2C_SCL);
+
+  if (!bno.begin()){
+    printString("No BNO055 detected", TFT_BLACK, TFT_YELLOW);
+    while (1){
+      delay(10);
+    }
+  }
+
+  delay(100);   
+
+  uint8_t system, gyro, accel, mag = 0;
+  while(system != 3) {
+    bno.getCalibration(&system, &gyro, &accel, &mag);
+    printString("Calibration!!!", TFT_WHITE, TFT_RED);
+    delay(100); 
+    }
+
+  printString("READING...", TFT_BLACK, TFT_DARKGREEN);
+  delay(100);
+}
+
+void loop(){
+
+  delay(8); // To make sampling rate around 100hz
+
+  imu::Vector<3> acc = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); 
+  imu::Vector<3> gyr = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);  
+
+  Serial.print(gyr.x(),4); 
+  Serial.print(", "); 
+  Serial.print(gyr.y(),4);
+  Serial.print(", "); 
+  Serial.print(gyr.z(),4);
+  Serial.print(", "); 
+  Serial.print(acc.x(), 4); 
+  Serial.print(", "); 
+  Serial.print(acc.y(),4); 
+  Serial.print(", "); 
+  Serial.println(acc.z(),4);
+}
+
+void displayInit(){
+  tft.begin();
+  tft.setRotation(1);
+  tft.setTextColor(TFT_WHITE,TFT_BLACK); 
+  tft.fillScreen(TFT_BLACK);
+  tft.setSwapBytes(true);
+  tft.setTextFont(4);
+  tft.setTextDatum(MC_DATUM);
+}
+
+void printString(String text, uint16_t textColor, uint16_t bgColor){
+  tft.fillScreen(bgColor);
+  tft.setTextColor(textColor, bgColor);
+  tft.drawCentreString(text, tft.width()/2, tft.height()/2, 4);
+}
diff --git a/ESP32_MPU6050/ESP32_MPU6050.ino b/ESP32_MPU6050/ESP32_MPU6050.ino
@@ -0,0 +1,69 @@
+#include <Adafruit_MPU6050.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+#include <TFT_eSPI.h>
+#define I2C_SDA 21
+#define I2C_SCL 22
+
+TwoWire I2CMPU = TwoWire(0);
+Adafruit_MPU6050 mpu;
+TFT_eSPI tft = TFT_eSPI();
+
+void setup(){
+  Serial.begin(115200);
+  displayInit();
+
+  I2CMPU.begin(I2C_SDA, I2C_SCL);
+
+  if (!mpu.begin(104, &I2CMPU, 0x68)) {
+    printString("No MPU6050 detected", TFT_BLACK, TFT_YELLOW);
+    while (1) {
+      delay(10);
+    }
+  }
+
+  delay(100);   
+
+  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
+  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
+
+  printString("READING...", TFT_BLACK, TFT_DARKGREEN);
+  delay(100);
+}
+
+void loop(){
+
+  delay(7); // To make sampling rate around 100hz
+
+  sensors_event_t a, g, temp;
+  mpu.getEvent(&a, &g, &temp);
+
+  Serial.print(g.gyro.x);
+  Serial.print(", ");
+  Serial.print(g.gyro.y);
+  Serial.print(", ");
+  Serial.print(g.gyro.z);
+  Serial.print(", ");
+
+  Serial.print(a.acceleration.x);
+  Serial.print(", ");
+  Serial.print(a.acceleration.y);
+  Serial.print(", ");
+  Serial.println(a.acceleration.z);
+}
+
+void displayInit(){
+  tft.begin();
+  tft.setRotation(1);
+  tft.setTextColor(TFT_WHITE,TFT_BLACK); 
+  tft.fillScreen(TFT_BLACK);
+  tft.setSwapBytes(true);
+  tft.setTextFont(4);
+  tft.setTextDatum(MC_DATUM);
+}
+
+void printString(String text, uint16_t textColor, uint16_t bgColor){
+  tft.fillScreen(bgColor);
+  tft.setTextColor(textColor, bgColor);
+  tft.drawCentreString(text, tft.width()/2, tft.height()/2, 4);
+}
diff --git a/README.md b/README.md
@@ -0,0 +1,43 @@
+# Python App for Data Sampling via Serial to CSV file
+
+![](app_demo.gif)
+
+You can easily save accelerometer and gyro readings from BNO055 or MPU6050 to your end device via Serial Comm. It was created to analyze the data obtained from the sensor and the GUI was configured using the PySimpleGUI. 
+
+## Requirements
+- python3
+- pip3 install -r requirements.txt
+
+## More information is available here
+* [IMU | Ep.1: Preparing an experiment to test linear positions (ft. MPU6050, GY-BNO055)][[Video]](https://youtu.be/3-IBOJ5FQvI)
+
+### Created & Maintained By
+[Eric Nam](https://github.com/0015)
+([Youtube](https://www.youtube.com/channel/UCRr2LnXXXuHn4z0rBvpfG7w))
+([Facebook](https://www.facebook.com/groups/138965931539175))
+
+## License
+
+Copyright (c) 2021 Eric N
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+
diff --git a/app_demo.gif b/app_demo.gif
diff --git a/main.py b/main.py
@@ -0,0 +1,163 @@
+import csv
+import queue
+import re
+import threading
+from time import perf_counter
+
+import PySimpleGUI as sg
+
+import serial_comm as my_serial
+
+
+class Application:
+
+    def __init__(self, *args, **kwargs):
+        super(Application, self).__init__(*args, **kwargs)
+        baud_rate = 115200
+        gui_queue = queue.Queue()
+        serial_connector = my_serial.SerialObj(baud_rate)
+
+        headerFont = ('Helvetica', 16)
+        middleFont = ('Helvetica', 14)
+        contextFont = ('Helvetica', 12)
+        smallFont = ('Helvetica', 10)
+        sg.theme('DarkBlue')
+
+        layout = [[sg.Text('GET ACCELEROMETER GYROSCOPE\nSAMPLING DATA VIA SERIAL', font=headerFont)],
+                  [sg.Text('Select your serial port', font=contextFont),
+                   sg.Button('Serial Port Reload', size=(20, 1), font=smallFont)],
+                  [sg.Listbox(values=[x[0] for x in my_serial.SerialObj.get_ports()],
+                              size=(40, 6),
+                              key='_SERIAL_PORT_LIST_',
+                              font=contextFont,
+                              enable_events=True)],
+                  [sg.Text('', key='_SERIAL_PORT_CONFIRM_', size=(40, 1), font=middleFont, ), ],
+                  [sg.Text('Buad Rate: {} bps'.format(baud_rate), size=(40, 1), font=middleFont, ), ],
+                  [sg.Text('How many samples?', font=contextFont, ), sg.VerticalSeparator(),
+                   sg.Input(do_not_clear=True, enable_events=True, key='_SAMPLE_IN_', font=contextFont, )],
+
+                  [sg.HorizontalSeparator()],
+                  [sg.Text('Serial Comm Status', font=contextFont, pad=((6, 0), (20, 0))), ],
+                  [sg.Text('', key='_OUTPUT_', size=(40, 2), font=middleFont, ), ],
+                  [sg.Button('Start', key='_ACT_BUTTON_', font=middleFont, size=(40, 1), pad=((0, 0), (0, 0)))],
+                  [sg.Button('Exit', font=middleFont, size=(40, 1), pad=((0, 0), (20, 0)))],
+                  [sg.Text('ThatProject - Version: 0.1', justification='right', size=(50, 1), font=smallFont, ), ]]
+
+        self.window = sg.Window('Simple Serial Application', layout, size=(320, 440), keep_on_top=True)
+
+        while True:
+            event, values = self.window.Read(timeout=100)
+
+            if event is None or event == 'Exit':
+                break
+
+            if event == 'Serial Port Reload':
+                self.get_ports()
+
+            if event == '_SERIAL_PORT_LIST_':
+                self.window['_SERIAL_PORT_CONFIRM_'].update(value=self.window['_SERIAL_PORT_LIST_'].get()[0])
+
+            if event == '_SAMPLE_IN_' and values['_SAMPLE_IN_'] and values['_SAMPLE_IN_'][-1] not in ('0123456789'):
+                self.window['_SAMPLE_IN_'].update(values['_SAMPLE_IN_'][:-1])
+
+            if event == '_ACT_BUTTON_':
+                print(self.window[event].get_text())
+                if self.window[event].get_text() == 'Start':
+
+                    if len(self.window['_SERIAL_PORT_LIST_'].get()) == 0:
+                        self.popup_dialog('Serial Port is not selected yet!', 'Serial Port', contextFont)
+
+                    elif len(self.window['_SAMPLE_IN_'].get()) == 0:
+                        self.popup_dialog('Set Sampling Count', 'Sampling Number Error', contextFont)
+
+                    else:
+                        self.stop_thread_trigger = False
+                        self.thread_serial = threading.Thread(target=self.start_serial_comm,
+                                                              args=(serial_connector,
+                                                                    self.window[
+                                                                        '_SERIAL_PORT_LIST_'].get()[
+                                                                        0],
+                                                                    int(self.window[
+                                                                            '_SAMPLE_IN_'].get()),
+                                                                    gui_queue, lambda: self.stop_thread_trigger),
+                                                              daemon=True)
+                        self.thread_serial.start()
+                        self.window['_ACT_BUTTON_'].update('Stop')
+
+                else:
+                    self.stop_thread_trigger = True
+                    self.thread_serial.join()
+                    self.window['_ACT_BUTTON_'].update('Start')
+
+            try:
+                message = gui_queue.get_nowait()
+            except queue.Empty:
+                message = None
+            if message is not None:
+                self.window['_OUTPUT_'].Update(message)
+                if 'Done' in message:
+                    self.window['_ACT_BUTTON_'].update('Start')
+                    self.popup_dialog(message, 'Success', contextFont)
+
+        self.window.Close()
+
+    def popup_dialog(self, contents, title, font):
+        sg.Popup(contents, title=title, keep_on_top=True, font=font)
+
+    def get_ports(self):
+        self.window['_SERIAL_PORT_LIST_'].Update(values=[x[0] for x in my_serial.SerialObj.get_ports()])
+
+    def start_serial_comm(self, serial_connector, serialport, sample_num, gui_queue, stop_thread_trigger):
+
+        start_time = 0
+
+        serial_connector.connect(serialport)
+        if serial_connector.is_connect():
+
+            gui_queue.put('Serial Connected!!')
+
+            n = 0
+            while n < sample_num:
+
+                try:
+                    if stop_thread_trigger():
+                        break
+
+                    data = serial_connector.get_data()
+                    if data is not None:
+
+                        if n == 0:
+                            gui_queue.put(' - Data Transmitting ::: Wait! ')
+                            start_time = perf_counter()
+
+                        decode_string = data.decode('utf-8')
+                        print(decode_string)
+                        if len(decode_string.split(',')) == 6:
+                            n += 1
+                            percent = n / sample_num * 100
+                            self.csv_writer('LoggedData_CalInertialAndMag.csv', n, decode_string)
+
+                            if percent % 10 == 0:
+                                gui_queue.put('Saving to CSV File: {}% complete'.format(int(percent)))
+
+                except OSError as error:
+                    print(error)
+
+                except UnicodeDecodeError as error:
+                    print(error)
+
+        serial_connector.disconnect()
+        time_taken = (perf_counter() - start_time)
+        sampling_rate = sample_num / time_taken
+        gui_queue.put('Sampling Rate: {} hz ::: Done!'.format(int(sampling_rate)))
+        return
+
+    def csv_writer(self, filename, index, data):
+        with open(filename, 'a') as f:
+            writer = csv.writer(f, delimiter=",", quoting=csv.QUOTE_NONE, escapechar=' ')
+            writer.writerow([index, re.sub(r"\s+", "", data), 0, 0,
+                             0])  # Dummy data for magnetometers, it doesn't use magnetometer in matlab.
+
+
+if __name__ == '__main__':
+    Application()
diff --git a/requirements.txt b/requirements.txt
@@ -0,0 +1,2 @@
+pysimplegui
+pyserial
diff --git a/serial_comm.py b/serial_comm.py
@@ -0,0 +1,32 @@
+import serial
+import serial.tools.list_ports
+
+
+class SerialObj:
+
+    def __init__(self, serial_speed):
+        self.serial = None
+        self.baud_rate = serial_speed
+
+    @staticmethod
+    def get_ports():
+        return list(serial.tools.list_ports.comports())
+
+    def connect(self, port):
+        self.serial = serial.Serial(port, self.baud_rate)
+        self.serial.flushInput()
+
+    def is_connect(self):
+        return self.serial.isOpen()
+
+    def get_data(self):
+        if not self.serial.isOpen():
+            return None
+        else:
+            return self.serial.readline()
+
+    def disconnect(self):
+        if self.serial is None:
+            return
+
+        self.serial.close()
