制动性能测试和数据处理已完成

code/automatic_test/do_brake.py

@@ -4,33 +4,33 @@ import paramiko
 import openpyxl
 import pandas
 import json
-from commons import clibs
+from common import clibs, openapi
 
 
 def initialization(path, sub, data_dirs, data_files, hr, w2t):
     def check_files():
-        if len(data_dirs) != 0 or len(data_files) != 6:
-            w2t("初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！", "red")
-            w2t("1. configs.xlsx\n2. reach33/reach66/reach100_xxxx.xlsx\n3. xxxx.zip\n", "red", "InitFileError")
+        msg = "初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！\n"
+        msg += "1. configs.xlsx\n2. reach33/reach66/reach100_xxxx.xlsx\n3. xxxx.zip\n"
+        if len(data_dirs) != 0 or len(data_files) != 5:
+            w2t(msg, "red", "InitFileError")
 
-        config_file, reach33, reach66, reach100, prj_file, result_dirs = None, None, None, None, None, []
+        config_file, reach33_file, reach66_file, reach100_file, prj_file, result_dirs = None, None, None, None, None, []
         for data_file in data_files:
             filename = data_file.split("/")[-1]
             if filename == "configs.xlsx":
                 config_file = data_file
             elif filename.startswith("reach33_") and filename.endswith(".xlsx"):
-                reach33 = data_file
+                reach33_file = data_file
             elif filename.startswith("reach66_") and filename.endswith(".xlsx"):
-                reach66 = data_file
+                reach66_file = data_file
             elif filename.startswith("reach100_") and filename.endswith(".xlsx"):
-                reach100 = data_file
+                reach100_file = data_file
             elif filename.endswith(".zip"):
                 prj_file = data_file
             else:
-                w2t("初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！", "red")
-                w2t("1. configs.xlsx\n2. reach33/reach66/reach100_xxxx.xlsx\n3. xxxx.zip\n", "red", "InitFileError")
+                w2t(msg, "red", "InitFileError")
 
-        if config_file and reach33 and reach66 and reach100 and prj_file:
+        if config_file and reach33_file and reach66_file and reach100_file and prj_file:
             os.mkdir(f"{path}/j1")
             os.mkdir(f"{path}/j2")
             os.mkdir(f"{path}/j3")
@@ -46,10 +46,9 @@ def initialization(path, sub, data_dirs, data_files, hr, w2t):
                         os.mkdir(f"{path}/j3/{dir_name}")
 
             w2t("数据目录合规性检查结束，未发现问题......\n", "blue")
-            return config_file, reach33, reach66, reach100, prj_file, result_dirs
+            return config_file, prj_file, result_dirs
         else:
-            w2t("初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！", "red")
-            w2t("1. configs.xlsx\n2. reach33/reach66/reach100_xxxx.xlsx\n3. xxxx.zip\n", "red", "InitFileError")
+            w2t(msg, "red", "InitFileError")
 
     def get_configs():
         robot_type = None
@@ -76,40 +75,45 @@ def initialization(path, sub, data_dirs, data_files, hr, w2t):
         clibs.insert_logdb("INFO", "do_brake", f"get_configs: 各关节角速度 {avs}")
         return avs
 
-    _config_file, _reach33, _reach66, _reach100, _prj_file, _result_dirs = check_files()
+    clibs.c_hr.set_socket_params(True, str(clibs.external_port), "\r", 1)
+    clibs.c_ec = openapi.ExternalCommunication(clibs.ip_addr, clibs.external_port)
+
+    _config_file, _prj_file, _result_dirs = check_files()
     _avs = get_configs()
 
-    return _config_file, _reach33, _reach66, _reach100, _prj_file, _result_dirs, _avs
+    return _config_file, _prj_file, _result_dirs, _avs
 
 
 @clibs.db_lock
