finger_viz.py

Go to the documentation of this file.

"""
Reads data from a serial port and displays it in real-time using matplotlib.
Make sure to set COM_PORT to the correct value before running.
"""
 
import serial
from rendering import PointsInSpace
 
COM_PORT = "COM13"
TRAILING_POINTS = 16
MIN_MESSAGE_BYTES = 16
FORCE_SCALING = 0.02
 
ser = serial.Serial(
    port=COM_PORT,
    baudrate=115200,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    bytesize=serial.EIGHTBITS,
    timeout=0,
)
 
print("Connected to: " + ser.portstr)
 
x_list = []
z_list = []
x_mirror_list = []
z_mirror_list = []
 
pp = PointsInSpace(
    "End Effector Position",
    "Z [m]",
    "X [m]",
    xlim=[-0.15, 0.15],
    ylim=[0, 0.15],
    enable_grid=True,
    enable_legend=True,
)
pp.register_plot("connected", alpha=0.4)
pp.register_plot("mirror", alpha=0.4)
# pp.register_plot("stiffness", m="-")
# pp.register_plot("damping", m="-")
# pp.register_plot("force", m="-")
 
while True:
    try:
        # Read bytes goofery
        bytes_to_read = ser.in_waiting
        if bytes_to_read < MIN_MESSAGE_BYTES:
            continue
        line = ser.read(bytes_to_read).decode("utf-8")
        segments = line.split()
 
        # Parse the message by reading the value after each label
        try:
            # Do this in two steps so that values are not changed if not all values exist
            def value_by_label(label):
                return float(segments[segments.index(f"{label}:") + 1])
 
            x = value_by_label("pos.at(0)")
            z = value_by_label("pos.at(2)")
            x_mirror = value_by_label("mirror_pos_0")
            z_mirror = value_by_label("mirror_pos_2")
            F_stiffness_x = 0
            F_stiffness_z = 0
            F_damping_x = 0
            F_damping_z = 0
            F_kx = 0
            F_kz = 0
 
            try:
                F_stiffness_x = value_by_label("F_stiffness_x")
                F_stiffness_z = value_by_label("F_stiffness_z")
                F_damping_x = value_by_label("F_damping_x")
                F_damping_z = value_by_label("F_damping_z")
                F_kx = value_by_label("F_kx")
                F_kz = value_by_label("F_kz")
            except Exception:
                pass
 
            x_list.append(x)
            z_list.append(z)
            x_mirror_list.append(x_mirror)
            z_mirror_list.append(z_mirror)
        except Exception as e:
            print(e)
            continue
 
        if len(x_list) > TRAILING_POINTS:
            x_list.pop(0)
            z_list.pop(0)
            x_mirror_list.pop(0)
            z_mirror_list.pop(0)
 
        # Display results
        pp.start_drawing()
        pp.draw_points("connected", z_list, x_list)
        pp.draw_points("mirror", z_mirror_list, x_mirror_list)
        # pp.draw_points(
        #     "stiffness",
        #     [z, z + F_stiffness_z * FORCE_SCALING],
        #     [x, x + F_stiffness_x * FORCE_SCALING],
        # )
        # pp.draw_points(
        #     "damping",
        #     [z, z + F_damping_z * FORCE_SCALING],
        #     [x, x + F_damping_x * FORCE_SCALING],
        # )
        # pp.draw_points(
        #     "force", [z, z + F_kz * FORCE_SCALING], [x, x + F_kx * FORCE_SCALING]
        # )
        pp.end_drawing()
    except Exception as e:
        print(e)
        ser.close()
        print("Closed connection")
        quit()
 
ser.close()
print("Closed connection")