You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
408 lines
20 KiB
408 lines
20 KiB
import time
|
|
import serial
|
|
from serial import Serial
|
|
import threading
|
|
from ETController import connectETController,sendCMD,send_Point,wait_stop
|
|
from gpio import init_gpio, turn_off_relay, turn_on_relay
|
|
from program_config import load_process_config, program_switch,sequence_1, sequence_2, sequence_3, sequence_4
|
|
from serial_init import serial_init
|
|
from serial_handler import handle_special_commands, read_cmd_from_shared, serial_receive_data
|
|
|
|
if __name__ == "__main__":
|
|
|
|
# 加载配置文件并提取参数
|
|
process_config = load_process_config()
|
|
if process_config:
|
|
# 提取预设参数列表（现在PRESET_PARAMS本身包含所有子配置）
|
|
PRESET_PARAMS = process_config["PRESET_PARAMS"]
|
|
|
|
init_gpio()
|
|
ser = serial_init()
|
|
# 检查串口初始化结果
|
|
if not ser or not isinstance(ser, serial.Serial) or not ser.is_open:
|
|
print("串口初始化失败，无法继续运行")
|
|
# 确保在函数内部返回
|
|
exit()
|
|
|
|
time.sleep(20)
|
|
# 默认关闭喷枪
|
|
turn_off_relay()
|
|
|
|
# 机器人IP地址
|
|
robot_ip = "192.168.1.100"
|
|
result = connectETController(robot_ip)
|
|
sock = None
|
|
if len(result) == 2:
|
|
conSuc, sock = result
|
|
if conSuc:
|
|
print("成功连接到机器人")
|
|
# 1.上电设置
|
|
ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1})
|
|
print("开始上电")
|
|
time.sleep(20)
|
|
print("开始检测上电状态")
|
|
ret, result, id = sendCMD(sock, "get_robot_power_status")
|
|
|
|
if result == 0:
|
|
ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1})
|
|
time.sleep(20)
|
|
else :
|
|
print("已上电")
|
|
# 2. 清除报警
|
|
print("清除报警")
|
|
ret, result, id = sendCMD(sock,"clearAlarm")
|
|
print("清除报警返回值")
|
|
print(result)
|
|
time.sleep(5)
|
|
print("报警清除完成")
|
|
|
|
# 获取同步状态
|
|
suc, result , id = sendCMD(sock, "getMotorStatus")
|
|
if result == 0:
|
|
# 同步伺服编码器数据
|
|
suc,result,id = sendCMD(sock, "syncMotorStatus")
|
|
if result:
|
|
print("同步伺服编码器数据成功")
|
|
else:
|
|
print("同步伺服编码器数据失败")
|
|
time. sleep (2)
|
|
|
|
# 3. 编码器零位校准
|
|
suc, result, id = sendCMD(sock, "getServoStatus")
|
|
print("获取伺服状态返回值")
|
|
print(result)
|
|
time.sleep(2)
|
|
if result == 0:
|
|
# 设置机械臂伺服状态为“开”（status=1）
|
|
suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
|
|
print("设置机械臂伺服状态返回值")
|
|
print(result)
|
|
time.sleep(5)
|
|
suc, result, id = sendCMD(sock, "calibrate_encoder_zero_position")
|
|
print("编码器校准返回值")
|
|
print(result)
|
|
time.sleep(5)
|
|
if result == 1:
|
|
print("编码器校准成功")
|
|
else:
|
|
print("编码器校准失败")
|
|
|
|
# 初始化透传服务
|
|
suc, result, id = sendCMD(sock, "get_transparent_transmission_state")
|
|
if id == 1:
|
|
print(suc, result, id)
|
|
if result == 1:
|
|
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
|
|
print("已清空透传缓存")
|
|
time.sleep(0.5)
|
|
else:
|
|
print("无缓存")
|
|
|
|
# 在此处做检测等相关操作
|
|
suc, result, id = sendCMD(sock, "getRobotState")
|
|
if (result == 4):
|
|
# 清除报警
|
|
suc, result, id = sendCMD(sock, "clearAlarm")
|
|
time.sleep(0.5)
|
|
|
|
# 获取同步状态
|
|
suc, result, id = sendCMD(sock, "getMotorStatus")
|
|
if (result == 0):
|
|
# 同步伺服编码器数据
|
|
suc, result, id = sendCMD(sock, "syncMotorStatus")
|
|
time.sleep(0.5)
|
|
else:
|
|
print("同步状态获取失败")
|
|
|
|
# 获取机械臂伺服状态
|
|
suc, result, id = sendCMD(sock, "getServoStatus")
|
|
if (result == 0):
|
|
# 设置机械臂伺服状态ON
|
|
suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
|
|
time.sleep(1)
|
|
else:
|
|
print("机械臂伺服取失败")
|
|
|
|
# 获取当前机器人是否处于透传状态
|
|
suc, result, id = sendCMD(sock, "get_transparent_transmission_state")
|
|
print(result)
|
|
if (result == 1):
|
|
# 清空透传缓存
|
|
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
|
|
time.sleep(0.5)
|
|
else:
|
|
print("非缓存状态")
|
|
|
|
# 启动串口接收线程（daemon=True：主程序退出时线程自动终止）
|
|
receive_thread = threading.Thread(target=serial_receive_data, args=(ser,), daemon=True)
|
|
receive_thread.start()
|
|
print("串口接收线程已启动")
|
|
|
|
delay_time_10_13579_30 = 2
|
|
|
|
# 外层主循环
|
|
while True:
|
|
try:
|
|
# 等待主指令（从共享缓冲区读取）
|
|
print("\n等待串口指令...")
|
|
main_cmd_bytes = None
|
|
while main_cmd_bytes is None:
|
|
# 喷漆工艺参数配置选择（速度、角度、搭接时间）
|
|
selected_delay_map, selected_relay_config = handle_special_commands(PRESET_PARAMS)
|
|
if selected_delay_map is not None and selected_relay_config is not None:
|
|
# 更新delay配置
|
|
program_delay_map = selected_delay_map
|
|
# 更新继电器参数
|
|
turn_on_relay_start = selected_relay_config["turn_on_relay_start"]
|
|
turn_on_relay_end = selected_relay_config["turn_on_relay_end"]
|
|
turn_off_relay_start = selected_relay_config["turn_off_relay_start"]
|
|
turn_off_relay_end = selected_relay_config["turn_off_relay_end"]
|
|
ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"]
|
|
ex_turn_on_relay_end = selected_relay_config["ex_turn_on_relay_end"]
|
|
ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"]
|
|
ex_turn_off_relay_end = selected_relay_config["ex_turn_off_relay_end"]
|
|
|
|
print(f"\n✅ 参数已更新为选中配置：")
|
|
print(f"program_delay_map: {program_delay_map}")
|
|
print(f"继电器开启区间：{turn_on_relay_start} ~ {turn_on_relay_end}")
|
|
print(f"继电器关闭区间：{turn_off_relay_start} ~ {turn_off_relay_end}")
|
|
print(f"继电器开启区间2：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}")
|
|
print(f"继电器关闭区间2：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}")
|
|
# 喷漆开始命令接收函数
|
|
main_cmd_bytes = read_cmd_from_shared(timeout=1.0)
|
|
# 解析主指令（帧头后第一字节为指令值）
|
|
cmd = main_cmd_bytes[1]
|
|
print(f"收到串口指令: 0x{cmd:02X}")
|
|
|
|
# region 情况1
|
|
# 情况1: 接收到1
|
|
if cmd == 1:
|
|
print("触发情况1: 执行program1")
|
|
program_switch[1](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
|
|
time.sleep(delay_time_10_13579_30)
|
|
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.01)
|
|
print("执行program1结束,已发送响应0x80 0x80")
|
|
|
|
# 等待子指令接收
|
|
sub_cmd_bytes = None
|
|
while sub_cmd_bytes is None:
|
|
sub_cmd_bytes = read_cmd_from_shared(timeout=1.0)
|
|
time.sleep(0.01)
|
|
sub_cmd = sub_cmd_bytes[0] # 应为0xAA
|
|
sub_cmd1 = sub_cmd_bytes[1] # 子指令值（如0x20、0x0a）
|
|
print(f"收到子指令: 0x{sub_cmd:02X} 0x{sub_cmd1:02X}")
|
|
|
|
if sub_cmd == 0xAA:
|
|
if sub_cmd1 == 0x20:
|
|
print("开始情况1循环序列: program2->300->4->100...")
|
|
current_index = 0
|
|
running = True
|
|
while running:
|
|
# 执行当前序列中的程序
|
|
current_program = sequence_1[current_index]
|
|
#print(f"执行序列程序: {current_program}")
|
|
|
|
# 读取可能的停止指令（超时0.1秒，不阻塞循环）
|
|
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
|
|
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
|
|
print("收到0xAA 0x30,停止循环")
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.01)
|
|
running = False
|
|
time.sleep(0.5)
|
|
else:
|
|
running = program_switch[current_program](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
|
|
# print(current_program)
|
|
|
|
# 按程序编号从字典获取延时，精准执行
|
|
delay = program_delay_map.get(current_program, 0.0)
|
|
if delay > 0:
|
|
print(f"program{current_program}执行完成，延时{delay}秒...")
|
|
time.sleep(delay)
|
|
else:
|
|
print(f"当前执行程序是: {current_program}（无指定延时）")
|
|
|
|
# 移动到下一个程序索引
|
|
#current_index = (current_index + 1) % len(sequence_1) # 注意这里要换成当前使用的 sequence
|
|
# time.sleep(0.05)
|
|
# 回到洗枪位置指令
|
|
elif sub_cmd1 == 0x0a:
|
|
print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
|
|
program_switch[10](sock)
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.01)
|
|
print("执行program10结束,已发送响应0x80 0x80")
|
|
print("返回最外层等待串口数据")
|
|
|
|
else:
|
|
print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待")
|
|
#endregion
|
|
# region 情况3
|
|
# 情况3: 接收到5
|
|
elif cmd == 5:
|
|
print("触发情况3: 执行program5")
|
|
program_switch[5](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
|
|
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.01)
|
|
print("执行program5结束,已发送响应0x80 0x80")
|
|
|
|
# 子指令接收（调用read_cmd_from_shared）
|
|
print("等待0x20开始循环或其他指令...")
|
|
sub_cmd_bytes = None
|
|
while sub_cmd_bytes is None:
|
|
sub_cmd_bytes = read_cmd_from_shared(timeout=1.0)
|
|
time.sleep(0.01)
|
|
sub_cmd = sub_cmd_bytes[0]
|
|
sub_cmd1 = sub_cmd_bytes[1]
|
|
print(f"收到子指令: 0x{sub_cmd:02X}")
|
|
|
|
if sub_cmd == 0xAA:
|
|
if sub_cmd1 == 0x20:
|
|
print("开始情况3循环序列: program6->700->8->500...")
|
|
current_index = 0
|
|
running = True
|
|
while running:
|
|
current_program = sequence_3[current_index]
|
|
print(f"执行序列程序: {current_program}")
|
|
|
|
# 读取可能的停止指令（超时0.1秒，不阻塞循环）
|
|
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
|
|
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
|
|
print("收到0xAA 0x30,停止循环")
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.01)
|
|
running = False
|
|
time.sleep(0.5)
|
|
else:
|
|
running = program_switch[current_program](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
|
|
|
|
# 统一字典映射延时
|
|
delay = program_delay_map.get(current_program, 0.0)
|
|
if delay > 0:
|
|
print(f"program{current_program}执行完成，延时{delay}秒...")
|
|
time.sleep(delay)
|
|
else:
|
|
print(f"当前执行程序是: {current_program}（无指定延时）")
|
|
|
|
# 移动到下一个程序索引
|
|
#current_index = (current_index + 1) % len(sequence_3) # 注意这里要换成当前使用的 sequence
|
|
# time.sleep(0.05)
|
|
#回到洗枪位置指令
|
|
elif sub_cmd1 == 0x0a:
|
|
print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
|
|
program_switch[10](sock)
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.01)
|
|
print("执行program10结束,已发送响应0x80 0x80")
|
|
print("返回最外层等待串口数据")
|
|
else:
|
|
print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待")
|
|
#endregion
|
|
|
|
# region 程序9，返回停机位置
|
|
elif cmd == 9:
|
|
print("执行program9,返回停机位置")
|
|
program_switch[9](sock)
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.1)
|
|
print("执行program9结束,已发送响应0x80 0x80")
|
|
print("返回最外层等待串口数据")
|
|
#endregion
|
|
|
|
# region 程序10，前往初始位置
|
|
elif cmd == 10:
|
|
print("触发情况6: 执行program10")
|
|
program_switch[10](sock)
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.1)
|
|
print("执行program10结束,已发送响应0x80 0x80")
|
|
print("返回最外层等待串口数据")
|
|
# endregion
|
|
|
|
# region 程序11，前往洗枪位置
|
|
elif cmd == 11:
|
|
print("执行program11,前往洗枪位置")
|
|
program_switch[11](sock)
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.1)
|
|
print("执行program11结束,已发送响应0x80 0x80")
|
|
print("返回最外层等待串口数据")
|
|
#endregion
|
|
|
|
# region 程序12，前往试枪位置
|
|
elif cmd == 12:
|
|
print("执行program12，前往试枪位置")
|
|
program_switch[12](sock)
|
|
time.sleep(delay_time_10_13579_30)
|
|
for i in range(3):
|
|
ser.write(b'\x80\x80')
|
|
ser.flush()
|
|
time.sleep(0.1)
|
|
print("执行program12结束,已发送响应0x80 0x80")
|
|
print("返回最外层等待串口数据")
|
|
#endregion
|
|
|
|
# 退出指令
|
|
elif cmd == 0x00:
|
|
print("收到退出指令，程序终止")
|
|
break
|
|
# 开喷枪
|
|
elif cmd == 16:
|
|
turn_on_relay()
|
|
print("打开喷枪")
|
|
# 关喷枪
|
|
elif cmd == 17:
|
|
turn_off_relay()
|
|
print("关闭喷枪")
|
|
# 未知指令
|
|
else:
|
|
print(f"未知指令: 0x{cmd:02X}，等待下一条指令...")
|
|
time.sleep(0.01)
|
|
|
|
except KeyboardInterrupt:
|
|
print("程序被手动中断")
|
|
break
|
|
except Exception as e:
|
|
print(f"主循环错误: {e}")
|
|
time.sleep(0.5)
|
|
else:
|
|
print("连接机器人失败")
|
|
else:
|
|
print("无法连接到机器人")
|
|
|
|
|
|
|
|
# 资源清理
|
|
if sock:
|
|
sock.close()
|
|
if ser and isinstance(ser, serial.Serial) and ser.is_open:
|
|
ser.close()
|
|
print("串口已关闭")
|
|
|