-def gen_result_file(path, axis, reach, load, speed, rounds):
+def gen_result_file(path, axis, t_end, reach, load, speed, rounds):
     d_vel, d_trq, d_stop = [], [], []
 
-    len_records = 12 * 1000 // 50
-    clibs.cursor.execute(f"select content from logs where content like 'diagnosis.result' limit {len_records}")
+    start_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(t_end-12))
+    end_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(t_end))
+    clibs.cursor.execute(f"select content from logs where time between '{start_time}' and '{end_time}' and content like '%diagnosis.result%' order by id asc")
     records = clibs.cursor.fetchall()
 
     for record in records:  # 保留最后12s的数据
-        data = eval(record)["data"]
+        data = eval(record[0])["data"]
         for item in data:
+            d_item = reversed(item["value"])
             if item.get("channel", None) == axis-1 and item.get("name", None) == "hw_joint_vel_feedback":
-                d_vel.extend(item["value"])
+                d_vel.extend(d_item)
             elif item.get("channel", None) == axis-1 and item.get("name", None) == "device_servo_trq_feedback":
-                d_trq.extend(item["value"])
+                d_trq.extend(d_item)
             elif item.get("channel", None) == 0 and item.get("name", None) == "device_safety_estop":
-                d_stop.extend(item["value"])
+                d_stop.extend(d_item)
 
     df1 = pandas.DataFrame.from_dict({"hw_joint_vel_feedback": d_vel})
     df2 = pandas.DataFrame.from_dict({"device_servo_trq_feedback": d_trq})
     df3 = pandas.DataFrame.from_dict({"device_safety_estop": d_stop})
     df = pandas.concat([df1, df2, df3], axis=1)
-    filename = f"{path}\\j{axis}\\reach{reach}_load{load}_speed{speed}\\reach{reach}_load{load}_speed{speed}_{rounds}.data"
+    filename = f"{path}/j{axis}/reach{reach}_load{load}_speed{speed}/reach{reach}_load{load}_speed{speed}_{rounds}.data"
     df.to_csv(filename, sep="\t", index=False)
 
 
 def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
-    count, total = 0, 63
+    count, total, speed_target = 0, 63, 0
     prj_name = prj_file.split("/")[-1].split(".")[0]
     display_pdo_params = [{"name": name, "channel": chl} for name in ["hw_joint_vel_feedback", "device_servo_trq_feedback"] for chl in range(6)]
     display_pdo_params.append({"name": "device_safety_estop", "channel": 0})
@@ -119,7 +123,9 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
     get_init_speed = float(ws.cell(row=3, column=2).value)
     single_brake = str(ws.cell(row=4, column=2).value)
     pon = ws.cell(row=5, column=2).value
-    w2t(f"write_diagnosis = {write_diagnosis}, get_init_speed = {get_init_speed}, single_brake = {single_brake}\n")
+    io_name = ws.cell(row=6, column=2).value.upper()
+    wb.close()
+    w2t(f"基本参数配置：write_diagnosis = {write_diagnosis}, get_init_speed = {get_init_speed}, single_brake = {single_brake}, pon = {pon}\n")
 
     if pon == "positive":
         md.write_pon(1)
@@ -136,7 +142,7 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
         speed = condition.split("_")[2].removeprefix("speed")
 
         # for single condition test
-        single_axis = 0
+        single_axis = -1
         if single_brake != "0":
             total = 3
             single_axis = int(single_brake.split("-")[0])
@@ -145,25 +151,28 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
 
         for axis in range(1, 4):
             # for single condition test
-            if (single_axis != 0 and single_axis != axis) or (axis == 3 and reach != "100"):
+            if (single_axis != -1 and single_axis != axis) or (axis == 3 and reach != "100"):
                 continue
 
             md.write_axis(axis)
             w2t(f"-"*90+"\n", "purple")
             for rounds in range(1, 4):
                 count += 1
+                _ = 3 if count % 3 == 0 else count % 3
                 this_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
                 prj_path = f"{prj_name}/_build/{prj_name}.prj"
