'FullArmControlG2.py' ändern

main_2Gelenke.py

@@ -1,296 +1,296 @@
-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 = [0, 0, 0, 0]
-stepper_og_values_rel = [0, 0, 0, 0]
-stepper_moveTo_angels_rel = [0, 0, 0, 0]
-stepper_actual_angels_rel = [0, 0, 0, 0]
-
-segments_length = [15, 50, 40, 30]
-end_effector_actual_pos = [0, 0, 0]  # y,z coordination of end effector and rotation angel theta
-# ik_m3_actual_pos = [0, 0, 0]  # y,z coordination of m3 and rotation angel theta
-end_effector_move_to_pos = [0, 0, 0]  # y,z coordination of end effector 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
-    print("relative:", relative_angels_stepper,
-          "absolute:", absolute_angels)
-    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_rel):
-    alpha_rel = np.array(stepper_angels_rel) * (np.pi / 180)
-    alpha = calculate_angels_to_horizon(alpha_rel)
-    start_x = 0
-    start_y = 0
-
-    z0 = np.cos(alpha[0]) * segments_length[0]
-    y0 = np.sin(alpha[0]) * 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]
-
-    end_effector_pos_2d = [y3, z3, stepper_angels_rel[0]]
-    m3_pos_2d = [y2, z2, stepper_angels_rel[0]]
-
-    return end_effector_pos_2d
-
-
-# Definition of function to calculate stepper angels to move end effector to desired coordination by changing M2 and M3
-def ik_calculate_angels(end_effector_position):
-    z0 = segments_length[0]
-    y0 = segments_length[1]
-
-    alpha = [90, -90, 0, 0]  # relative stepper angels to reach desired coordination
-    beta = [0, 0, 90]  # angels of right triangle
-
-    y = end_effector_position[0] - y0
-    z = end_effector_position[1] - z0
-
-    l2 = segments_length[2]
-    l3 = segments_length[3]
-    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis
-
-    gamma = calculate_triangle_angles(l3, l2, l_h)
-
-    beta[0] = np.rad2deg(np.arctan2(z, y))
-    beta[1] = np.rad2deg(np.arctan2(y, z))
-
-    alpha[2] = gamma[0] + beta[0]
-    alpha[3] = -(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
-end_effector_actual_pos = fk_calculate_position(stepper_actual_angels_rel)
-
-# 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()
-        end_effector_move_to_pos = np.array(end_effector_actual_pos) + np.array([0, ik_step_size, 0])
-        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
-
-        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
-            ik_calculate_angels(end_effector_move_to_pos))
-
-        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
-        update_slider_values(stepper_actual_angels_rel)
-        ik_m3_actual_pos = end_effector_move_to_pos
-
-    elif event == "-IK_Down_Button-" and arm_isCalibrated == 1:
-        ik_step_size = get_step_size()
-        end_effector_move_to_pos = np.array(end_effector_actual_pos) - np.array([0, ik_step_size, 0])
-        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
-
-        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
-            ik_calculate_angels(end_effector_move_to_pos))
-        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
-        update_slider_values(stepper_actual_angels_rel)
-        end_effector_actual_pos = end_effector_move_to_pos
-
-    elif event == "-IK_Forward_Button-" and arm_isCalibrated == 1:
-        ik_step_size = get_step_size()
-        end_effector_move_to_pos = np.array(end_effector_actual_pos) + np.array([ik_step_size, 0, 0])
-        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
-
-        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
-            ik_calculate_angels(end_effector_move_to_pos))
-        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
-        update_slider_values(stepper_actual_angels_rel)
-        end_effector_actual_pos = end_effector_move_to_pos
-
-    elif event == "-IK_Backward_Button-" and arm_isCalibrated == 1:
-        ik_step_size = get_step_size()
-        end_effector_move_to_pos = np.array(end_effector_actual_pos) - np.array([ik_step_size, 0, 0])
-        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
-
-        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
-            ik_calculate_angels(end_effector_move_to_pos))
-        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
-        update_slider_values(stepper_actual_angels_rel)
-        end_effector_actual_pos = end_effector_move_to_pos
-
-    stepper_actual_angels_abs = calculate_angels_to_horizon(stepper_actual_angels_rel)
-    end_effector_actual_pos = fk_calculate_position(stepper_actual_angels_abs)
-
-    # Formate data before sending
-    # str(arm_calibrate)
-    data = "<" + str(0) + "," + 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]
-    arm_calibrate = 1
-
-ser.close()
-window.close()
+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 = [0, 0, 0, 0]
+stepper_og_values_rel = [0, 0, 0, 0]
+stepper_moveTo_angels_rel = [0, 0, 0, 0]
+stepper_actual_angels_rel = [0, 0, 0, 0]
+
+segments_length = [15, 50, 40, 30]
+end_effector_actual_pos = [0, 0, 0]  # y,z coordination of end effector and rotation angel theta
+# ik_m3_actual_pos = [0, 0, 0]  # y,z coordination of m3 and rotation angel theta
+end_effector_move_to_pos = [0, 0, 0]  # y,z coordination of end effector 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
+    print("relative:", relative_angels_stepper,
+          "absolute:", absolute_angels)
+    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_rel):
+    alpha_rel = np.array(stepper_angels_rel) * (np.pi / 180)
+    alpha = calculate_angels_to_horizon(alpha_rel)
+    start_x = 0
+    start_y = 0
+
+    z0 = np.cos(alpha[0]) * segments_length[0]
+    y0 = np.sin(alpha[0]) * 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]
+
+    end_effector_pos_2d = [y3, z3, stepper_angels_rel[0]]
+    m3_pos_2d = [y2, z2, stepper_angels_rel[0]]
+
+    return end_effector_pos_2d
+
+
+# Definition of function to calculate stepper angels to move end effector to desired coordination by changing M2 and M3
+def ik_calculate_angels(end_effector_position):
+    z0 = segments_length[0]
+    y0 = segments_length[1]
+
+    alpha = [90, -90, 0, 0]  # relative stepper angels to reach desired coordination
+    beta = [0, 0, 90]  # angels of right triangle
+
+    y = end_effector_position[0] - y0
+    z = end_effector_position[1] - z0
+
+    l2 = segments_length[2]
+    l3 = segments_length[3]
+    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis
+
+    gamma = calculate_triangle_angles(l3, l2, l_h)
+
+    beta[0] = np.rad2deg(np.arctan2(z, y))
+    beta[1] = np.rad2deg(np.arctan2(y, z))
+
+    alpha[2] = gamma[0] + beta[0]
+    alpha[3] = -(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
+end_effector_actual_pos = fk_calculate_position(stepper_actual_angels_rel)
+
+# 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()
+        end_effector_move_to_pos = np.array(end_effector_actual_pos) + np.array([0, ik_step_size, 0])
+        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(end_effector_move_to_pos))
+
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
+        update_slider_values(stepper_actual_angels_rel)
+        ik_m3_actual_pos = end_effector_move_to_pos
+
+    elif event == "-IK_Down_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        end_effector_move_to_pos = np.array(end_effector_actual_pos) - np.array([0, ik_step_size, 0])
+        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(end_effector_move_to_pos))
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
+        update_slider_values(stepper_actual_angels_rel)
+        end_effector_actual_pos = end_effector_move_to_pos
+
+    elif event == "-IK_Forward_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        end_effector_move_to_pos = np.array(end_effector_actual_pos) + np.array([ik_step_size, 0, 0])
+        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(end_effector_move_to_pos))
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
+        update_slider_values(stepper_actual_angels_rel)
+        end_effector_actual_pos = end_effector_move_to_pos
+
+    elif event == "-IK_Backward_Button-" and arm_isCalibrated == 1:
+        ik_step_size = get_step_size()
+        end_effector_move_to_pos = np.array(end_effector_actual_pos) - np.array([ik_step_size, 0, 0])
+        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]
+
+        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
+            ik_calculate_angels(end_effector_move_to_pos))
+        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
+        update_slider_values(stepper_actual_angels_rel)
+        end_effector_actual_pos = end_effector_move_to_pos
+
+    stepper_actual_angels_abs = calculate_angels_to_horizon(stepper_actual_angels_rel)
+    end_effector_actual_pos = fk_calculate_position(stepper_actual_angels_abs)
+
+    # Formate data before sending
+    # str(arm_calibrate)
+    data = "<" + str(0) + "," + 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]
+    arm_calibrate = 1
+
+ser.close()
+window.close()