| 1 | import serial |
| 2 | import struct |
| 3 | import numpy as np |
| 4 | import matplotlib.pyplot as plt |
| 5 | import matplotlib |
| 6 | |
| 7 | matplotlib.use('gtk3agg') |
| 8 | |
| 9 | def main(): |
| 10 | print("Opening serial port...") |
| 11 | ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1) |
| 12 | |
| 13 | figure, (w_plot, x_plot, y_plot, z_plot) = plt.subplots(4, 1, figsize=(10, 6)) |
| 14 | time_axis = np.arange(0, 10, 0.1) |
| 15 | w_data = np.zeros(time_axis.size) |
| 16 | x_data = np.zeros(time_axis.size) |
| 17 | y_data = np.zeros(time_axis.size) |
| 18 | z_data = np.zeros(time_axis.size) |
| 19 | |
| 20 | # Starting plot |
| 21 | w_line, = w_plot.plot(time_axis, w_data, label='W Data') |
| 22 | x_line, = x_plot.plot(time_axis, x_data, label='X Data') |
| 23 | y_line, = y_plot.plot(time_axis, y_data, label='Y Data') |
| 24 | z_line, = z_plot.plot(time_axis, z_data, label='Z Data') |
| 25 | |
| 26 | w_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50)) |
| 27 | x_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50)) |
| 28 | y_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50)) |
| 29 | z_plot.set(ylim=(-1, 1), yticks=np.arange(-1, 1, 50)) |
| 30 | |
| 31 | w_plot.set_title('W Data') |
| 32 | x_plot.set_title('X Data') |
| 33 | y_plot.set_title('Y Data') |
| 34 | z_plot.set_title('Z Data') |
| 35 | |
| 36 | plt.tight_layout() |
| 37 | plt.ion() # Interactive mode on |
| 38 | plt.show() |
| 39 | |
| 40 | i = 0 |
| 41 | loop = True |
| 42 | while loop: |
| 43 | try: |
| 44 | read_bytes = ser.read(16) |
| 45 | |
| 46 | if len(read_bytes) == 16: |
| 47 | w = struct.unpack('f', read_bytes[0:4])[0] |
| 48 | x = struct.unpack('f', read_bytes[4:8])[0] |
| 49 | y = struct.unpack('f', read_bytes[8:12])[0] |
| 50 | z = struct.unpack('f', read_bytes[12:16])[0] |
| 51 | |
| 52 | # shift the data and append the new values |
| 53 | w_data = np.roll(w_data, -1) |
| 54 | x_data = np.roll(x_data, -1) |
| 55 | y_data = np.roll(y_data, -1) |
| 56 | z_data = np.roll(z_data, -1) |
| 57 | |
| 58 | w_data[-1] = w |
| 59 | y_data[-1] = x |
| 60 | x_data[-1] = y |
| 61 | z_data[-1] = z |
| 62 | |
| 63 | w_line.set_ydata(w_data) |
| 64 | x_line.set_ydata(x_data) |
| 65 | y_line.set_ydata(y_data) |
| 66 | z_line.set_ydata(z_data) |
| 67 | |
| 68 | # Redraw the plots |
| 69 | plt.draw() |
| 70 | plt.pause(0.01) # Pause to allow the plot to update |
| 71 | except KeyboardInterrupt: |
| 72 | print("Exiting...") |
| 73 | loop = False |
| 74 | |
| 75 | if __name__ == "__main__": |
| 76 | main() |
| 77 | |