-                w2t(f"[{this_time} | {count}/{total}] 正在执行 {axis} 轴 {condition} 的第 {count} 次制动测试...\n")
+                w2t(f"[{this_time} | {count}/{total}] 正在执行 {axis} 轴 {condition} 的第 {_} 次制动测试...\n")
 
                 # 1. 触发软急停，并解除，目的是让可能正在运行着的机器停下来，切手动模式并下电
-                md.trigger_estop()
-                md.reset_estop()
-                md.clear_alarm()
+                md.r_soft_estop(0)
+                md.r_soft_estop(1)
+                clibs.c_ec.setdo_value(io_name, "true")
+                md.r_reset_estop()
+                md.r_clear_alarm()
                 md.write_act(0)
-                time.sleep(write_diagnosis)  # 软急停超差后，等待写诊断时间，可通过configs.xlsx配置
+                time.sleep(write_diagnosis)  # 急停超差后，等待写诊断时间，可通过configs.xlsx配置，2.3 版本之后设置为 0 即可
 
-                while count == 1:
+                while count % 3 == 1:
                     # 2. 修改要执行的场景
                     rl_cmd = ""
                     ssh = paramiko.SSHClient()
@@ -176,24 +185,24 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
                     rl_speed = f"VelSet {speed}"
                     rl_tool = f"tool p_tool = tool{sub.removeprefix("tool")}"
                     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 += '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; '
-                    cmd += 'sudo sed -i "/tool p_tool/d" projects/target/_build/brake/main.mod; '
-                    cmd += f'sudo sed -i "/VelSet/i {rl_tool}" projects/target/_build/brake/main.mod; '
+                    cmd += f'sudo sed -i "/brake_E/d" projects/{prj_name}/_build/brake/main.mod; '
+                    cmd += f'sudo sed -i "/DONOTDELETE/i {rl_cmd}" projects/{prj_name}/_build/brake/main.mod; '
+                    cmd += f'sudo sed -i "/VelSet/d" projects/{prj_name}/_build/brake/main.mod; '
+                    cmd += f'sudo sed -i "/MoveAbsJ/i {rl_speed}" projects/{prj_name}/_build/brake/main.mod; '
+                    cmd += f'sudo sed -i "/tool p_tool/d" projects/{prj_name}/_build/brake/main.mod; '
+                    cmd += f'sudo sed -i "/VelSet/i {rl_tool}" projects/{prj_name}/_build/brake/main.mod; '
                     stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True)
                     stdin.write(clibs.password + "\n")
                     stdout.read().decode()  # 需要read一下才能正常执行
                     stderr.read().decode()
 
                     # 3. reload工程后，pp2main，并且自动模式和上电，最后运行程序
-                    hr.execution("overview.reload", prj_path=prj_path, tasks=["brake", "stop0_related"])
-                    hr.execution("rl_task.pp_to_main", tasks=["brake", "stop0_related"])
+                    hr.execution("overview.reload", prj_path=prj_path, tasks=["brake"])
+                    hr.execution("rl_task.pp_to_main", tasks=["brake"])
                     hr.execution("state.switch_auto")
                     hr.execution("state.switch_motor_on")
                     hr.execution("rl_task.set_run_params", loop_mode=True, override=1.0)
-                    hr.execution("rl_task.run", tasks=["brake", "stop0_related"])
+                    hr.execution("rl_task.run", tasks=["brake"])
                     t_start = time.time()
                     while True:
                         if md.read_ready_to_go() == 1:
@@ -201,21 +210,24 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
                             break
                         else:
                             time.sleep(1)
-                            if (time.time() - t_start) // 20 > 1:
+                            if (time.time() - t_start) > 20:
                                 w2t("20s 内未收到机器人的运行信号，需要确认 RL 程序编写正确并正常执行...", "red", "ReadySignalTimeoutError")
                 # 4. 找出最大速度，传递给RL程序，最后清除相关记录
