128 lines
3.0 KiB
Python
128 lines
3.0 KiB
Python
print("modbus test")
|
|
from func import *
|
|
|
|
# from func import set_user_reg_800
|
|
from pymodbus.client import ModbusTcpClient
|
|
import time
|
|
import glob
|
|
|
|
client = ModbusTcpClient(
|
|
host=MODBUS_SERVER_HOST,
|
|
port=MODBUS_SERVER_PORT,
|
|
)
|
|
client.connect()
|
|
# максимальное количество coils = 286
|
|
|
|
# bulb(2)
|
|
# get_coordinates()
|
|
# get_or_set_speed(10)
|
|
|
|
all_files = glob.glob("data/*.NC.result")
|
|
file = all_files[0]
|
|
data = []
|
|
with open(file, "r") as fp:
|
|
lines = fp.readlines()
|
|
for l in lines:
|
|
r = l.strip().split(",")
|
|
r[1] = int(float(r[1]) * 1000)
|
|
r[2] = int(float(r[2]) * 1000)
|
|
r[3] = int(float(r[3]) * 1000)
|
|
|
|
data.append(r)
|
|
|
|
|
|
# print(data)
|
|
def multiply_1000(n):
|
|
return int(n * 1000)
|
|
|
|
|
|
GOTO_Y = 5
|
|
try:
|
|
total = 0
|
|
state = None
|
|
step = 0
|
|
counter = 0
|
|
|
|
paths = data
|
|
paths = [
|
|
("line", 0, 0, 277.8, 0, 5, 0),
|
|
# ("line", 30, 30, 0, 0, 0, 0),
|
|
# ("line", 30, -30, 0, 0, 0, 0),
|
|
# ("line", 0, 0, 0, 0, 0, 5),
|
|
# ("line", -30, -30, 0, 0, 0, 0),
|
|
# ("line", -30, 30, 0, 0, 0, 0),
|
|
# ("line", 0, 0, 0, 0, -5, -5),
|
|
# ("line", 0, 0, 30, 0, 0, 0),
|
|
]
|
|
|
|
bulb(GOTO_Y, False, client)
|
|
get_or_set_speed(10, client)
|
|
|
|
# ставим пользовательские переменные в ноль
|
|
set_user_reg_800(
|
|
[
|
|
(0, to_double(0)),
|
|
(1, to_double(0)),
|
|
(2, to_double(0)),
|
|
(3, [0, 0]),
|
|
(4, [0, 0]),
|
|
(5, [0, 0]),
|
|
],
|
|
client,
|
|
)
|
|
# _, _, _, u_target, v_target, w_target = collect_coordinates(client)
|
|
|
|
# старт в авторежиме single loop
|
|
start_in_auto(client)
|
|
|
|
while True:
|
|
time.sleep(0.01)
|
|
total += 1
|
|
green_light = client.read_coils(GOTO_Y, 1, MODBUS_SLAVE_ID).bits[0]
|
|
|
|
if state == green_light:
|
|
continue
|
|
|
|
state = green_light
|
|
|
|
if state == True:
|
|
continue
|
|
|
|
x, y, z, u, v, w = collect_coordinates(client)
|
|
|
|
if step >= len(paths):
|
|
# counter += 1
|
|
# step = 0
|
|
stop_immediately(client)
|
|
exit()
|
|
|
|
current_step = paths[step]
|
|
line_type, *coord = current_step
|
|
print(f"{line_type} {step} of {len(paths)} {current_step}")
|
|
|
|
if line_type == "line":
|
|
set_x, set_y, set_z, set_u, set_v, set_w = coord
|
|
|
|
set_user_reg_800(
|
|
[
|
|
(0, to_double(multiply_1000(set_x))),
|
|
(1, to_double(multiply_1000(set_y))),
|
|
(2, to_double(multiply_1000(set_z))),
|
|
(3, to_double(multiply_1000(set_u))),
|
|
(4, to_double(multiply_1000(set_v))),
|
|
(5, to_double(multiply_1000(set_w))),
|
|
],
|
|
client,
|
|
)
|
|
# bulb(4, True, client)
|
|
|
|
if not x == y == z == u == v == w == 0:
|
|
bulb(GOTO_Y, True, client)
|
|
step += 1
|
|
|
|
|
|
except Exception as e:
|
|
print("error", e)
|
|
|
|
client.close()
|