Dateien hochladen nach ''

main.py

@@ -0,0 +1,299 @@
+import os
+
+import PySimpleGUI as sg
+import serial
+import numpy as np
+import time
+
+# sg.preview_all_look_and_feel_themes()  #to display all themes
+
+# open serial communication
+ser = serial.Serial("COM7", 9600)
+# Waiting for arduino to start sketch
+time.sleep(2)
+
+slider_width = 50
+slider_height = 40
+button_width = 10
+button_height = 2
+
+stepper_range = ((0, 360), (-115, 115), (-115, 115), (-115, 115))
+stepper_default_values_rel = [180, 60, 60, -20]
+stepper_og_values_rel = [0, -115, -115, -115]
+stepper_moveTo_angels_rel = [0, 0, 0, 0]
+stepper_actual_angels_rel = [0, 120, 60, -40]
+
+segments_length = [15, 50, 40, 30]
+end_effector_pos = [0, 0, 0]  # x,y,z coordination of m4 and rotation angel theta
+ik_m3_actual_pos = [0, 0, 0]  # y,z coordination of m3 and rotation angel theta
+ik_m3_move_to_pos = [0, 0, 0]  # y,z coordination of m3 and rotation angel theta
+ik_step_size = 5  # default step size 5 cm
+
+arm_isCalibrated = 0
+arm_calibrate = 0
+
+FK_slider_values = [0, 0, 0, 0]
+
+
+# Definition of function to calculate angels of a triangle with known side lengths using the law of cosine
+def calculate_triangle_angles(a, b, c):
+    angle_a = np.rad2deg(np.arccos((b ** 2 + c ** 2 - a ** 2) / (2 * b * c)))
+    angle_b = np.rad2deg(np.arccos((c ** 2 + a ** 2 - b ** 2) / (2 * c * a)))
+    angle_c = 180 - angle_a - angle_b  # The sum of angles in a triangle is 180 degrees
+
+    return angle_a, angle_b, angle_c
+
+
+# Definition of function to calculate joints angels relative to horizon
+def calculate_angels_to_horizon(relative_angels_stepper):
+    absolute_angels = [0, 0, 0, 0]
+    absolute_angels[0] = relative_angels_stepper[0]
+    absolute_angels[1] = absolute_angels[0] + relative_angels_stepper[1]
+    absolute_angels[2] = absolute_angels[1] + relative_angels_stepper[2]
+    absolute_angels[3] = absolute_angels[2] + relative_angels_stepper[3]
+
+    for j in range(4):
+        if absolute_angels[j] > 360:
+            absolute_angels[j] = absolute_angels[j] - 360
+
+    return absolute_angels
+
+
+# Definition of function to calculate joints angels relative to another, based on their angels relative to horizon
+def calculate_angels_relative(absolute_angels_stepper):
+    relative_angels = [0, 0, 0, 0]
+    relative_angels[0] = absolute_angels_stepper[0]
+    relative_angels[1] = absolute_angels_stepper[1] - relative_angels[0]
+    relative_angels[2] = absolute_angels_stepper[2] - relative_angels[1]
+    relative_angels[3] = absolute_angels_stepper[3] - relative_angels[2]
+
+    for j in range(4):
+        if relative_angels[j] > 360:
+            relative_angels[j] = relative_angels[j] - 360
+
+    return relative_angels
+
+
+# Definition of function to calculate coordination of end effector and Motor3
+def fk_calculate_position(stepper_angels):
+    alpha = np.array(stepper_angels) * (np.pi / 180)
+    start_x = 0
+    start_y = 0
+
+    y0 = 0
+    z0 = segments_length[0]
+
+    y1 = y0 + np.cos(alpha[1]) * segments_length[1]
+    z1 = z0 + np.sin(alpha[1]) * segments_length[1]
+
+    y2 = y1 + np.cos(alpha[2]) * segments_length[2]
+    z2 = z1 + np.sin(alpha[2]) * segments_length[2]
+
+    y3 = y2 + np.cos(alpha[3]) * segments_length[3]
+    z3 = z2 + np.sin(alpha[3]) * segments_length[3]
+
+    m4_pos_2d = [y3, z3, stepper_angels[0]]
+    m3_pos_2d = [y2, z2, stepper_angels[0]]
+
+    return m3_pos_2d, m4_pos_2d
+
+
+# Definition of function to calculate stepper angels to move Motor3 to desired coordination
+def ik_calculate_angels(m3_position, m4_angel):
+    z0 = segments_length[0]
+    # y0 = 0
+
+    alpha = [90, 0, 0, m4_angel]  # relative stepper angels to reach desired coordination
+    beta = [0, 0, 90]  # angels of right triangle
+    # gamma = [0, 0, 0]  # angels of triangle l_l1_l2
+
+    y = m3_position[0]  # - y0
+    z = m3_position[1] - z0
+
+    l1 = segments_length[1]
+    l2 = segments_length[2]
+    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis
+
+    gamma = calculate_triangle_angles(l2, l1, l_h)
+
+    beta[0] = np.rad2deg(np.arctan2(z, y))
+    beta[1] = np.rad2deg(np.arctan2(y, z))
+
+    alpha[1] = -(90 - gamma[0] - beta[0])
+    alpha[2] = -(180 - gamma[2])
+
+    return alpha
+
+
+# Define function to update slider values to match actual stepper values
+def update_slider_values(slider_updated_values):
+    window['-Slider_Stepper0_FK-'].update(slider_updated_values[0])
+    window['-Slider_Stepper1-'].update(slider_updated_values[1])
+    window['-Slider_Stepper2-'].update(slider_updated_values[2])
+    window['-Slider_Stepper3_FK-'].update(slider_updated_values[3])
+
+
+# Define function to get chosen step size
+def get_step_size():
+    if values['-Radio_Option1-']:
+        step_size = 1
+    elif values['-Radio_Option2-']:
+        step_size = 5
+    elif values['-Radio_Option3-']:
+        step_size = 10
+
+    return step_size
+
+
+# Calculate Default angels and position
+stepper_actual_angels_abs = calculate_angels_to_horizon(stepper_actual_angels_rel)
+ik_m3_actual_pos, end_effector_pos = fk_calculate_position(stepper_actual_angels_abs)
+
+# Definition of GUI Layout
+FK_column = [
+    [sg.Text("Forward Kinematics:")],
+    [sg.Text("")],
+    [sg.Text("Stepper motor 0")],
+    [sg.Text(""),
+     sg.Slider(range=(0, 360), size=(slider_width, slider_height), default_value=stepper_default_values_rel[0],
+               orientation="h", key="-Slider_Stepper0_FK-"), sg.Text("")],
+    [sg.Text("Stepper motor 1")],
+    [sg.Text(""),
+     sg.Slider(range=(-115, 115), size=(slider_width, slider_height), default_value=stepper_default_values_rel[1],
+               orientation="h", key="-Slider_Stepper1-"), sg.Text("")],
+    [sg.Text("Stepper motor 2")],
+    [sg.Text(""),
+     sg.Slider(range=(-115, 115), size=(slider_width, slider_height), default_value=stepper_default_values_rel[2],
+               orientation="h", key="-Slider_Stepper2-"), sg.Text("")],
+    [sg.Text("Stepper motor 3")],
+    [sg.Text(""),
+     sg.Slider(range=(-115, 115), size=(slider_width, slider_height), default_value=stepper_default_values_rel[3],
+               orientation="h", key="-Slider_Stepper3_FK-"), sg.Text("")],
+    [sg.Button("Submit", key="-FK_Set_Button-", size=(button_width, button_height)),
+     sg.Button("Reset", key="-FK_Reset_Button-", size=(button_width, button_height)),
+     sg.Button("Calibrate", key="-FK_Calibrate_Button-", size=(button_width, button_height))]
+
+]
+
+IK_column = [
+    [sg.Text("Inverse Kinematics:")],
+    [sg.Text("")],
+    [sg.Text("Choose Step size:"),
+     sg.Radio('1 cm', "RADIO1", key="-Radio_Option1-"),
+     sg.Radio('5 cm', "RADIO1", key="-Radio_Option2-", default=True, ),
+     sg.Radio('10 cm', "RADIO1", key="-Radio_Option3-")],
+    [sg.Text(""), sg.Text(""), sg.Text(""),
+     sg.Button("Up", key="-IK_Up_Button-", size=(button_width, button_height))],
+    [sg.Button("Backward", key="-IK_Backward_Button-", size=(button_width, button_height)),
+     sg.Button("Forward", key="-IK_Forward_Button-", size=(button_width, button_height))],
+    [sg.Text(""), sg.Text(""), sg.Text(""),
+     sg.Button("Down", key="-IK_Down_Button-", size=(button_width, button_height))],
+    [sg.Text("")],
+    [sg.Text("")],
+    #[sg.Text(ik_m3_actual_pos, key="-m3_pos-")],
+    [sg.Text("")]
+]
+
+layout = [
+    [
+        sg.Column(FK_column),
+        sg.VSeparator(),
+        sg.Column(IK_column),  # vertical_alignment="Top"
+        sg.VSeparator(),
+    ]
+]
+window = sg.Window("Robot Control", layout)
+
+# Loop for GUI feedback and Serial Communication
+
+while True:
+    event, values = window.read()
+    if event == "EXIT" or event == sg.WIN_CLOSED:
+        break
+
+    elif event == "-FK_Set_Button-" and arm_isCalibrated == 1:
+        FK_slider_values[0] = values["-Slider_Stepper0_FK-"]
+        FK_slider_values[1] = values["-Slider_Stepper1-"]
+        FK_slider_values[2] = values["-Slider_Stepper2-"]
+        FK_slider_values[3] = values["-Slider_Stepper3_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(FK_slider_values)
+        stepper_actual_angels_rel = np.array(FK_slider_values)
+
+    elif event == "-FK_Calibrate_Button-":
+        update_slider_values(stepper_og_values_rel)
+
+        arm_calibrate = 1
+        arm_isCalibrated = 1
+        stepper_actual_angels_rel = stepper_og_values_rel
+
+    elif event == "-FK_Reset_Button-":
+        update_slider_values(stepper_default_values_rel)
+
+    # Do not forget maximal reachable coordination!
+    elif event == "-IK_Up_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        ik_m3_move_to_pos = np.array(ik_m3_actual_pos) + np.array([0, ik_step_size, 0])
+        ik_m3_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))  # may need to with - and +
+
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        update_slider_values(stepper_actual_angels_rel)
+        ik_m3_actual_pos = ik_m3_move_to_pos
+
+    elif event == "-IK_Down_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        ik_m3_move_to_pos = np.array(ik_m3_actual_pos) - np.array([0, ik_step_size, 0])
+        ik_m3_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        update_slider_values(stepper_actual_angels_rel)
+        ik_m3_actual_pos = ik_m3_move_to_pos
+
+    elif event == "-IK_Forward_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        ik_m3_move_to_pos = np.array(ik_m3_actual_pos) + np.array([ik_step_size, 0, 0])
+        ik_m3_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        update_slider_values(stepper_actual_angels_rel)
+        ik_m3_actual_pos = ik_m3_move_to_pos
+
+    elif event == "-IK_Backward_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        ik_m3_move_to_pos = np.array(ik_m3_actual_pos) - np.array([ik_step_size, 0, 0])
+        ik_m3_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(ik_m3_move_to_pos, values["-Slider_Stepper3_FK-"]))
+        update_slider_values(stepper_actual_angels_rel)
+        ik_m3_actual_pos = ik_m3_move_to_pos
+
+    stepper_actual_angels_abs = calculate_angels_to_horizon(stepper_actual_angels_rel)
+    ik_m3_actual_pos, end_effector_pos = fk_calculate_position(stepper_actual_angels_abs)
+
+   # window["-m3_pos-"].update(ik_m3_actual_pos)  # Update showed position
+
+    print(ik_step_size)
+    print(ik_m3_move_to_pos)
+
+    # Formate data before sending
+    data = "<" + str(arm_calibrate) + "," + str(stepper_moveTo_angels_rel[0]) + "," + str(stepper_moveTo_angels_rel[1]) \
+           + "," + str(stepper_moveTo_angels_rel[2]) + "," + str(stepper_moveTo_angels_rel[3]) + ">"
+    print(data)
+    ser.write(data.encode())
+
+    received_string = ser.readline().decode()
+    print(received_string)
+
+    stepper_moveTo_angels_rel = [0, 0, 0, 0]
+
+ser.close()
+window.close()