+                    time.sleep(10)  # 消除前 10s 的不稳定数据
+                    start_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
                     time.sleep(get_init_speed)  # 指定时间后获取实际【正|负】方向的最大速度，可通过configs.xlsx配置
-                    clibs.execution("rl_task.stop", tasks=["brake"])
+                    end_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
+                    hr.execution("rl_task.stop", tasks=["brake"])
+                    time.sleep(5)  # 确保数据都拿到
 
                     # 找出最大速度
                     @clibs.db_lock
                     def get_speed_max():
                         _speed_max = 0
-                        len_records = int(get_init_speed * 20) + 1  # 1000 / 50 = 20
-                        clibs.cursor.execute(f"select content from logs where content like 'diagnosis.result' limit {len_records}")
+                        clibs.cursor.execute(f"select content from logs where time between '{start_time}' and '{end_time}' and content like '%diagnosis.result%' order by id asc")
                         records = clibs.cursor.fetchall()
                         for record in records:
-                            data = eval(record)["data"]
+                            data = eval(record[0])["data"]
                             for item in data:
                                 if item.get("channel", None) == axis-1 and item.get("name", None) == "hw_joint_vel_feedback":
                                     _ = clibs.RADIAN * sum(item["value"]) / len(item["value"])
@@ -234,20 +246,17 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
                     md.write_speed_max(speed_max)
 
                     if speed_max < 10:
-                        md.clear_alarm()
+                        md.r_clear_alarm()
                         w2t("未获取到正确的速度，即将重新获取...\n", "red")
                         continue
                     else:
                         break
 
-                # 5. 清除软急停，重新运行程序，并打开曲线发送继续运动信号，当速度达到最大值时，通过DO触发急停
-                md.reset_estop()  # 其实没必要
-                md.clear_alarm()
-
-                hr.execution("rl_task.pp_to_main", tasks=["brake", "stop0_related"])
+                # 5. 重新运行程序，发送继续运动信号，当速度达到最大值时，通过DO触发急停
+                hr.execution("rl_task.pp_to_main", tasks=["brake"])
                 hr.execution("state.switch_auto")
                 hr.execution("state.switch_motor_on")
-                hr.execution("rl_task.run", tasks=["brake", "stop0_related"])
+                hr.execution("rl_task.run", tasks=["brake"])
                 for i in range(3):
                     if md.read_ready_to_go() == 1:
                         md.write_act(1)
@@ -258,21 +267,38 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
                     w2t("3s 内未收到机器人的运行信号，需要确认 RL 程序配置正确并正常执行...", "red", "ReadySignalTimeoutError")
 
                 time.sleep(10)  # 排除从其他位姿到零点位姿，再到轴极限位姿的时间
-                md.write_probe(1)
-                t_start = time.time()
-                while True:
-                    if md.read_brake_done() == 1:
-                        time.sleep(4)  # 保证速度归零
-                        md.write_probe(0)
-                        break
-                    else:
-                        time.sleep(1)
-                        if (time.time() - t_start) > 30:
-                            md.write_probe(0)
-                            w2t(f"30s 内未触发急停，该条数据无效，需要确认 RL/Python 程序配置正确并正常执行，或者判别是否是机器本体问题，比如正负方向速度是否一致...", "red", "NoEstopTriggeredError")
 
