diff --git a/aio/code/automatic_test/do_brake.py b/aio/code/automatic_test/do_brake.py index 7da5cc1..3fe1feb 100644 --- a/aio/code/automatic_test/do_brake.py +++ b/aio/code/automatic_test/do_brake.py @@ -1,3 +1,4 @@ +import random from time import sleep, time from sys import argv from os import scandir, mkdir @@ -149,13 +150,30 @@ def gen_result_file(path, curve_data, axis, _reach, _load, _speed, count): def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t): _count = 0 + speed_max = 0 + display_pdo_params = [ + {"name": "hw_joint_vel_feedback", "channel": 0}, + {"name": "hw_joint_vel_feedback", "channel": 1}, + {"name": "hw_joint_vel_feedback", "channel": 2}, + {"name": "hw_joint_vel_feedback", "channel": 3}, + {"name": "hw_joint_vel_feedback", "channel": 4}, + {"name": "hw_joint_vel_feedback", "channel": 5}, + {"name": "device_servo_trq_feedback", "channel": 0}, + {"name": "device_servo_trq_feedback", "channel": 1}, + {"name": "device_servo_trq_feedback", "channel": 2}, + {"name": "device_servo_trq_feedback", "channel": 3}, + {"name": "device_servo_trq_feedback", "channel": 4}, + {"name": "device_servo_trq_feedback", "channel": 5}, + {"name": "device_safety_estop", "channel": 0}, + ] for condition in result_dirs: _reach = condition.split('_')[0].removeprefix('reach') _load = condition.split('_')[1].removeprefix('load') _speed = condition.split('_')[2].removeprefix('speed') for axis in range(1, 4): - _get_speed_max = 0 + md.write_axis(axis) + speed_max = 0 if axis == 3 and _reach != '100': continue for count in range(1, 4): @@ -163,39 +181,37 @@ def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t): w2t(f"[{_count}/64-{count}] 正在执行{axis}轴{condition}的制动测试......", 0, 0, 'purple', 'Automatic Test') # 1. 关闭诊断曲线，触发软急停，并解除，目的是让可能正在运行着的机器停下来，切手动模式并下电 - _response = execution('diagnosis.open', hr, w2t, open=False, display_open=False) - md.set_stop0(1) + md.trigger_estop() md.reset_estop() + _response = execution('diagnosis.open', hr, w2t, open=False, display_open=False) sleep(2) # 让曲线彻底关闭 _response = execution('state.switch_manual', hr, w2t) _response = execution('state.switch_motor_off', hr, w2t) # 2. 修改未要执行的场景 - ssh = SSHClient() - ssh.set_missing_host_key_policy(AutoAddPolicy()) - ssh.connect('192.168.0.160', 22, username='luoshi', password='luoshi2019') - _rl_cmd = f"brake_E(j{axis}_{_reach}_p, j{axis}_{_reach}_n, p_speed, p_tool)" - _rl_speed = f"VelSet {_speed}" - cmd = 'cd /home/luoshi/bin/controller/; ' - cmd += 'sudo sed -i "/brake_E/d" projects/target/_build/brake/main.mod; ' - cmd += f'sudo sed -i "/DONOTDELETE/i {_rl_cmd}" projects/target/_build/brake/main.mod; ' - cmd += f'sudo sed -i "/VelSet/d" projects/target/_build/brake/main.mod; ' - cmd += f'sudo sed -i "/MoveAbsJ/i {_rl_speed}" projects/target/_build/brake/main.mod; ' - stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True) - stdin.write('luoshi2019' + '\n') - stdin.flush() - print(stdout.read().decode()) # 必须得输出一下stdout，才能正确执行sudo - print(stderr.read().decode()) # 顺便也执行以下stderr + if count == 1: + ssh = SSHClient() + ssh.set_missing_host_key_policy(AutoAddPolicy()) + ssh.connect('192.168.0.160', 22, username='luoshi', password='luoshi2019') + _rl_cmd = f"brake_E(j{axis}_{_reach}_p, j{axis}_{_reach}_n, p_speed, p_tool)" + _rl_speed = f"VelSet {_speed}" + cmd = 'cd /home/luoshi/bin/controller/; ' + cmd += 'sudo sed -i "/brake_E/d" projects/target/_build/brake/main.mod; ' + cmd += f'sudo sed -i "/DONOTDELETE/i {_rl_cmd}" projects/target/_build/brake/main.mod; ' + cmd += f'sudo sed -i "/VelSet/d" projects/target/_build/brake/main.mod; ' + cmd += f'sudo sed -i "/MoveAbsJ/i {_rl_speed}" projects/target/_build/brake/main.mod; ' + stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True) + stdin.write('luoshi2019' + '\n') + stdin.flush() + print(stdout.read().decode()) # 必须得输出一下stdout，才能正确执行sudo + print(stderr.read().decode()) # 顺便也执行以下stderr - # 3. reload工程后，pp2main，并且自动模式和上电 + # 3. reload工程后，pp2main，并且自动模式和上电，最后运行程序 prj_path = 'target/_build/target.prj' _response = execution('overview.reload', hr, w2t, prj_path=prj_path, tasks=['brake', 'stop0_related']) - _response = execution('overview.get_cur_prj', hr, w2t) _response = execution('rl_task.pp_to_main', hr, w2t, tasks=['brake', 'stop0_related']) _response = execution('state.switch_auto', hr, w2t) _response = execution('state.switch_motor_on', hr, w2t) - - # 4. 第一次开始运行程序，运行两轮 _response = execution('rl_task.run', hr, w2t, tasks=['brake', 'stop0_related']) for i in range(3): if md.read_ready_to_go() == 1: @@ -208,33 +224,16 @@ def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t): else: w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test') - if _get_speed_max == 0: - # 5. 第一次打开诊断曲线，并执行采集 - _get_speed_max = 1 + # 4. 第一次打开诊断曲线，并执行采集8s，之后触发软急停，关闭曲线采集，找出最大速度，传递给RL程序，最后清除相关记录 + if count == 1: + sleep(4) # 排除从零点位置到目标位置的干扰 _response = execution('diagnosis.open', hr, w2t, open=True, display_open=True) - display_pdo_params = [ - {"name": "hw_joint_vel_feedback", "channel": 0}, - {"name": "hw_joint_vel_feedback", "channel": 1}, - {"name": "hw_joint_vel_feedback", "channel": 2}, - {"name": "hw_joint_vel_feedback", "channel": 3}, - {"name": "hw_joint_vel_feedback", "channel": 4}, - {"name": "hw_joint_vel_feedback", "channel": 5}, - {"name": "device_servo_trq_feedback", "channel": 0}, - {"name": "device_servo_trq_feedback", "channel": 1}, - {"name": "device_servo_trq_feedback", "channel": 2}, - {"name": "device_servo_trq_feedback", "channel": 3}, - {"name": "device_servo_trq_feedback", "channel": 4}, - {"name": "device_servo_trq_feedback", "channel": 5}, - {"name": "device_safety_estop", "channel": 0}, - ] _response = execution('diagnosis.set_params', hr, w2t, display_pdo_params=display_pdo_params) sleep(8) # 前八秒获取实际最大速度 - # 6. 关闭诊断曲线，并处理数据，找出最大速度 - _response = execution('rl_task.stop', hr, w2t, tasks=['brake']) + md.trigger_estop() _response = execution('diagnosis.open', hr, w2t, open=False, display_open=False) # 找出最大速度 - speed_max = 0 for _msg in hr.c_msg: if 'diagnosis.result' in _msg: dict_results = loads(_msg)['data'] @@ -244,71 +243,60 @@ def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t): speed_max = max(_, speed_max) wb = load_workbook(config_file, read_only=True) ws = wb['Target'] - speed_target = float(ws.cell(row=3, column=axis+1).value) - if speed_max < speed_target*0.95: + speed_target = float(ws.cell(row=3, column=axis+1).value) * float(_speed) / 100 + print(f"speed target = {speed_target}") + print(f"speed max = {speed_max}") + if speed_max < speed_target*0.95 or speed_max > speed_target*1.05: w2t(f"Axis: {axis}-{count} | Speed: {speed_max*RADIAN} | Shouldbe: {speed_target}", 0, 0, 'indigo') - # 7. 清零c_msg，重新运行程序，并打开曲线发送继续运动信号，开始运动，当速度达到最大值时，通过DO触发急停 - for _msg in hr.c_msg: - if 'diagnosis.result' in _msg: - _index = hr.c_msg.index(_msg) - del hr.c_msg[_index:] - hr.c_msg_xs.clear() - break + md.write_speed_max(speed_max) + sleep(1) - _response = execution('rl_task.pp_to_main', hr, w2t, tasks=['brake', 'stop0_related']) - _response = execution('state.switch_auto', hr, w2t) - _response = execution('state.switch_motor_on', hr, w2t) - _response = execution('rl_task.run', hr, w2t, tasks=['brake', 'stop0_related']) - for i in range(3): - if md.read_ready_to_go() == 1: - md.write_act(True) - sleep(1) - md.write_act(False) - break + for _msg in hr.c_msg: + if 'diagnosis.result' in _msg: + _index = hr.c_msg.index(_msg) + del hr.c_msg[_index:] + hr.c_msg_xs.clear() + break + + # 5. 清除软急停，重新运行程序，并打开曲线发送继续运动信号，当速度达到最大值时，通过DO触发急停 + if count == 1: + md.reset_estop() + _response = execution('overview.reload', hr, w2t, prj_path=prj_path, tasks=['brake', 'stop0_related']) + _response = execution('rl_task.pp_to_main', hr, w2t, tasks=['brake', 'stop0_related']) + _response = execution('state.switch_auto', hr, w2t) + _response = execution('state.switch_motor_on', hr, w2t) + _response = execution('rl_task.run', hr, w2t, tasks=['brake', 'stop0_related']) + for i in range(3): + if md.read_ready_to_go() == 1: + md.write_act(True) + sleep(1) + md.write_act(False) + break + else: + sleep(1) else: - sleep(1) - else: - w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test') + w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test') _response = execution('diagnosis.open', hr, w2t, open=True, display_open=True) _response = execution('diagnosis.set_params', hr, w2t, display_pdo_params=display_pdo_params) - sleep(4) - _flg = 1 + sleep(random.randint(2, 8)) + md.write_probe(True) _t_start = time() - while _flg: - sleep(0.05) - for _msg in hr.c_msg: - if 'diagnosis.result' in _msg: - if _flg == 0: - break - dict_results = loads(_msg)['data'] - for item in dict_results: - if item.get('channel', None) == axis-1 and item.get('name', None) == 'hw_joint_vel_feedback': - _ = abs(RADIAN*sum(item['value'])/len(item['value'])) - if abs(_-speed_max) < 5: - print(f"speed actual = {_}") - print(f"speed max = {speed_max}") - print(f"speed target = {speed_target*float(_speed)/100}") - print(f"abs minus = {abs(_-speed_max)}") - for _i in range(5): - # md.set_stop0(0) # 触发IO急停 - _response = execution('register.set_value', hr, w2t, name='stop0_signal', type='bool', bias=0, value=0) - sleep(0.02) - _flg = 0 - break - else: - if (time() - _t_start) // 60 > 1: - w2t(f"规定时间内未找到合适的点触发急停，需要确认RL/Python程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test') - else: - break + while True: + if md.read_brake_done() == 1: + sleep(1) # 保证所有数据均已返回 + md.write_probe(False) + _response = execution('diagnosis.open', hr, w2t, open=False, display_open=False) + sleep(1) # 保证所有数据均已返回 + break + else: + if (time() - _t_start) // 60 > 1: + w2t(f"规定时间内未找到合适的点触发急停，需要确认RL/Python程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test') + else: + sleep(1) - # 8. 关闭诊断曲线，且换成手动模式 - sleep(1) # 保证所有数据均已返回 - _response = execution('diagnosis.open', hr, w2t, open=False, display_open=False) - _response = execution('state.switch_manual', hr, w2t) - - # 9. 保留数据并处理输出 + # 6. 保留数据并处理输出 curve_data = [] for _msg in hr.c_msg: if 'diagnosis.result' in _msg: diff --git a/aio/code/automatic_test/openapi.py b/aio/code/automatic_test/openapi.py index 020cabd..703e9c7 100644 --- a/aio/code/automatic_test/openapi.py +++ b/aio/code/automatic_test/openapi.py @@ -5,7 +5,7 @@ import selectors from time import time, sleep from os.path import dirname from pymodbus.client.tcp import ModbusTcpClient -from pymodbus.payload import BinaryPayloadDecoder +from pymodbus.payload import BinaryPayloadDecoder, BinaryPayloadBuilder from pymodbus.constants import Endian MAX_FRAME_SIZE = 1024 @@ -125,13 +125,48 @@ class ModbusRequest(object): self.w2t(f"{Err}") self.w2t("无法读取准备信号，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name) - def set_stop0(self, number): + def write_stop0(self, number): try: self.c.write_register(41004, number) except Exception as Err: self.w2t(f"{Err}") self.w2t("无法通过IO操作stop0急停，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name) + def write_speed_max(self, speed): + try: + builder = BinaryPayloadBuilder(byteorder=Endian.BIG, wordorder=Endian.LITTLE) + builder.add_32bit_float(float(speed)) + payload = builder.build() + self.c.write_registers(41005, payload, skip_encode=True) + except Exception as Err: + self.w2t(f"{Err}") + self.w2t("无法写入速度值，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name) + + def read_brake_done(self): + try: + results = self.c.read_holding_registers(41007, 1) + return results.registers[0] + except Exception as Err: + self.w2t(f"{Err}") + self.w2t("无法读取制动已执行信号，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name) + + def write_axis(self, axis): + try: + builder = BinaryPayloadBuilder(byteorder=Endian.BIG, wordorder=Endian.LITTLE) + builder.add_32bit_int(int(axis)) + payload = builder.to_registers() + self.c.write_registers(41008, payload) + except Exception as Err: + self.w2t(f"{Err}") + self.w2t("无法写入速度值，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name) + + def write_probe(self, probe): + try: + self.c.write_register(41010, probe) + except Exception as Err: + self.w2t(f"{Err}") + self.w2t("无法写入速度探测信号，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name) + class HmiRequest(object): def __init__(self, w2t): @@ -188,7 +223,8 @@ class HmiRequest(object): md.reset_estop() md.clear_alarm() md.write_act(False) - md.set_stop0(1) + md.write_probe(False) + md.write_axis(1) except Exception as Err: self.w2t("Connection failed...", 0, 0, 'red', tab_name=self.tab_name) with open(f"{current_path}/../../assets/templates/heartbeat", "w", encoding='utf-8') as f_hb: @@ -233,7 +269,7 @@ class HmiRequest(object): f_hb.write(_flag) if _flag == '0': self.w2t(f"{_id} 心跳丢失，连接失败，重新连接中...", 0, 7, 'red', tab_name=self.tab_name) - sleep(2) + sleep(1.5) # with open(f"{current_path}/../../assets/templates/c_msg.log", "w", encoding='utf-8') as f: # for msg in self.c_msg: # f.write(str(loads(msg)) + '\n')