New_Algorithm.py - master - tracie/Sirius_projects

New_Algorithm.py
134 строки5 KB
Ksenia Vasileva
done
13 окт 2025, 07:51
13 окт 2025, 07:517ae8d58
import re
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np

def parse_gcode(file_path):
    absolute_coordinates = []
    displacements = []

    # Начальная позиция
    current_x, current_y, current_z = 0.0, 0.0, 0.0

    with open(file_path, 'r') as file:
        for line in file:
            x_match = re.search(r'X([-+]?\d*\.?\d+)', line)
            y_match = re.search(r'Y([-+]?\d*\.?\d+)', line)
            z_match = re.search(r'Z([-+]?\d*\.?\d+)', line)

            # Новые координаты (если есть)
            new_x = float(x_match.group(1)) if x_match else current_x
            new_y = float(y_match.group(1)) if y_match else current_y
            new_z = float(z_match.group(1)) if z_match else current_z

            # Вычисление смещения
            dx = new_x - current_x
            dy = new_y - current_y
            dz = new_z - current_z

            # Добавляем смещение только если оно ненулевое
            if not (round(dx, 4) == 0 and round(dy, 4) == 0 and round(dz, 4) == 0):
                displacements.append((dx, dy, dz))

            # Обновляем текущую позицию
            current_x, current_y, current_z = new_x, new_y, new_z

            # Сохраняем абсолютные координаты для графика
            absolute_coordinates.append((current_x, current_y, current_z))

    return absolute_coordinates, displacements

def apply_rotation(coordinates, rotation_matrix):
    # Применяем матрицу поворота к каждой точке
    rotated_coordinates = []
    for x, y, z in coordinates:
        original_point = np.array([x, y, z])
        rotated_point = rotation_matrix.dot(original_point)
        rotated_coordinates.append(tuple(rotated_point))

    return rotated_coordinates

def generate_rapid_with_displacements(displacements, output_file):
    with open(output_file, 'w') as file:
        # Шапка модуля
        file.write("MODULE Module1\n")
        file.write("    VAR robtarget home;\n")
        file.write("    VAR robtarget p0;\n")
        file.write("    VAR loaddata my_load:=[1.50062,[-9.44125,1.51473,53.2227],[1,0,0,0],0,0,0];\n")
        file.write("\n")

        file.write("    PROC MoveToPoint(num dX, num dY, num dZ, speeddata V)\n")
        file.write("        p0 := Offs(p0, dX, dY, dZ);\n")
        file.write("        MoveL p0, V, fine, tWeldGun\\WObj:=wobjIngot;\n")
        file.write("    ENDPROC\n")
        file.write("\n")

        file.write("    PROC Object_trajectory()\n")
        file.write("        ShpindelOn;\n")
        file.write("        FCSpdChgAct 50;\n")
        file.write("        VAR speeddata V := v50;\n")
        # Вызовы MoveToPoint
        for dx, dy, dz in displacements:
            file.write(f"        MoveToPoint {dx:.4f}, {dy:.4f}, {dz:.4f}, V;\n")

        file.write("        FCSpdChgDeact;\n")
        file.write("    ENDPROC\n")
        file.write("\n")
        # Основная программа
        file.write("    PROC main()\n")
        file.write("        wobjIngot.oframe.trans := CPos(\Tool:=tWeldGun \WObj:=wobj0);\n")
        file.write("        p0 := CRobT(\Tool:=tWeldGun \WObj:=wobjIngot);\n")
        file.write("        home := Offs(p0, 0, 0, -50);\n")
        file.write("        MoveL home, v100, fine, tWeldGun\WObj:=wobjIngot;\n")
        file.write("        FCInit;\n")
        file.write("        Object_trajectory;\n")
        file.write("        ShpindelOff;\n")
        file.write("    ENDPROC\n")
        file.write("\n")

        file.write("    PROC FCInit()\n")
        file.write("        my_load := FCLoadID(\MaxMoveAx5:=15 \MaxMoveAx6:=30 \WarningsOff);\n")
        file.write("        FCCalib my_load;\n")
        file.write("    ENDPROC\n")
        file.write("    PROC ShpindelOn()\n")
        file.write("        SetDO do_Shpindel_motor, 1;\n")
        file.write("    ENDPROC\n")
        file.write("    PROC ShpindelOff()\n")
        file.write("        SetDO do_Shpindel_motor, 0;\n")
        file.write("    ENDPROC\n")

        file.write("ENDMODULE\n")

def plot_trajectory(absolute_coordinates):
    x_vals = [coord[0] for coord in absolute_coordinates]
    y_vals = [coord[1] for coord in absolute_coordinates]
    z_vals = [coord[2] for coord in absolute_coordinates]

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(x_vals, y_vals, z_vals, marker='o', color='b', label='Траектория')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_title("3D Траектория движения по координатам GCODE")
    plt.legend()
    plt.show()

# Основной код
gcode_file = "GcodeQuad.txt"
output_rapid_file = "output_rapid_with_procedure.txt"

reflection_matrix = np.array([
    [1, 0, 0],
    [0, 1, 0],
    [0, 0, -1]
])

absolute_coordinates, displacements = parse_gcode(gcode_file)
rotated_displacements = apply_rotation(displacements, reflection_matrix)
rotated_absolute_coordinates = apply_rotation(absolute_coordinates, reflection_matrix)

generate_rapid_with_displacements(rotated_displacements, output_rapid_file)
plot_trajectory(rotated_absolute_coordinates)

print(f"RAPID-код с процедурами успешно сохранен в {output_rapid_file}")