+                def exec_brake():
+                    flag, start, data, record = True, time.time(), None, None
+                    while flag:
+                        time.sleep(0.2)
+                        if time.time() - start > 20:
+                            w2t("20s 内未触发急停，需排查......", "red", "BrakeTimeoutError")
+
+                        try:
+                            clibs.lock.acquire(True)
+                            clibs.cursor.execute(f"select content from logs where content like '%diagnosis.result%' order by id desc limit 1")
+                            record = clibs.cursor.fetchone()
+                            data = eval(record[0])["data"]
+                        finally:
+                            clibs.lock.release()
+
+                        for item in data:
+                            if item.get("channel", None) != axis-1 or item.get("name", None) != "hw_joint_vel_feedback":
+                                continue
+
+                            speed_moment = clibs.RADIAN * sum(item["value"]) / len(item["value"])
+                            if (pon == "positive" and speed_moment > 0) or (pon == "negative" and speed_moment < 0):
+                                if abs(speed_moment) > speed_target * 0.95:
+                                    clibs.c_ec.setdo_value(io_name, "false")
+                                    time.sleep(5)
+                                    flag = False
+                                    break
+                    return time.time()
+
+                t_end = exec_brake()
                 # 6. 保留数据并处理输出
-                gen_result_file(path, axis, reach, load, speed, rounds)
+                gen_result_file(path, axis, t_end, reach, load, speed, rounds)
     else:
         w2t(f"\n{sub.removeprefix("tool")}%负载的制动性能测试执行完毕，如需采集其他负载，须切换负载类型，并更换其他负载，重新执行。\n", "green")
         hr.execution("diagnosis.open", open=False, display_open=False, overrun=True, turn_area=True, delay_motion=False)
@@ -280,21 +306,22 @@ def run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t):
 
 
 def main():
-    # path, hr, md, loadsel, w2t
+    s_time = time.time()
     path = clibs.data_at["_path"]
     sub = clibs.data_at["_sub"]
     w2t = clibs.w2t
     hr = clibs.c_hr
     md = clibs.c_md
 
-    s_time = time.time()
     data_dirs, data_files = clibs.traversal_files(path, w2t)
-    config_file, reach33, reach66, reach100, prj_file, result_dirs, avs = initialization(path, sub, data_dirs, data_files, hr, w2t)
+    config_file, prj_file, result_dirs, avs = initialization(path, sub, data_dirs, data_files, hr, w2t)
     clibs.c_pd.push_prj_to_server(prj_file)
     run_rl(path, sub, hr, md, config_file, prj_file, result_dirs, avs, w2t)
+
     e_time = time.time()
     time_total = e_time - s_time
-    w2t(f"处理总时长：{time_total // 3600:02.0f} h {time_total % 3600 // 60:02.0f} m {time_total % 60:02.0f} s", "green")
+    w2t(f"-" * 90 + "\n", "purple")
+    w2t(f"处理总时长：{time_total // 3600:02.0f} h {time_total % 3600 // 60:02.0f} m {time_total % 60:02.0f} s\n", "green")
 
 
 if __name__ == "__main__":

code/automatic_test/do_current.py

@@ -201,7 +201,7 @@ def run_rl(path, prj_file, hr, md, sub, w2t):
         # 4. 执行采集
         time.sleep(10)  # 消除前 10s 的不稳定数据
         start_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
-        single_time, stall_time, scenario_time = 30, 10, 0
+        single_time, stall_time, scenario_time = 40, 10, 0
         if number < 6:  # 单轴
             time.sleep(single_time)
         elif number < 12:  # 堵转
@@ -234,6 +234,7 @@ def run_rl(path, prj_file, hr, md, sub, w2t):
 
 
 def main():
+    s_time = time.time()
     path = clibs.data_at["_path"]
     sub = clibs.data_at["_sub"]
     w2t = clibs.w2t
@@ -245,6 +246,11 @@ def main():
     clibs.c_pd.push_prj_to_server(prj_file)
     run_rl(path, prj_file, hr, md, sub, w2t)
 
+    e_time = time.time()
+    time_total = e_time - s_time
+    w2t(f"-" * 90 + "\n", "purple")
+    w2t(f"处理总时长：{time_total // 3600:02.0f} h {time_total % 3600 // 60:02.0f} m {time_total % 60:02.0f} s", "green")
+
 
 if __name__ == "__main__":
     main()