117 lines
3.7 KiB
Python
117 lines
3.7 KiB
Python
import socket
|
|
from time import sleep
|
|
import time
|
|
import threading
|
|
import logging
|
|
from logging.handlers import RotatingFileHandler
|
|
|
|
def setup_logger(log_file='app.log'):
|
|
# 创建日志记录器
|
|
logger = logging.getLogger('my_app')
|
|
logger.setLevel(logging.DEBUG)
|
|
# 创建格式化器
|
|
formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
|
|
# 创建旋转文件处理器
|
|
# maxBytes=50MB (50 * 1024 * 1024), backupCount=5
|
|
handler = RotatingFileHandler(
|
|
log_file,
|
|
maxBytes=50 * 1024 * 1024, # 50MB
|
|
backupCount=5, # 保留5个备份文件
|
|
encoding='utf-8'
|
|
)
|
|
handler.setFormatter(formatter)
|
|
# 添加处理器到日志记录器
|
|
logger.addHandler(handler)
|
|
return logger
|
|
def ServoP(client, pt_floats):
|
|
tcp_command = f"ServoP({pt_floats[0]},{pt_floats[1]},{pt_floats[2]},{pt_floats[3]},{pt_floats[4]},{pt_floats[5]}, t=0.004, aheadtime=80, gain=500)"
|
|
tcp_command = tcp_command.encode()
|
|
client.send(tcp_command)
|
|
|
|
def ServoJ(client, J1_angle):
|
|
tcp_command = "servoj(0,%f,%f,0,0,0,t=0.008,aheadtime=20, gain=200)" % (J1_angle,J1_angle/2)
|
|
tcp_command = tcp_command.encode()
|
|
client.sendall(tcp_command)
|
|
# buf = client.recv(200).decode() # 接收反馈信息的长度
|
|
# print(buf)
|
|
|
|
def servoj_2(client, J1_angle):
|
|
tcp_command = "servoj(%f,0,0,0,0,0,t=2,aheadtime=50, gain=500)" % (J1_angle)
|
|
tcp_command = tcp_command.encode()
|
|
print(tcp_command)
|
|
client.send(tcp_command)
|
|
|
|
def EnableRobot(client):
|
|
tcp_command = "EnableRobot()"
|
|
tcp_command = tcp_command.encode()
|
|
client.send(tcp_command)
|
|
buf = client.recv(200).decode() # 接收反馈信息的长度
|
|
index_str = buf[buf.find('{') + 1:buf.find('}')]
|
|
if (index_str == ''):
|
|
return -1
|
|
return buf[buf.find('{') + 1:buf.find('}')]
|
|
stop = True
|
|
def enableStatus(client):
|
|
global stop
|
|
logger = setup_logger()
|
|
servoj_2(client, 0)
|
|
sleep(3)
|
|
J1 = 0
|
|
bias = 0.04
|
|
pl = bias
|
|
while stop:
|
|
time_home = time.time()
|
|
if J1 > 60 and bias == pl:
|
|
bias = -pl
|
|
elif J1 < -1 and bias == -pl:
|
|
bias = pl
|
|
J1 = J1 + bias
|
|
ServoJ(client, J1)
|
|
time.sleep(0.007)
|
|
tie = time.time()-time_home
|
|
logger.info(f"time: {tie}")
|
|
print(tie)
|
|
|
|
SERVER3_ADDRESS = '192.168.5.1'
|
|
SERVER3_PORT = 29999
|
|
client_socket1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
|
|
# 设置TCP_NODELAY禁用Nagle算法
|
|
client_socket1.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
|
client_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 100000)
|
|
|
|
client_socket1.connect((SERVER3_ADDRESS, SERVER3_PORT))
|
|
#client_socket1.setblocking(False)
|
|
print('成功连接到第1个服务端')
|
|
#enableStatus(client_socket1)
|
|
def worker(): #子线程接受模式
|
|
global stop
|
|
while True:
|
|
try:
|
|
data = client_socket1.recv(1024).decode()
|
|
if data:
|
|
print(data)
|
|
index_str = data[data.find('{') + 1:data.find('}')]
|
|
if (index_str == ''):
|
|
stop = False
|
|
print(11111111111)
|
|
else:
|
|
stop = True
|
|
except BlockingIOError:
|
|
time.sleep(0.001)
|
|
time_h = time.time()
|
|
try:
|
|
EnableRobot(client_socket1)
|
|
feed_thread = threading.Thread(
|
|
target=worker) # 机器状态反馈线程
|
|
feed_thread.daemon = True
|
|
feed_thread.start()
|
|
enableStatus(client_socket1)
|
|
except KeyboardInterrupt:
|
|
print("用户中断操作。")
|
|
print((time.time()-time_h)/60)
|
|
except Exception as e:
|
|
print("发生异常：", e)
|
|
print((time.time() - time_h) / 60)
|
|
|
|
print((time.time()-time_h)/60) |