From b1322495b06885df2e4437471938bead858ec3b8 Mon Sep 17 00:00:00 2001 From: "Ziwei.He" Date: Mon, 19 May 2025 10:42:57 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=9B=E6=8E=A7=E9=83=A8=E5=88=86=E5=BC=80?= =?UTF-8?q?=E5=8F=91=E5=AE=8C=E6=88=90=EF=BC=8C=E5=90=8E=E7=BB=AD=E8=B0=83?= =?UTF-8?q?=E6=95=B4=E5=BA=95=E5=B1=82=E6=95=B0=E5=AD=A6=E5=8D=95=E4=BD=8D?= =?UTF-8?q?=E5=90=8E=EF=BC=8C=E5=8F=AF=E5=BC=80=E5=8F=91=E4=B8=8A=E5=B1=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Massage/MassageControl/MassageRobot_nova5.py | 146 +- .../admittance_controller.cpython-39.pyc | Bin 3705 -> 3564 bytes .../position_controller.cpython-39.pyc | Bin 2439 -> 2439 bytes .../algorithms/admittance_controller.py | 45 +- .../algorithms/position_controller.py | 2 +- Massage/MassageControl/config/admittance.yaml | 12 +- .../MassageControl/config/robot_config.yaml | 6 +- .../__pycache__/dobot_nova5.cpython-39.pyc | Bin 15043 -> 15436 bytes .../__pycache__/force_sensor.cpython-39.pyc | Bin 15294 -> 10816 bytes .../MassageControl/hardware/dobot_nova5.py | 66 +- .../MassageControl/hardware/force_sensor.py | 796 +- .../logs/MassageRobot_nova5_test.log | 0 .../__pycache__/serial_tools.cpython-39.pyc | Bin 0 -> 2513 bytes .../__pycache__/ssh_tools.cpython-39.pyc | Bin 0 -> 671 bytes logs/MassageRobot_nova5_test.log | 8723 +++++++++++++++++ logs/test.py | 5 + 16 files changed, 9233 insertions(+), 568 deletions(-) create mode 100644 Massage/MassageControl/logs/MassageRobot_nova5_test.log create mode 100644 Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc create mode 100644 Massage/MassageControl/tools/__pycache__/ssh_tools.cpython-39.pyc create mode 100644 logs/test.py diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 1c8b31a..b95eab0 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -36,7 +36,7 @@ class MassageRobot: self.arm = dobot_nova5(self.arm_config['arm_ip']) self.arm.start() # 传感器 - self.force_sensor = XjcSensor(self.arm_config['arm_ip'],60000) + self.force_sensor = XjcSensor() self.force_sensor.connect() # 控制器初始化(初始化为导纳控制) self.controller_manager = ControllerManager(self.arm_state) @@ -135,6 +135,7 @@ class MassageRobot: b_rotation_s = R.from_quat(self.arm_state.arm_orientation).as_matrix() # 读取数据 sensor_data = self.force_sensor.read() + # self.logger.log_info(f"传感器数据{sensor_data}") if sensor_data is None: self.force_sensor.stop_background_reading() self.logger.log_error("传感器数据读取失败") @@ -153,20 +154,6 @@ class MassageRobot: wrench = self.wrench_coordinate_conversion(self.s_tf_matrix_t,wrench) # 交给ARM STATE集中管理 self.arm_state.external_wrench_tcp = wrench - # self.arm_state.external_wrench_base = np.concatenate([b_rotation_s @ self.arm_state.external_wrench_tcp[:3], - # b_rotation_s @ self.arm_state.external_wrench_tcp[3:]]) - - # 卡尔曼滤波 - - # x_base_predict, P_base_predict = self.kalman_predict(x = self.x_base, - # P = self.P_base, - # Q = self.Q_base) - # self.x_base, self.P_base = self.kalman_update(x_predict = x_base_predict, - # P_predict = P_base_predict, - # z = self.arm_state.external_wrench_base, - # R = self.R_base) - # self.arm_state.external_wrench_base = self.x_base - # self.arm_state.last_external_wrench_base = self.arm_state.external_wrench_base # 对tcp坐标系下的外力外矩进行平滑 x_tcp_predict, P_tcp_predict = self.kalman_predict(x = self.x_tcp, P = self.P_tcp, @@ -179,6 +166,70 @@ class MassageRobot: self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp return 0 + # def arm_measure_loop(self): + # self.logger.log_info("机械臂测量线程启动") + # while (not self.arm.is_exit) and (not self.exit_event.is_set()): + # try: + # if not self.is_waitting: + # self.arm_state.arm_position,self.arm_state.arm_orientation = self.arm.get_arm_position() + # code = self.update_wrench() + # self.logger.log_blue(f"measure position: {self.arm_state.arm_position}") + # self.logger.log_blue(f"measure orientation: {self.arm_state.arm_orientation}") + # self.logger.log_blue(f"measure update wrench:{self.arm_state.external_wrench_tcp}") + # if code == -1: + # self.sensor_fail_count += 1 + # self.logger.log_error(f"传感器线程数据读取失败-{self.sensor_fail_count}") + # if self.sensor_fail_count > 10: + # self.logger.log_error("传感器线程数据读取失败超过10次,程序终止") + # self.stop() + # break + # else: + # self.sensor_fail_count = 0 + # except Exception as e: + # self.logger.log_error(f"机械臂测量线程报错:{e}") + # self.exit_event.set() # 控制退出while + # self.sensor_rate.precise_sleep() # 控制频率 + + # def arm_command_loop(self): + # self.logger.log_info("机械臂控制线程启动") + # self.arm.filter_matirx = np.zeros((8,7)) # 滑动窗口 + # while (not self.arm.is_exit) and (not self.exit_event.is_set()): + # try: + # if not self.is_waitting: + # process_start_time = time.time() + # self.controller_manager.step(self.control_rate.to_sec()) + # status, joints_processed = self.arm.process_command(self.arm_state.arm_position_command, + # self.arm_state.arm_orientation_command) + # self.logger.log_blue(f"处理后的关节角度: {joints_processed}") + # run_time = time.time() - process_start_time + # sleep_duration = self.control_rate.to_sec() - run_time + # delta_command = np.linalg.norm(self.arm_state.arm_position_command - self.arm_state.arm_position * 1000) + # self.logger.log_blue(f"期望位置：{self.arm_state.desired_position}") + # self.logger.log_blue(f"现在的位置：{self.arm_state.arm_position}") + # self.logger.log_blue(f"指令位置：{self.arm_state.arm_position_command}") + # self.logger.log_blue(f"位置变化量: {delta_command}") + # if delta_command > 20: # 20 mm + # self.logger.log_error("机械臂位置变化过大，可能是数据异常") + # break + # if sleep_duration > 0: + # libc.usleep(int(sleep_duration * 1e6)) + # if status == 0: + # # self.logger.log_blue("不进行伺服运动，只计算") + # self.logger.log_info("进行伺服运动") + # code = self.arm.ServoJoint(joints_processed,t=0.008,aheadtime=20,gain=200) + # else: + # self.logger.log_error("滑动窗口数据处理失败") + # self.exit_event.set() + # if code == -1: + # self.logger.log_error("机械臂急停") + # # self.stop() # 底层已经做了急停处理 + # break + # except Exception as e: + # self.logger.log_error(f"机械臂控制失败:{e}") + # self.exit_event.set() + + ####################### TEST ##########################3 + def arm_measure_loop(self): self.logger.log_info("机械臂测量线程启动") while (not self.arm.is_exit) and (not self.exit_event.is_set()): @@ -186,9 +237,9 @@ class MassageRobot: if not self.is_waitting: self.arm_state.arm_position,self.arm_state.arm_orientation = self.arm.get_arm_position() code = self.update_wrench() - self.logger.log_blue(f"measure position: {self.arm_state.arm_position}") - self.logger.log_blue(f"measure orientation: {self.arm_state.arm_orientation}") - self.logger.log_blue(f"measure update wrench:{self.arm_state.external_wrench_tcp}") + # self.logger.log_blue(f"measure position: {self.arm_state.arm_position}") + # self.logger.log_blue(f"measure orientation: {self.arm_state.arm_orientation}") + # self.logger.log_blue(f"measure update wrench:{self.arm_state.external_wrench_tcp}") if code == -1: self.sensor_fail_count += 1 self.logger.log_error(f"传感器线程数据读取失败-{self.sensor_fail_count}") @@ -201,38 +252,47 @@ class MassageRobot: except Exception as e: self.logger.log_error(f"机械臂测量线程报错:{e}") self.exit_event.set() # 控制退出while - self.sensor_rate.precise_sleep() # 控制频率 - + self.sensor_rate.sleep() # 控制频率 + def arm_command_loop(self): self.logger.log_info("机械臂控制线程启动") - self.arm.filter_matirx = np.zeros((8,7)) # 滑动窗口 + # self.arm.filter_matirx = np.zeros((8,7)) # 滑动窗口 ## 暂时禁用滑动窗口 while (not self.arm.is_exit) and (not self.exit_event.is_set()): try: if not self.is_waitting: process_start_time = time.time() - self.controller_manager.step(self.control_rate.to_sec()) - status, joints_processed = self.arm.process_command(self.arm_state.arm_position_command, - self.arm_state.arm_orientation_command) - self.logger.log_blue(f"处理后的关节角度: {joints_processed}") + self.controller_manager.step(self.control_rate.to_sec()) + # self.logger.log_blue(f"目标位姿: {self.arm_state.arm_position_command},{self.arm_state.arm_orientation_command}") run_time = time.time() - process_start_time sleep_duration = self.control_rate.to_sec() - run_time - delta_command = np.linalg.norm(self.arm_state.arm_position_command - self.arm_state.arm_position * 1000) - self.logger.log_blue(f"期望位置：{self.arm_state.desired_position}") - self.logger.log_blue(f"现在的位置：{self.arm_state.arm_position}") - self.logger.log_blue(f"指令位置：{self.arm_state.arm_position_command}") - self.logger.log_blue(f"位置变化量: {delta_command}") - if delta_command > 20: # 20 mm + delta_command = np.linalg.norm(self.arm_state.arm_position_command - self.arm_state.arm_position) + # self.logger.log_blue(f"期望位置：{self.arm_state.desired_position}") + # self.logger.log_blue(f"现在的位置：{self.arm_state.arm_position}") + # self.logger.log_blue(f"指令位置：{self.arm_state.arm_position_command}") + # self.logger.log_blue(f"位置变化量: {delta_command}") + if delta_command > 0.1: # m self.logger.log_error("机械臂位置变化过大，可能是数据异常") break if sleep_duration > 0: libc.usleep(int(sleep_duration * 1e6)) - if status == 0: - # self.logger.log_blue("不进行伺服运动，只计算") - self.logger.log_info("进行伺服运动") - code = self.arm.ServoJoint(joints_processed,t=0.008,aheadtime=20,gain=200) - else: - self.logger.log_error("滑动窗口数据处理失败") - self.exit_event.set() + # self.logger.log_blue("不进行伺服运动，只计算") + # self.logger.log_blue(f"输入位置:{self.arm_state.arm_position_command}") + # self.logger.log_blue(f"输入姿态:{self.arm_state.arm_orientation_command}") + command_pos_in_mm = self.arm_state.arm_position_command * 1000 + command_ori_in_degree = R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True) + combined = np.concatenate([command_pos_in_mm, command_ori_in_degree]) # 合并成一个数组 + # self.logger.log_blue(f"合并后的位姿: {combined.tolist()}") + # self.logger.log_info("进行伺服运动") + # 数据记录 + # self.logger.log_info("desired_pos",self.arm_state.desired_position) + # self.logger.log_info("desired_ori",self.arm_state.desired_orientation) + # self.logger.log_info("command_pos",self.arm_state.arm_position_command) + # self.logger.log_info("command_ori",self.arm_state.arm_orientation_command) + code = self.arm.ServoPose(combined) + # else: + # self.logger.log_error("滑动窗口数据处理失败") + # self.exit_event.set() + if code == -1: self.logger.log_error("机械臂急停") # self.stop() # 底层已经做了急停处理 @@ -240,6 +300,11 @@ class MassageRobot: except Exception as e: self.logger.log_error(f"机械臂控制失败:{e}") self.exit_event.set() + + + + + def start(self): if self.exit_event.is_set(): # 线程 @@ -265,7 +330,7 @@ class MassageRobot: self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}") self.command_rate.precise_sleep() - poistion ,quat_rot = self.arm.get_arm_position() + poistion ,quat_rot = self.arm.get_arm_position() self.arm_state.desired_position = poistion self.arm_state.arm_position_command = poistion self.arm_state.desired_orientation = quat_rot @@ -333,7 +398,7 @@ class MassageRobot: def get_tf_matrix(self,position,orientation): tf_matrix = np.eye(4) rotation_matrix = R.from_quat(orientation).as_matrix() - tf_matrix[:3,3] = position + tf_matrix[:3,3] = position/1000 tf_matrix[:3,:3] = rotation_matrix return tf_matrix def wrench_coordinate_conversion(self,tf_matrix, wrench): @@ -375,4 +440,5 @@ if __name__ == "__main__": print("Exception occurred at line:", e.__traceback__.tb_lineno) print("发生异常：", e) + # robot.arm.disableRobot() \ No newline at end of file diff --git a/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc index af78773a2c2ff364c7692281cd0ff4ec02010954..95fb21cc5213b85ff8a92db6370a99dfb0dbe1cb 100644 GIT binary patch delta 789 zcmZ8cO=}ZD7@pZC`@Ne`ny+r!rWI^cOBE_YBVP0%2rA+&Aj^=Mwwq>mq5e69cO6}5pfK{x$1z01>c%fY;6|5gi^e55Apj{+B8g;FESq}FxD5P}R< zyY&o9la)h&txs?YN!6|Yr=^b73&+|8YK+fHu$8ydV2Y@FqM}D<&*VMGKP!8W3*Fje z8B0?*RG2(*Vg;)Xn8JxV1w%Df1*gQI3%dCkR|G@tG8e0J)`&H}r9OP39p^qs^a0zz zdYnH6*vKYW0+|`IPr*kyO(;67jH!&J>}AJth8mN`6vV(?$KWYw7Wj^~Ir55|0MIp& ze_+0%BVwHF=rWEL@9dHwr2k#lDa-({Hw7EZ}D?u)lshIOxctc zr7|qTMX4@TrKQ{kwDY>eZ=1j2I^VZ$z?-~ly@Pl8cdH4n^OD`#*%Dh-s6umo>KRsH P@j!*L;B-;N?AGBQN}}26 delta 898 zcmZvZO=}ZD7{_Ne$-Zxzz9vo5G`^&z711^oLMR9ddQuM-!Cuy-GuBO;Y@JL+LrM;@ z-URDh)Kem;SD_dE0KVV{@US02#JdN zxc4=^7ti8fduQsctB9yXdyI%im3CxxoK6sZLt;Ou)<&yII!+9lBqop)ousKsx{Wak zRkD%^wbaS3BVxVAT}+d9<{uVxO!Yf*eKj2@f^VX^{Q_Q$#WlE>4Er+d`nP>57x7XP zRVJGCeau=)XNA+8ZwEXJ_+?Q5-*hEg51d*(Jt<&U9_WcxuZZIs>BW|O(lVL zRHzzFRixExEp;RMOk$t;NumXpgHsRvxGja55oS^^GY98YP7#a5N%ZL<9qQ9a7ym~S z;iT!_q~QZ^39dGEjCrIf_l%=WB~R0BgEtE94PTt`*NLSL9wX- z7h}F@hqGdUqBW0I*X#I05=Z>DgXcg diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py index 6d93619..66e1866 100644 --- a/Massage/MassageControl/algorithms/admittance_controller.py +++ b/Massage/MassageControl/algorithms/admittance_controller.py @@ -6,8 +6,8 @@ from .arm_state import ArmState from pathlib import Path sys.path.append(str(Path(__file__).resolve().parent.parent)) import time - from tools.yaml_operator import read_yaml +import random class AdmittanceController(BaseController): def __init__(self, name, state:ArmState,config_path) -> None: super().__init__(name, state) @@ -57,7 +57,7 @@ class AdmittanceController(BaseController): #wrench_err = self.state.external_wrench_base - self.state.desired_wrench wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench - # if time.time() - self.laset_print_time > 5: + # if time.time() - self.laset_print_time > 1: # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) # if time.time() - self.laset_print_time > 5: @@ -67,8 +67,8 @@ class AdmittanceController(BaseController): self.clip_command(self.state.arm_desired_twist,"vel") delta_pose = self.state.arm_desired_twist * dt - delta_pose[:3] = self.pos_scale_factor * delta_pose[:3] - delta_pose[3:] = self.rot_scale_factor * delta_pose[3:] + # delta_pose[:3] = self.pos_scale_factor * delta_pose[:3] + # delta_pose[3:] = self.rot_scale_factor * delta_pose[3:] # if time.time() - self.laset_print_time > 5: # print("delta_pose:",delta_pose) @@ -79,32 +79,33 @@ class AdmittanceController(BaseController): self.clip_command(delta_pose,"pose") # testlsy - delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix() + # delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix() + + + random_array = np.random.rand(3) # 生成长度为 3 的数组 + + delta_ori_mat = R.from_rotvec(random_array/10000).as_matrix() - #arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix() + arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix() arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat - # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() - self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() # 归一化四元数 self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) - - - # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:] - # self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat() - # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() self.state.arm_position_command = self.state.arm_position + delta_pose[:3] - if time.time() - self.laset_print_time > 0.1: - print("-------------admittance_1-------------------------------") - print("arm_position:",self.state.arm_position) - print("desired_position:",self.state.desired_position) - print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True)) - print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True)) - print("arm_position_command",self.state.arm_position_command) - print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True)) + + if time.time() - self.laset_print_time > 2: + # print("-------------admittance_1-------------------------------") + # print("arm_position:",self.state.arm_position) + # print("desired_position:",self.state.desired_position) + print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') + print("self.state.arm_desired_acc:",self.state.arm_desired_acc) + # print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True)) + # print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True)) + # print("arm_position_command",self.state.arm_position_command) + # print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True)) print("delta_pose:",delta_pose) - print("delta_ori_mat",delta_ori_mat) + # print("delta_ori_mat",delta_ori_mat) self.laset_print_time = time.time() diff --git a/Massage/MassageControl/algorithms/position_controller.py b/Massage/MassageControl/algorithms/position_controller.py index 9e4da73..ffac6b0 100644 --- a/Massage/MassageControl/algorithms/position_controller.py +++ b/Massage/MassageControl/algorithms/position_controller.py @@ -51,7 +51,7 @@ class PositionController(BaseController): # 归一化四元数 self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) self.state.arm_position_command = self.state.arm_position + delta_pose[:3] - + # if time.time() - self.laset_print_time > 0.5: # print("---------------positioner-------------------------------------") # print("arm_position:",self.state.arm_position) diff --git a/Massage/MassageControl/config/admittance.yaml b/Massage/MassageControl/config/admittance.yaml index 558fc54..30c027f 100644 --- a/Massage/MassageControl/config/admittance.yaml +++ b/Massage/MassageControl/config/admittance.yaml @@ -53,14 +53,14 @@ rot_scale_factor: 1 -mass_tran: [2.0, 2.0, 2.0] -mass_rot: [0.2, 0.2, 0.2] -stiff_tran: [400, 400, 400] -stiff_rot: [4, 4, 4] +mass_tran: [9.0, 9.0, 9.0] +mass_rot: [0.8, 0.8, 0.8] +stiff_tran: [0, 0, 0] +stiff_rot: [7, 7, 7] # stiff_tran: [100, 100, 100] # stiff_rot: [1, 1, 1] -damp_tran: [20, 20, 20] -damp_rot: [0.6, 0.6, 0.6] +damp_tran: [100, 100, 100] +damp_rot: [1, 1, 1] # damp_tran: [40, 40, 40] # damp_rot: [3, 3, 3] diff --git a/Massage/MassageControl/config/robot_config.yaml b/Massage/MassageControl/config/robot_config.yaml index c1d2a44..5ce451e 100644 --- a/Massage/MassageControl/config/robot_config.yaml +++ b/Massage/MassageControl/config/robot_config.yaml @@ -8,6 +8,6 @@ controller: ['Massage/MassageControl/config/admittance.yaml', # massage heads diretory massage_head_dir: 'Massage/MassageControl/config/massage_head' -control_rate: 120 -sensor_rate: 120 -command_rate: 120 \ No newline at end of file +control_rate: 100 +sensor_rate: 100 +command_rate: 100 \ No newline at end of file diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc index 0e4dece3c345e02626578e10b82530d0b560a1ea..b631f7cbc1be5ea6902db0e234916758240d4272 100644 GIT binary patch delta 5250 zcmbtYd2C$88Gp0;cK7XRukG~}U$K*rwH+Vvk;J)UJLEKp9TK`NY&V`~d(B>%_tu!O zj-g3_Rt+Q!mns!(wH&V6D9BV4Duh&o_y?gD1uDHtG)07BRjQyu6-rUl@0;h?>&2;} zc31xG%)FWJ{l0T%{C4`iabLvibxH6^JXJr^e%A$G1^MJc!(-&*3vVPQ|*3gw_q)-9fL2GFpj1|mqHPA4bPCO(&LfA zd-v*UyctF`3xGmc8MoLTu13ME9H9(hQ35Vz0sfM$lH~DsY+tR%M?qp(qX|7|F)U3j zA!{NX&skx(w#1^EKA4QBw(aJBvwyD%Ul4MRMPg~SUmYCFo$pOWm@Mv;lKIdYc?kIc zK`*OT|#1@U1G zfMGu}!qf<5O`=01Y8-;lrhyLNT0tk2cOz`Vx;b|#HP#7Bzt8{Wx`Th^+suDal*gZR zx3&I6xJ|(n#MY>jTB&jl5>>ZQCnhM2+D9GK1y1`2#@zfXcO8ExZ=;QXt@C-m=M_@N zW$$VWYvyab7muQ9A^Bi(Y@bOVwPG^TE(F_jrx{ZddUrA&k0j{c9@Fq>b3-1`lPR_y zn@V>wp+^&GRhWOq&I)+FZ?ii~A)fLTmz;pPS|fmDFD5SHgRhGy4t|fnnpE;r{<7JR ze19ZBFJ4%wVj{6E2wM@hAz-qz?Fc&%b|UOT=mx;o!jIt|O+o@`VayV?7oYA!*pF}k zA)8i}}v>sT@~J?%ApU-! zTmB3d@o?!e9xJZzcu_dnw%}wtm1+LGll|x%S}=flWF3`hA-L}oa9S?1^F!CV8R<)a708W(I| z){Hde&RC}jZdVg>W>Th>`H7}8ag!+kR|(w?nXY1Y0^dbvv7^{MhOh(R`iHWW)nbknbH z^egVu2esi_@vP1CEbdJ#gs^NOg9nDwWCST}ErCj@;Z`LqTVK5xMIWwQ8>b(itJ2WItV1=`9 zR`ik1MZxld)EU`ScL+!dmvm)OTad~|kyB3L$r~I=g25No_wkLjhsk;VquSu4E>Y`^xSxU+M5rtx zRHiZn=$?cJX4phEfe_6=zEC^Nag<0CHqlccd+Zr&#+I?4#n^UGCv}~XGL9)5bx)HX zIGjx=83oT!eGEO8ZUeab*4eq~-(Ee>*VZ*OoK%KYy(R?|MTJV^4*B_GuU&oo=}+H0 zclD*WuKxU$&tHAv8hL=#alLM9C#pANVfVlf9#>d21-;eSBjY)`vo$arhd^Vu@xRus z+lLtyw3{Jqh6|h@a6=J3G323Wf`ZIcEUH6|Ln4NphNp&%;gjX;PJYj-viclDJ3tQ< zcK2dr=MzPoAo%mEK7~WWnbo~(#iA65W(;Ae3_2=^gmam4O8!u<#*5po!Nf${DnyD!2hyyc+HC*Bj5T-Emjd>AXUfTS#jb<3)nfex_I5n6jguE?; zC@q&ePyiEWzXf}+W4)I8OJc8Ug|psC#Y}%j27Z3jAV}$;TZqAV@L6m@aiM%DZuCf_|MDS ztI%Sc_jQ)I#epJ${X2Q8v#v70o`sI#9Ae3M_yqhH#!wU17CXPPp_{jFZ0e|C#V~=DAe15m z5JX1dU0&SE*+Faxj}Kt$2!Ced<`Jw4><0*^5zZhyjPMA8@b?qgdLH3d2#YQkts)%5 z(--cy8eSI^qLeF&QmXisI%SnosZ_fB3UAp|;j}BV;!p|{H$S|ovhs(x5}h#o&CHh{ z96bKu_F?G2=gRhADSv%aeQgbNm<>VPNbq`R$b-q${e}(x#gw`q+#+|??nk$UzAw!pYz0Uh8A_xwDwrP%)i^&Xfy delta 5030 zcmbVQeQX>@72mnryW9J;&pv;(c6_n(b@4}>I*DUDcG|>FnmBbGJ89`rdaQ5O_Fe9F zFLQft;$G^~qy>taCLJh3D#$MM532>WGE(^zNFY!F(Wn#&ZKVoTfQS^T3M!EL2k_qP z#UB?JmEP%o_h#nJ?3>?v^JZp$bM9w_kQxm7B>41wee29m#;=AN$@^E^E|VKqf6(5S zuAzP!puwjdMAE|PS}28{CLq^o;f4A|=_1N*S|R?!3k{1Bk*H%ridFI|GD+U!VP_40 ziu8p0Xceu#MB;whPb0ME65-cKYtsR`i8e!Rm`3U5OH#aw4$v0b3N_VqkXJg!WfJVznl(1OE-vdpB2izZ!N=`hrTUUn)gM#Hd zkV)muq>;@H9)$8g;3v&>@N1q1e%jO9iKEB8vudWGjcdu-HHL`Z!JNwS(2_4?%wx2d z|HyN6(+kk0D5a$ZvM603AUU2X5~)b2R3Pk8zFDazkMh0B*tYv6D#`G@=p@ovX}6Sr zwhNqr-v{7#7FvNjFMnGZ+Z^+;+tHH1l%~-`YVypGYN~7#R9Vg$?Hp_7pY}HP#~e(s z(FpbBmimus<`L*T37zkP_Iw0Da`=b>--u)Rm)<+byr8ijd*6oOKxja~hg-qntj4A_ zJ$Zh@G;)?NnbA}>%$UL04rq`2hjn!_qm3JrhRL>Jw%GLfq3+B^7{LYuZXLoM^-q-Erb?LtvGZ0Kf6FK7${ ze#y-$__a`f%lheB;qih#W~6j8F_uVyl-D%gQPD`i_DJXQxr(>Rd=$E~JqUXd_965j z>_=!v=tby5=tscS#||J2APgd4$g?{E&=UBw{Aqk_K7osZ9l_S42zMhKLnv7aA;!cC zs9I<0aqRsC{$5xoAM)lJv*WVxjAzA1!dt+tr^LN0?WPLw{VS;R@}JkV_M>#uTiEJ{ z)*CR2EA11WxdH6~;g5fs{*sT5pl6ms{Ik`s+QbM>R_Q2XVgnME0wUyv7f+PDh||TT z!d5)V4q*`;$qpl+51EWGgn-=i+6)P9R^51_&`a(YIrt zl(qR!%o-a7W7xe2BM4=YxawY`o5@@vX|!dts!nfRGGK+h<4oX81eJf`uhvJv^S`Ly$zQ5%AUC+L z;a*^^VcxkQ7hQ|Mkm;hqqU$2;=D-tXw9pwx{z^%%0M1@U{uXX#LFa;<1zl9|Tha@I z-d|y)s?w@+F8E+?au=lqf6={2sG~u;;3;~jqoh|oFQ~`;Cj!(9gU4*@**MY}ML_b| z1i~o<%qnwod(<453$#a9$QhXPv`3Mj0d^WSN~86y$yT>6&v_{puw+a^mNTp7EZ0=V zP)&PRx(^@r@!d6y>h~jj3gH2SIKnMC4eEr0yc1e8cnH((zd$h$xlWQ`rJf=ISf^sG zE<11kJc&i2ej^sHyP@H_2SEg14_|JanGhz6tpX@8PZ>r=Y@0Ecoq0*ha+^?@NPVvVW(N=ups^DOI4qvW2|f#sD^mt;Fwr_jWYoM|{YW56ue4B52oZ4NogY&{KY zZ>_BiM~%BD)x7q04v#%BU=jMwTU>3fr`m{)hPWmXqX3 z-rpLVgt&DUoe;N9h+DZRQ{1r!^?)f+`DWl2VFgiGLDXF*U2ut#XOTe6JBzNO`w0>8 zRH5EWQqi;EqP|5k1ZU$5N>PEJ_w#G5gS>uA6gK(ZEd%?|wj~F92>$To!csXXwP1;y zD>Iz6LbVEeE!)Ch-qJCT%O&Qv%L)0)QgYBe0cKb-N@WIEo@x65C&7q(0G}z5N;u?&9DHi)pI}BK+lF@twsX+L z7{Yq;#T+ov!W_Q0t@Gv_g!u}Xf0sU;vF*3#W#SM8fSAXQy48R~tJyh#xL3tqXm;Es z#IlE43fK)JYZ;1$<0(`UTcpCZ?eBY|P-F%GKe(mmW0?QAZT^pUbgwht57ej3)T}m= z(uD`gXW+AEfJvUsB0P++!Gzd(Y?wn>&z=>8i5xN5!z!cjl)|zR^Em!*`@d$}|4v)? zo>B-2`hCarSq-inXueoWJJ5ht2Lx>Mhxu^(_Mr`|j(f!c&vt_lH?n^9Ay4pE+TR)x zj}8o_@~h{2@m#cE$yx66GCBfuO~2>)8y!E~V^8Jq`ul@DpT}+`lyyPfi_o!#|E}|u zKGE4bf%k_YrcST%;s$*!pSWlwC}@614Dsf!SN4k`6g-uGj1jzf&=Ui^CI)z;>#e;w zfPENT;el=Rbie4EHO!Qu|5Wt;+wMahF)|7_A29}AtXK%4j5SXpLA6XG@j8F-j!yo3 z_xEAp-nZwiwtG+)c`x0a)}6|sN$g;9N>7>B`02gZ+>fCCQ@m~8R#L%F?%UN-_MA!x z)`ZHk2}mWdFbw@&Tz#k{LC)ac-Zw<*`QP`Q@J8S)p`JfqEsK5hCQA$)6 z;UJL+o6dzQJWO2hfK&~K3GXynbYLl64P7He&orSycp2dY?FiJ?U@g?Ft*ga4{_CDQ zYp;R(&@TZJa4GKp0&Cr82LJ#7 diff --git a/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc index 83b71af5e91c9ef3880a77d617c13c0c2bd44976..23534e5594223701080aaa0c3b7cf20c2bb75a6e 100644 GIT binary patch literal 10816 zcmcIqTW}lKdEOfqOAx$@mMDo7NTTv1^ooQ#9>4T>e z_51%_++<2l9tv{L{hU3Q|MLC+*=%25O2O~l%e&{D2NdPsDAW0~kU50MyQC_LP=s1k zGEuB%RL*M|ons?oaGb~_I5smT$H`2RV=H5E+>`0yIF(7cJ%!$S+D#$sZ+7w*fr8{W;h;cgF9Rwac^5y3mXd?mw5!V zG>e-SMl+*oH1Exs&CTBKs7NlWnJw<%vXa>k2BsSBwU3~Hlv9_j{wc8T4HZ+k&8l*~q~@~*JwD}~%ix8#-Ohv|L7eP*BUFMa9c;faRw z_o~wP5R1tf9qXht5%du}-U@=MT2&mSsv%UXIzpuC`l^DIA*c?dj1Yv3Fe#`Vh!|my zpQ`tAyL2#$K7DkYet&(SsucJro={3V48I^r5`%|2Mhl0XnQZR#oGe#L!tv*&n-$@pK6 zI;Yi?rm2=XsHG7G!!4TI+w7d9^K1Tym*^S5Vt%qwK_;`mB`Bg zxfmb;E!dN?(DUxo>< zvIx4-@Xj#lI3@S2RdCA+qGm`egY5k)fOatrGVuXu-JkwRH3~@yan}urRvjH-*}yVKYa)@ zNc#B&w_NeD3=?#j&lWT0Nsc>+pbsOyrAit&nrC~mBPg4|q&)iH8e!l&*pfuXb}8p zJ6dP=ATrnvtx8MZR@1AR(6D5sKr&LF#HJilHY?Zw?Ti)GQ3+BAXEl~(`RmtlOnWd; z0|{kZmAw=h(ed4fw4_CZ#>^4~1Fx>N#4y2PYZCPy?VEz-S?ZV?473-><}t52k=I#f5vjmE6^ zgOD`Q`@FUpBH<~mX4DcyTATTOr*-dF$R#!_A|0puP-DdDUZfMJb-)7+<1##WLJ@r@ z732nJd|}i~XrdRPi3SRT7u6-9Dzzk5e?M9bR+G(nAU8xS0k9aR`4)yyWAF%lG{TeE z%Ube$vUDhlCqOp-Fvfc$9&Z~)*w7wNboWX%drg3SFxx-Fx;OBCo1mGI%3(S|JN{k% zpnhfL`)jXXy83tTG~WEle}3@o#tSc9dF{Q+@4tBU)gNE}>9^J{zTEikbJzalYf*z% z$EoR45~r_7;?^<0G;kGk|jNoNO`wj2mvcL8CYIwIq=m4rBhV4+Xeb@tv8<>?{=ZhNui8 zxuYrk zn|FamtjAEtDHFAoq93XskvA5tpGG-`>3I|=2}2zrv7>2_sVP0J4(cRZE&q4|<7sYMv4CAH>3x8OULQ!Hs>DMe+ZK_QPO zxSjw5N@97G5E>-UlyBCP0%bLoM2NY zMI>7?NQctXgvv4q^2PfnlJ$^5R-EoZjUK0yu{@$SDYWT5t;^8w6={+lkVZX_M(KK= z=tF(K7~pqkwFIP65i+E|FaRlX7o>;@sWgUugH^LBm00ee=DbOx2u`u&AxZXBN1vgO z>oWl)ULUSvHz03@r#r;NCWwhHxp2guf^?XYFou<26XZfvdtW3wj@XAM><2#ExE;0f z0D=bTPf2>SL0YvjiXa#wsUXyWn&72lwq`)FNU?aY39|FdY}(HX@IX zn;pvA5Tv}H;!;-LK2$Z4Zs}$2okHyoTfM)*?Rs0PKkEGkm%=|#)6lz*dq2*-`_cPf zTfP5*+YQ{%`ggD` zSaq-%Lyn}ljr(q_p=aqkU^94Fz%t(i%NSs@Ll}Ki)o8LAaxh^F03~8G%@y$$ep^3U zn9W(lAiy|pFbF`*tVrzp^LX5C7-?&JT$+1FF9HGFD+U4o48g;ua8(CWjs=HWihMHX z=bwQI2^+|@BbvO^v$Ir+GGJ(LA@6ylD;LVbZFPhGZwQb|r2p+m|08^0k`+Gq91wst z|Kwr&Gm{aA+mYs9x7m>+_T*$RbQ0>GUIeooAYtNIAc-J@o4?>5j7~joJ~rlZ6pgya z-hUs)&i@MZeT%X8+xs#0;aiNo-@X@PAMPIeKALwhe1dkhRQ7FHisVgj1&gi^H^oSV z%52!3)T9g>CD?tcbGu`?L1#ikFpQb46pKrdKqY||47Only$wL7_wF`;kq7C3Sm+Hg zSdoXQV3GntVmHb=1`rHZk^nU{r4I4N@{%PYI}AwL$;S|kCn96xIMRRym<53Z`8!m< zo><%BmdI)1L=@(9QrMg5ydS!cj~K&XK*L=z+I=*9>;+tTmm zQ|J)mlPA6Ok>{(=1`uwrbjWjkPnUSR|yD;C$mT#xMr3#jSh5 z3uZw8^ac;LIQeMaqXYRZ`9H9yrf&To?D?%mmRZ{19bGH{KNd|*{08J70rhF-u!!V) zJ2ZKVvgHika%-v{*CT??QOorxcxybZ^+@=a=oERj41%a8Lp~TGfD4e}Ht1S>=1a zC3<732i>7>58+}UUW3#uxPX+Uvh+Wo;RIt+IJc{b8)!IjK0#C~tQt}ia6vH*_XXoE z8a_b~uuFjvtlHbEZ)4C0;WDM{px zrz7~-0*#>#XKI)tv&>-=jS*vLy61ejojQ=?@M2baZUpES;c0|^Z4=_S84+DKNR+>e zH?Bv63|D`JO7?jwMg#%3z$_v+717uz#GnbC0GeiU+7^9vbpiE9)IOfe|P5KziEF_~C+L7&R_UA#ukQIC8Zpek+rAsl;-QuT( z)dsq4kbCCtHpo45cXNDrymklo$M5)eilh4?PW8uH*`$k4*4<_di0aI-E9k96qD{TD1+n3+}%e4!C z+^V<(J)*sdw&dC`UbuScTbJK|d+ow2S6}^pq-nPJmmzSoUyL6!G%C=$e7&3pao89LSr2 z3b_gGZX^C>8$U4uua*V-USoky*FtHA4Z8s)=z5G$G5z79OuY-hVz-f2&Fi?87-@4m zU`6v<1_QT#lqN~bTu#yg9IX&H@Qsl=jY|ZYjQkV@H~1uQ(IwoX;~b=!(jeWpKyhuC zP@&i9c=&Z>Fn|$p?Ez6@Q;IJO>?W8NuucVCU+KP4ND#{cw7~LqpmC_RL$Fw>TRdIB z=y9_$jUij~y>{{4wHGhJXT5gey=(9OZ0*8pmw)!NwHH_6*S_-Zg{y!1ow)G*=dS$u zYKwEO{Qi4aUV5iB$aJvlaLXqOw=>_zlA)F@QfA4Pl~M`b*(tyX6V97C*FOc(&2)4K zvxywBbR#K;5QOHb=*o9=9ak%EkiFBPDYEWDxdh@OkL-Fo&!*NS?K-)=6$A948n`qa zWLgMzv`_tJPTiXy3R#J?t@QDOM|UiIC|>uW07*JH7+_1dc7*v7BsJgJ-YHg~XYd{ZLX|x$iQ2&%!c-m?<lI za7sHmPl)zRGFaHgs%N1OW{o<9&EKabqkS>rV?VSbiZ12W;@^lcgqwBMeBCVP* zr()pihb?U~y|E$okf>5pTXvC+Z=&xjO4$Y1al(}2z=>9Yo5cy!j&r7xEk-r+c51(! z0y>>AjU)8)xdqpsFAGUD9$C%zQ|crIblOr-;8O4~1tkhtNd%N4sUB%iIykwE0AH&_ zUpwU7BJ@Rv9@#}r7_yT7;O{neA_0{)nzk$*&g;JZr1e>A%=#RjQLD!qhZWs#^;v__ z#}-)SLKb&q4&AVm*Nd0mWzo>cv^z)ecpDMGj=}W{E{I{jkTD!9JAFFXeZtMTFvWy@ z#FxcS9l6`4k7?+ekU5#nx$JP;yLT@ekl1s+1JJ!CeBMFrU`^<-F#rN2Hojw(w7{^R z%#Uo*nTPwo=*nvAY(dHL0$SXB(sU9{UlZ6`i*8XsPv1g?{5Ar-+NAL-3-VdT~a6-n=LDA%#oyy<+_)`Jnl5i(Ld5^8WmHvj0_lK*z-{m&}TDzgOKELiL8tV9Zz WnEEjJi}){%@^mwoh`^o&2>5?luRNRp literal 15294 zcma)jdvF}bdFQ-#cXqK@fFKAyM9Cvs55l2901{~nktI@;VA&C2homigBiClRGXNG` z?1DYBBr&&;C6N+oiWDh}l46mPKt<1UDC%NIqDWdw<*HJ3cd7iB{4x0_RpH{1KhCbY zT;)o4k?!|>J^KP7*$Zq>Pxse7J>B2q_kG=)ZEsH~`21||E#v3Dt0@0MH_aa%H{0-a z{|^#dvDJc-mRB{c^1YVU_^PK3zDCj!zDCngT(v@M(oCC^@pN3Jc6uQ(*_KXnUM;kz z+fil|Qj@84N>#q4*bzH=M6siXwRDHwW5?~p5jEXux7o=fN_vI8+HSW~DCx4-*q!zY zq`3udp)k*_Ko&U zxUO<~?626?5#_L&UhSCn&3MiR`>W_-jeU!KD=5~6{oZEZj?#7ZM!OfKH`rgZ{{YwZ zM-@X!--wC3BZ&WSBDdQqx+SsrDc-$T4~+4W>jlPCNgx%Nv7FH~Rg@>D+kvmU2T4V( zC>f=qAyq3nQq86k-KG+wqRd9lE67FpK4uez$@#dKm~G>_q}M*1swfj3#00j6aT(=* zLe~i^{8W#WLSvJv+WvvWsYtN#3*I_=O@R%9nF zdWj)x;k}QmZydG0{XMIC?)~a3kN@WCvFck-S{nxL9_$;~`i;JUzJdP!4Zpei)1}Sn z?Q6~Fe{r#T>d(WbTlxo=A*X2?vgTfRf9~Xai${;n&3-ul>`}Bp@0$m=^l#qM-~W)` z+-_0D`;83ub_x?25vPPCJ!p3e*#k}{Yumzc-N0~5xqU(Rm{WA<8JV2O4Q$PL*}VlP zXluUd%IwYN_Kk^BxoBs+ap7d`pflrorKzThpgmK_x?YBA^Tn|sE(s}XJWLO2uT^CV zbQ{GZ59PhU6wX-Q^&Al-?7W*R6^l;J>oqVsr!czY4c=A6I=slIyEc!PCY{X_?&w~b z?994ucFfsT+FSB64^X-}EUI01l!~4x6*iA&g?%t9oXw*pk#jOGvuEGbbYNyO`C{J7 zWZLlrw+D$5i>kH1geG$d&F`vnbziMXj%K{}Ar7W*2fpqGBp!IwRu60LdQbJhxren` zaO!#DWbp8W0iM*#Nf1^~zHsg1XXf5HU%h%NF!M#%%NBD^ZzPC~XWgvli6G*ZryL=o zAP!=gOwl=*$%q?q2iekd1*~aSjJZK{A6{|HeOC?QnM@AL;)0B3PTW9=g@gv?Qa_58 zP`gxrMe|#6zb!ANfslBJMe`Uk{)nf{;?*Wpe6{l=ka(#Heb%s55;e|8kVf&1jcQcp zv2adA&%zc^=~?9!gY@hpO7Sj3@#51@s>%%R{q>+UDk|zVs(%s`KZ1RjQ6E!_33=^T z`cZ)bfbZa!5FF5v) zHPV|1q9hcK7sR#?XTCl1;GUpu_jh;vQD*m^T@P;GDTp_MgzI>u=}Tphh21n5HHb`! ze9;Ty4<5=nY%t!{#9C^nI&uvf3$Y2s8}W5Jktmvmg-EL1>UuSWzvd+gpF$cGb|xFF zat=-LD?z5*sZ!B(G6km?M1|v(MKMS0SNo9psjr-e8~-NEy)vs-YU`xTY89-{j9$?n zQFcRpd-{YieUAz|iZzRXb+HDRe<)AXMW>5cC4*Nff^{@%tHf)h%@~itsO<(d<#?5( z()|Y-hZTORr=DAU{0XVksvms-l{t<{)Ro08LWfj;3Ml`Ht#{}Sx_)mZFNTcwUXy1C(FfrE=xGVf*_Z~kkuQL zY!!npva^K(45O1fAc!x5SjcDM4%~@8B)t)4CNV$-gG2)bDp^4EQyFttoyCGq>NS(a z7ri?91J0cwa=VczF%8_7go;b4Ca2)JbqzJJv|+C*sBMc{Am*YktA)3aA=a{0fU+J> zoz-k&E4A3or8-K##-#?8!g}i!;GOf@4A{>d_6$HjFq$3Pjb3IW70ov7_*2?iXv#;m z8GXh;Nn$NOVV~|ki#rvls4dLLQDcPp81ni)UA&AwleIpPZ|8ghN+VVN3I+UU! zWM)Bp_wOLyF=!2MStP^kw?mIuz9UMkfEGW;XJqj+q=L-JBIyL*+TBb;N@~Zh9ae7K z$?cQ;Z`8?#gA>qZJ?9n>F;TW7_vSDZB}I;F2fi0d9v z?Bs43z}dLng`R-Pc{JKggpg__a$XHpFR=V+bvh9$->A)w2$U~cL;0pqcQ4d$l+|z4 z?y10-N+ZeYH#!r2EK0OC=lZEl2ihD6+gKRoYOk{bN&E74Fs@O^$(r(C(|Xj@HpD|p zhjoXwaPdm@)Qi=pF3ex~RWn$YdfD1IS}0|`I|t`xuL8~V*1M2ObjYd~P~XL=KBzt+ zveO&4Zt6wn^=`x7{$%*jrs3&L!~Uk7hc@k;-n7&IaZ3l`V4BtR9Rqa=DaflK$Ssn) z!y1PC4vV|-ivDUyuB_c6rF(~Uf4_D2*B1$0jfi`YO0gs-Ws%^};u)Zimq$m@8!M7+L_uaL(4ky_n>Gav#LbK{w{wL--qV0qPiRQf5SrS^FIthD(;!u?$Ik?n6--V(ZD-(atU3zN%-OFGI39;s{K3{$MFRLdm zkG=eJ`h0X>9x6idyLZqU10fUZ$Wy!zmOafW7 zF|vT(W#od$j6^l!Ds(*%jGaP1S3y^|5U=RmVg+o1VRzM9+bf}SyUR42)zcT}&%AT( z(v|A*cYsqCK7L;Mg-b!oq77oH;4Fz$klwwWp$YUIzzSoO?e|+-xBHfaC(Bao*kNto zF39|Xj3Li5WCQ30WM9$|p7{WX^8W_Y{Usd@T7&55{x9ih(7Fp9-S2mPzX+$7VkTfu zic_%46+`se-=xugWqZy8uC+XoExMC=*JW&7q%ekUWj!mcJ>;|M_HP+xM~~bZt$j&p(<2sA>b-dKAnx7lMO)XcQa8isc-1BsKx5Bwgw&Z>luJYZ+yIjEOGs281-3p?KHP@^>*Z^jgQ z=yvm!9Xg7$!G#B}8q)cG>X(ZzU1)N?{Z&g$P_9sN9ot{eT0iuYsEqt1>2QkB7DOsXSNV)wbzHSPMg6NC}^qGVrxkdH68*|nODYeWI0c_GDtU*YFv9zmY zRamuI5*Ez`OD5iwV~e_HOmIlUHcoP6(xy8l6XP}}&1@Kw7(thH_yJLd5LZt>TRru@ z1S;3AK&;RI=IV*Li>Kz_{#jUf>G8S0IA8ZkmiV_L{;l`dgK%uMY3}l_l`W7Rnzlr_ zSVYk7X4v1KVJ3$CVY@&n}5W`%9Vlfbeg5ykuHbBChexhq#O_4#_KjJGREe5G4 z6Y9xSZ-a7YCDkMW%SaBw@b12r1tgV4VFOlK^hjDjUL-)CM-;7Tk#;Lsqz%oB#1pHP zTPAkl*$?5s&7FIE;gjdCvF^ddLKQ*rG$s~!t}!9Yv27<5O-tZ^j^3KknF5!;Ii9qX zFh?}C4+W9xkdC%~#*`Vn5!e!u#KYODJGMoRQ)Xihol>u9w@HfqJ3PUyiJN?L!k+ zkHV3sO43!KNxBI^U05+p4wwb|X!>?c-!^5ff~nH>o3f5dkV85uL$$Y{i*S?piXomHho+wVYG-7OhJ$cH0aHoz8WfE>MilR zdV{}+kZW&S`3PR5F8T9sJwg6QZ3`5t;dlEY^~Sq+8^Rr`7g+f8v&g-186sCb{u;o~ z9Rp#B46oOTeQS95%m4jbAIQ4^_ymELf$-L3$#(ks`uv-hz)--YV#%`{D1J0{#{o%u zm-8Ng8m0RXhEzw(g~GJ-A-WnNj9LJHL@S_?*TU_J#c>4s7^cOQSFdy2Kmt z7Ks(wiCO=clDjCWyVq=vLgSNe3%lmf?J`CvxS>X{j3;6ew{Jk_N)$0#74Z&yNex9c z+I*tuZGBVV)`n1iNslcOLVB(aHw;MCY1N;CT7o_&k)45zKsBlEqw>C@t)Y;;A|8dD z(8d9@HuSsU4(;zoFx7zjOgOJr6RAXOa^kxY=2StUx<$lKVk89sE06+69nO!8!E($* z?Y5biot!aiPp+7i7@qX^OP>7qvj0k~Vy?lK!%Vyq7so46$SfRwKq1qMRv>3zS7yxp z$HP8;i>J1uoWrShN;>s&1O$pIQEXv!)Wgn+nKqPkB8Z&Wr=LdD2X`H~!xqMARoLij z1-${TU9!eri6An!pq%U-wcV9Au1PdX>(4YRQH(uVNrvI%_(THX-=Bq=La&FDrKV^@ z-QPl2w6o^X5l(Jz0c++=`(y2_c{s5X1X0_K|!N$s$w}Xzs6MNVA|D!JD zB$_{P1bgduAYhus_UyQ0$>`qgu0=6P3Q$gA_nO>$mg#!yWi`xoVdUNULw95kLF#$4 zf3sH-`^%2(nIg&-Ldq6m$`*pk7NW}5(y%gZ^;!ARI@qU;Vz)kqVTP5A3;q|RNwGK` z+2d~-#wiiU8pk_1f;G-49i6Za=Dl&eQMjjPVJAOdvPTvzv|Svbgrr8QnXi1i2!(+| zMxJAha&O`Go!w`B3o6WCY4PsPCaWIO9oQ2UFz6d)_bKI(H967lqQr~{?|^CDGQ5dhFR53lGzuL{pwHCSz!f&&3a3!tjq)DAA5*;vnm4K^ zpoOoCJf|93m>$@I04dl^IEsLYOEb=A$q)%W0<}ly%K$?32XLlqMxz~Apglw?q++t8 zHz<$lw7-hIC|QR?d|_RT>!LF;s86cHc7G*OiIS2WgQ9^*n#if_b1e2@mbVZPY`+imkk_1m!`k> zCOXelgcizVf7{EY^NN#$%0;1 zkO$4te37OcBWv>JghL6rzG4DNkc1q1`P`)AjhAe8eOSUpnJ5lW@+2knsyH1c4=UxH z0oRTPVG#kuU5SmrVsM*%jm0Hu;Z>CbK&A6E|+s#hXL?|$oikU|0Ci6jg zRw^nH&^%h>)4i?bgbX!_sc&a>F~4#%wk#Q>*tD|RL^S6AAXmO@u(P&TPbTP0a(hvJFbYtF@?Db|*6#3Hd7p8Kn37B0R}_fo3ouw!x( z?z`M8xpwIp87o@+`BO5SDq|jtuRn()l-vO))l+nusNtQk4PQv3`& zUA~zZ`Wl}dyRLvu%`cP*6`920FWl|LIt8rvjKAnQtO{2HUq<$>C z1N^TM#zw3+%U?zD!iBf0&mG5UtfNO4&ps_rTFG^pfAS+N+`MKX(nhsgM-Zwbv>()a)S0cPseg71ub@Amhc<=%YBq;7A zib=Ol@y6RV3Vx2fnVUUYJ^FO@!}rKR3k+IxobnD=4u@4~&pZcQha;@n17n#{;p`8R zlldZ!ks{gwZI8R`p{6_6vm18_LMPCVlT&UGueES-W|v}gQm?XdWHrecDo+)pdV?g8 z-BFP@2U3a^;1D2Ifz={{C zmzR*FTRZ$2^yh>$BdiUDH#LHBEmXdRn{G1ZD#SMeJ z3TKkM@Z2p=TG=V)=zxmi?ghEhZUP4353q=K3Todd$I7U@6A%i{D*kZ52-{_}uOR7g z{D@khEQPK59{MvhL|$9|{$0F{r?j9aGIV5IcHrw;7zu$Df+w_T05=gZ1qjMGuhZQG zcE9Wh_H?6AZ8)V*j^;_kj<^O}@J%&swHj>eCrLS@uL$;rWiCpmhqw#|fUj^C!xH&K zJ$xdcX7`fvxrcFVvuOmKc>ap;`87Ho1=BUg(}f>Wgmf6q%KB((~dwBa~{>qh8&rrqBO#C0vyPBDxO(jA>ZQYbd~I(Z0M!Ky!Pi7znSerF z0aY)bs-F7ri#kB%v6n_5U;0!5hhp%)W#Ob_Zh0^LUIf2`0IEz!5AopB(P_RSlAH(h zO-Bn3BENRp7!!FLKbAlPDc@22`T((K@fgODPI~#VaSFXp(>Y|r9WNaO7EXns9-Ij+ z7d$uU2rUu(8ZKBWjVq}442xt)@n1<2$-%-24Lyak8S7Z8Si*4Mt)@PYYBCMw4dB#N z#U-?8kt#ykWDHoUx>>5gw{QRs>(UFM)1k!oC`Bj;r8@Z-VJRdo42EFy4gi8Bjjsti zUOQEX@I#xdg-3(ad3HNdU;`k`%d>Z&O4X?laIrffxj31}luh?_>aIHV$|clI_jPIn zp|~&2UbTd}={}^!8CHNe1bXSp(e@s;(vXscj}J>ps($$9wO^lW$kXF*FC6|g8+4YG zbSzxbCd19Y7?8p?cl=G*ST^jW(&kR!2O~#){hom>e4I!8XB7JSy|{f3xpF6xW&^tM zA-|W^wjTuzOes?|TTzlDspm;rCE1+~vYIJnGO`3^as`)IRIVfofn8G4#xh9229C`i z@e&-ux2TG2lK6m<4=Gt6uL*dI`TwE{p6PO0ELDV-^`V{kDZwap^J_k3pv0SW{kiHb_?#5q3DwSRC- zu@kQ&l!;KI3XYH4$&>o2emD!U2?R3{B5XgY&6pLOE+q|(YeH^&AuOoZc5aKE*cANl z&6|RF55RdEEzYcV@Tm`$IX>CZ>3_sXayP&PN30mDAig894K~|L){fmjutxdFI|5r4 z+`KG-B5uaXcKimVfvOvTXUKXO>dz{THgzn$V;mcWXV1lb%^+uEZ{q|hJ(2v^A?v$k zZ>sFohe3t>TsABIrV$GD7fa=_@y2s_-0|$8fCCMhK0wcb6WsL zO{WMKQM{;Fp4{t*bX#pMGLwLC^|woR*CKLt{sKWdX*&n<4El*&J&0o(G5pL1UIP7& zFfp2U3O2of>qlwIZOczJMn{v~+s1ld;zRbuXEC-Q?qE3l`z;27flDExZ6> 1) ^ polynomial + else: + crc >>= 1 + table.append(crc) + return table + + def crc16(self,data: bytes): + crc = 0xFFFF + for byte in data: + crc = (crc >> 8) ^ self.crc16_table[(crc ^ byte) & 0xFF] + return (crc >> 8) & 0xFF, crc & 0xFF + + + def set_zero(self): + """ + 传感器置零，重新上电之后需要置零 + """ + # 确保后台读取已停止 + was_reading = False + if self._background_thread is not None and self._background_thread.is_alive(): + was_reading = True + self.stop_background_reading() + try: - self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.sock.settimeout(1.0) # 设置超时时间 - self.sock.connect((self.host, self.port)) - print(f"Connected to {self.host}:{self.port}") - except Exception as e: - print(f"TCP connection failed: {e}") - raise + # 重置输入输出缓冲区 + self.ser.reset_input_buffer() + self.ser.reset_output_buffer() - def send_and_receive(self, data: bytes, expected_response_len: int) -> Optional[bytes]: - """ - 发送数据并接收响应 - :param data: 要发送的字节数据 - :param expected_response_len: 预期响应的长度 - :return: 收到的字节数据（失败返回 None） - """ - try: - self.sock.sendall(data) - response = self.sock.recv(expected_response_len) - return response - except socket.timeout: - print("Timeout while waiting for response") - return None - except Exception as e: - print(f"TCP communication error: {e}") - return None + # 清除串口缓冲区中的数据 + if self.ser.in_waiting > 0: + self.ser.read(self.ser.in_waiting) - def set_zero(self) -> int: - """传感器置零（TCP 版本）""" - if self.slave_address == 0x01: - zero_cmd = bytes.fromhex('01 10 46 1C 00 02 04 00 00 00 00 E8 95') - elif self.slave_address == 0x09: - zero_cmd = bytes.fromhex('09 10 46 1C 00 02 04 00 00 00 00 C2 F5') + # 根据地址设置置零命令 + if self.slave_adress == 0x01: + zero_cmd = bytes.fromhex('01 10 46 1C 00 02 04 00 00 00 00 E8 95') # 传感器内置通信协议--清零传感器数据 + elif self.slave_adress == 0x09: + zero_cmd = bytes.fromhex('09 10 46 1C 00 02 04 00 00 00 00 C2 F5') - response = self.send_and_receive(zero_cmd, 8) - if not response: - print("set zero fail") - return -1 + # 发送置零命令 + self.ser.write(zero_cmd) + response = bytearray(self.ser.read(8)) + print(f"set zero response: {response}") - # CRC 校验（假设协议要求） - hi_check, lo_check = self.crc16(response[:-2]) - if (response[0] != self.slave_address or - response[1] != 0x10 or - response[-2] != lo_check or - response[-1] != hi_check): - print("Set zero failed: CRC check error") - return -1 - - print("Set zero success!") - return 0 - - def read_data_f32(self) -> np.ndarray: - """ - TCP版本 - 读取六维力传感器数据 (float32格式) - 返回: - np.ndarray(6,) - 六维力数据 [Fx,Fy,Fz,Mx,My,Mz] - 读取失败返回 -1 - """ - # 构造读取命令 - - if self.slave_address == 0x01: - command = bytes.fromhex('01 04 00 00 00 0C F0 0F') - elif self.slave_address == 0x09: - command = bytes.fromhex('09 04 00 54 00 0C B0 97') - - try: - # 清空接收缓冲区 - self._clear_tcp_buffer() - - # 接收完整响应 - response = self.send_and_receive(command, 29) - - # CRC校验 + # CRC 校验 Hi_check, Lo_check = self.crc16(response[:-2]) - if (response[0] != self.slave_address or - response[1] != 0x04 or - response[2] != 24 or # 数据长度字节 - response[-2] != Lo_check or - response[-1] != Hi_check): - print(f"Protocol error! Received: {response.hex(' ')}") - print(f"Expected slave:{self.slave_address}, " - f"func:0x04, len:24, " - f"CRC:{Lo_check:02X}{Hi_check:02X}") + if response[0] != self.slave_adress or response[1] != 0x10 or \ + response[-1] != Hi_check or response[-2] != Lo_check: + print("set zero failed!") + return -1 + + print("set zero success!") + result = 0 + + except serial.SerialException as e: + print(f"Serial communication error: {e}") + print(f"Details - Address: {self.slave_adress}, Port: {self.ser.port}") + result = -1 + except Exception as e: + print(f"Unexpected error: {e}") + result = -1 + + # 如果之前在进行后台读取，恢复它 + if was_reading: + self.start_background_reading() + + return result + + # 可能比较慢 + def read_data_f32(self): + self.ser.reset_input_buffer() # 清除输入缓冲区 + self.ser.reset_output_buffer() # 清除输出缓冲区 + if self.ser.in_waiting > 0: + self.ser.read(self.ser.in_waiting) + if self.slave_adress == 0x01: + # command = bytes.fromhex('01 04 00 54 00 0C B1 DF') # (旧版)传感器内置通信协议--读取一次六维力数据 + command = bytes.fromhex('01 04 00 00 00 0C F0 0F') # (新版)传感器内置通信协议--读取一次六维力数据 + elif self.slave_adress == 0x09: + command = bytes.fromhex('09 04 00 54 00 0C B0 97') # 传感器内置通信协议--读取一次六维力数据 + try: + self.ser.write(command) + response = bytearray(self.ser.read(29)) + # print(response) + Hi_check, Lo_check = self.crc16(response[:-2]) + if response[0] != self.slave_adress or response[1] != 0x04 or \ + response[-1] != Hi_check or response[-2] != Lo_check or response[2] != 24: + print("read data failed!") return -1 - - # 解析6个float32数据 (大端序) sensor_data = struct.unpack('>ffffff', response[3:27]) - return np.array(sensor_data) - - except socket.timeout: - print("Timeout while waiting for sensor response") - return -1 - except ConnectionError as e: - print(f"TCP connection error: {e}") + except serial.SerialException as e: + print(f"Serial communication error: {e}") return -1 except Exception as e: - print(f"Unexpected error in read_data_f32: {str(e)}") + print(f"Unexpected error: {e}") return -1 + return np.array(sensor_data) + + def enable_active_transmission(self): + """ + Activate the sensor's active transmission mode + """ + # 确保后台读取已停止 + was_reading = False + if self._background_thread is not None and self._background_thread.is_alive(): + was_reading = True + self.stop_background_reading() - def _clear_tcp_buffer(self): - """清空TCP接收缓冲区""" - self.sock.settimeout(0.1) # 短暂超时，避免阻塞 try: - while True: - data = self.sock.recv(1024) - if not data: # 连接关闭或无数据 - break - print(f"Cleared TCP buffer: {data.hex(' ')}") # 调试输出 - except socket.timeout: - pass # 预期内的超时，表示缓冲区已空 - finally: - self.sock.settimeout(2.0) # 恢复默认超时 + # 根据不同的速率设置命令 + if self.rate == 100: + if self.slave_adress == 0x01: + rate_cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 00 AB 6A') # 传感器内置通信协议--100Hz + elif self.slave_adress == 0x09: + rate_cmd = bytes.fromhex('09 10 01 9A 00 01 02 00 00 CC AA') + print("Set mode in 100Hz") + elif self.rate == 250: + if self.slave_adress == 0x01: + rate_cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 01 6A AA') + elif self.slave_adress == 0x09: + rate_cmd = bytes.fromhex('09 10 01 9A 00 01 02 00 01 0D 6A') + print("Set mode in 250Hz") + elif self.rate == 500: + if self.slave_adress == 0x01: + rate_cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 02 2A AB') # 传感器内置通信协议--500Hz + elif self.slave_adress == 0x09: + rate_cmd = bytes.fromhex('09 10 01 9A 00 01 02 00 02 4D 6B') + print("Set mode in 500Hz") + else: + print("Rate not supported") + result = -1 + return result - def enable_active_transmission(self) -> int: - """启用主动传输模式（TCP 版本）""" - if self.rate == 100: - cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 00 AB 6A') if self.slave_address == 0x01 else \ - bytes.fromhex('09 10 01 9A 00 01 02 00 00 CC AA') - elif self.rate == 250: - cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 01 6A AA') if self.slave_address == 0x01 else \ - bytes.fromhex('09 10 01 9A 00 01 02 00 01 0D 6A') - elif self.rate == 500: - cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 02 2A AB') if self.slave_address == 0x01 else \ - bytes.fromhex('09 10 01 9A 00 01 02 00 02 4D 6B') - else: - print("Unsupported rate") - return -1 + # 检查串口是否打开 + if not self.ser.is_open: + raise serial.SerialException("Serial port is not open") - response = self.send_and_receive(cmd, 8) - if response and response[1] == 0x10: # 检查响应功能码 - print(f"Active transmission enabled at {self.rate}Hz") + # 重置输入缓冲区 + self.ser.reset_input_buffer() + + # 写入指令 + self.ser.write(rate_cmd) + print("Transmission command sent successfully") + result = 0 + + except serial.SerialException as e: + print(f"Serial communication error: {e}") + print(f"Details - Rate: {self.rate}, Address: {self.slave_adress}, Port: {self.ser.port}") + result = -1 + except Exception as e: + print(f"Unexpected error: {e}") + result = -1 + + # 如果之前在进行后台读取，恢复它 + if was_reading: + self.start_background_reading() + + return result + + def disable_active_transmission(self): + """ + Disable the sensor's active transmission mode + """ + try: + # 禁用传感器活动传输模式的命令 + rate_cmd = bytes.fromhex('FF FF FF FF FF FF FF FF FF FF FF') + print("Disable the sensor's active transmission mode") + + # 重置输入缓冲区 + if not self.ser.is_open: + raise serial.SerialException("Serial port is not open") + self.ser.reset_input_buffer() + + # 发送禁用传输模式的命令 + self.ser.write(rate_cmd) + print("Transmission disabled successfully") return 0 - else: - print("Failed to enable active transmission") + + except serial.SerialException as e: + print(f"Serial communication error: {e}") + print(f"Details - Port: {self.ser.port}") + return -1 + except Exception as e: + print(f"Unexpected error: {e}") return -1 + + def read(self): + """ + Read the sensor's data. + """ + try: + if self.ser.in_waiting > 0: + self.ser.read(self.ser.in_waiting) + while True: + if int.from_bytes(self.ser.read(1), byteorder='big') == 0x20: + if int.from_bytes(self.ser.read(1), byteorder='big') == 0x4E: + break + response = bytearray([0x20, 0x4E]) # 使用bytearray创建16进制数据的列表 + response.extend(self.ser.read(14)) # 读取12个字节的数据并添加到列表中 + Hi_check, Lo_check = self.crc16(response[:-2]) + if response[-1] != Hi_check or response[-2] != Lo_check: + print("check failed!") + return None + sensor_data = self.parse_data_passive(response) + return sensor_data + except serial.SerialException as e: + print(f"Serial communication error: {e}") + return None + + + def parse_data_passive(self, buffer): + values = [ + int.from_bytes(buffer[i:i+2], byteorder='little', signed=True) + for i in range(2, 14, 2) + ] + Fx, Fy, Fz = np.array(values[:3]) / 10.0 + Mx, My, Mz = np.array(values[3:]) / 1000.0 + return np.array([Fx, Fy, Fz, Mx, My, Mz]) + def disconnect(self): - """关闭 TCP 连接""" - if self.sock: - try: - self.sock.close() - print("TCP connection closed") - except Exception as e: - print(f"Error while closing socket: {e}") - self.sock = None + """ + 完全断开传感器连接并清理相关资源 + """ + # 首先停止后台读取任务 + if hasattr(self, '_background_thread') and self._background_thread is not None: + self.stop_background_reading() + + try: + # 1. 尝试禁用传输 + for _ in range(3): + try: + send_str = "FF " * 50 + send_str = send_str[:-1] + rate_cmd = bytes.fromhex(send_str) + if hasattr(self, 'ser') and self.ser.is_open: + self.ser.write(rate_cmd) + time.sleep(0.1) + except Exception as e: + print(f"禁用传输失败: {e}") + + # 2. 关闭并清理串口 + if hasattr(self, 'ser'): + try: + if self.ser.is_open: + self.ser.flush() # 清空缓冲区 + self.ser.reset_input_buffer() # 清空输入缓冲区 + self.ser.reset_output_buffer() # 清空输出缓冲区 + self.ser.close() + del self.ser # 删除串口对象 + except Exception as e: + print(f"关闭串口失败: {e}") + + except Exception as e: + print(f"断开连接过程中发生错误: {e}") + + finally: + # 确保清理所有属性 + if hasattr(self, 'ser'): + del self.ser + + def __del__(self): + self.disconnect() + def start_background_reading(self): """ 启动后台读取任务，每秒读取一次传感器数据 @@ -234,336 +368,40 @@ class XjcSensor: with self._reading_lock: return self._last_reading - # ------------------------- 以下方法保持不变 ------------------------- - @staticmethod - def generate_crc16_table(): - """CRC16 表生成（与原代码相同）""" - table = [] - polynomial = 0xA001 - for i in range(256): - crc = i - for _ in range(8): - if crc & 0x0001: - crc = (crc >> 1) ^ polynomial - else: - crc >>= 1 - table.append(crc) - return table - - def crc16(self, data: bytes): - """CRC16 计算（与原代码相同）""" - crc = 0xFFFF - for byte in data: - crc = (crc >> 8) ^ self.crc16_table[(crc ^ byte) & 0xFF] - return (crc >> 8) & 0xFF, crc & 0xFF - - def __del__(self): - """析构时断开连接""" - self.disconnect() - - def disable_active_transmission(self) -> int: - """ - 禁用传感器的主动传输模式（TCP 版本） - Returns: - 0 表示成功，-1 表示失败 - """ - try: - # 构造禁用命令（原代码中的 FF x11 字节流） - disable_cmd = bytes.fromhex('FF FF FF FF FF FF FF FF FF FF FF') - print("Disabling active transmission mode...") - - # 检查 TCP 连接是否有效 - if not self.sock: - raise ConnectionError("TCP connection is not established") - - # 发送禁用命令 - self.sock.sendall(disable_cmd) - print("Active transmission disabled successfully") - return 0 - - except ConnectionError as e: - print(f"Connection error: {e}") - return -1 - except socket.timeout: - print("Timeout while sending disable command") - return -1 - except Exception as e: - print(f"Unexpected error: {e}") - return -1 - - def read(self): - """ - TCP Version - Read the sensor's data in passive mode. - Returns: - np.ndarray(6,) - Six-axis force/torque data [Fx, Fy, Fz, Mx, My, Mz] - None if reading fails - """ - try: - # Clear any existing data in the TCP buffer first - # self._clear_tcp_buffer() - - # Search for frame header (0x20 0x4E) - header_found = False - start_time = time.time() - timeout = 1.0 # 1 second timeout - - while not header_found and (time.time() - start_time < timeout): - # Read one byte at a time looking for header - byte1 = self.sock.recv(1) - if not byte1: - continue # No data available yet - - if byte1 == b'\x20': - byte2 = self.sock.recv(1) - if byte2 == b'\x4E': - header_found = True - - if not header_found: - print("Frame header not found within timeout period") - return None - - # Now read the remaining 14 bytes of the frame - response = bytearray([0x20, 0x4E]) # Start with the header we found - remaining_bytes = 14 - bytes_received = 0 - start_time = time.time() - - while bytes_received < remaining_bytes and (time.time() - start_time < timeout): - chunk = self.sock.recv(remaining_bytes - bytes_received) - if chunk: - response.extend(chunk) - bytes_received += len(chunk) - - if bytes_received < remaining_bytes: - print(f"Incomplete frame received. Got {len(response)} bytes, expected 16") - return None - - # Verify CRC checksum - Hi_check, Lo_check = self.crc16(response[:-2]) - if response[-1] != Hi_check or response[-2] != Lo_check: - print("CRC check failed!") - print(f"Received CRC: {response[-2]:02X}{response[-1]:02X}") - print(f"Calculated CRC: {Lo_check:02X}{Hi_check:02X}") - return None - - # Parse the sensor data - sensor_data = self.parse_data_passive(response) - return sensor_data - - except socket.timeout: - print("Timeout while waiting for sensor data") - return None - except ConnectionError as e: - print(f"TCP connection error: {e}") - return None - except Exception as e: - print(f"Unexpected error in TCP read(): {str(e)}") - return None - - def parse_data_passive(self, buffer): - values = [ - int.from_bytes(buffer[i:i+2], byteorder='little', signed=True) - for i in range(2, 14, 2) - ] - Fx, Fy, Fz = np.array(values[:3]) / 10.0 - Mx, My, Mz = np.array(values[3:]) / 1000.0 - return np.array([Fx, Fy, Fz, Mx, My, Mz]) - - -# 外部 -def test_sensor_frequency(sensor, mode='active', duration=5.0): - """ - 测试传感器在不同模式下的实际数据获取频率 - - 参数: - sensor: XjcSensor实例 - mode: 'active'(主动模式) 或 'polling'(查询模式) - duration: 测试持续时间(秒) - - 返回: - dict: 包含测试结果的字典 - """ - # 准备测试环境 - if mode == 'active': - print("\n=== 测试主动传输模式 ===") - sensor.enable_active_transmission() - read_func = sensor.read - else: - print("\n=== 测试查询模式 ===") - sensor.disable_active_transmission() # 确保不在主动模式 - time.sleep(0.5) - sensor.disable_active_transmission() # 确保不在主动模式 - time.sleep(0.5) - sensor.disable_active_transmission() # 确保不在主动模式 - time.sleep(0.5) - read_func = sensor.read_data_f32 - - # 初始化测试变量 - timestamps = [] - data_count = 0 - start_time = time.perf_counter() - end_time = start_time + duration - - print(f"开始测试，持续时间 {duration} 秒...") - - # 主测试循环 - while time.perf_counter() < end_time: - data = read_func() - print(data) - if data is not None: - timestamps.append(time.perf_counter()) - data_count += 1 - # print(timestamps) - # 计算统计结果 - if data_count < 2: - print("警告: 采集到的数据点不足") - return None - - intervals = np.diff(timestamps) - save_intervals_to_csv(timestamps) - avg_interval = np.mean(intervals) - min_interval = np.min(intervals) - max_interval = np.max(intervals) - std_interval = np.std(intervals) - avg_freq = 1.0 / avg_interval - - # 打印结果 - print("\n测试结果:") - print(f"总数据点数: {data_count}") - print(f"平均频率: {avg_freq:.2f} Hz") - print(f"最小间隔: {min_interval*1000:.3f} ms") - print(f"最大间隔: {max_interval*1000:.3f} ms") - print(f"间隔标准差: {std_interval*1000:.3f} ms") - - # 返回结果字典 - return { - 'mode': mode, - 'duration': duration, - 'data_count': data_count, - 'avg_freq': avg_freq, - 'min_interval': min_interval, - 'max_interval': max_interval, - 'std_interval': std_interval, - 'timestamps': timestamps, - 'intervals': intervals - } - -def plot_test_results(active_results, polling_results): - """绘制两种模式的测试结果对比图""" - plt.figure(figsize=(12, 8)) - - # 间隔时间分布图 - plt.subplot(2, 1, 1) - plt.hist(active_results['intervals']*1000, bins=50, alpha=0.7, label='主动模式') - plt.hist(polling_results['intervals']*1000, bins=50, alpha=0.7, label='查询模式') - plt.xlabel('间隔时间 (ms)') - plt.ylabel('出现次数') - plt.title('数据间隔时间分布') - plt.legend() - plt.grid(True) - - # 间隔时间序列图 - plt.subplot(2, 1, 2) - plt.plot(np.arange(len(active_results['intervals'])), - active_results['intervals']*1000, 'b.', label='主动模式') - plt.plot(np.arange(len(polling_results['intervals'])), - polling_results['intervals']*1000, 'r.', label='查询模式') - plt.xlabel('数据点序号') - plt.ylabel('间隔时间 (ms)') - plt.title('数据间隔时间序列') - plt.legend() - plt.grid(True) - - plt.tight_layout() - plt.show() - -def compare_modes(sensor, duration=5.0): - """比较两种工作模式的性能""" - # 测试主动模式 - active_results = test_sensor_frequency(sensor, 'active', duration) - time.sleep(1) # 模式切换间隔 - - # 测试查询模式 - polling_results = test_sensor_frequency(sensor, 'polling', duration) - time.sleep(1) - - # 打印对比报告 - print("\n=== 模式对比报告 ===") - print(f"{'指标':<15} {'主动模式':>15} {'查询模式':>15}") - print(f"{'平均频率(Hz)':<15} {active_results['avg_freq']:>15.2f} {polling_results['avg_freq']:>15.2f}") - print(f"{'最小间隔(ms)':<15} {active_results['min_interval']*1000:>15.3f} {polling_results['min_interval']*1000:>15.3f}") - print(f"{'最大间隔(ms)':<15} {active_results['max_interval']*1000:>15.3f} {polling_results['max_interval']*1000:>15.3f}") - print(f"{'间隔标准差(ms)':<15} {active_results['std_interval']*1000:>15.3f} {polling_results['std_interval']*1000:>15.3f}") - - # 绘制对比图表 - plot_test_results(active_results, polling_results) - - return active_results, polling_results - -def save_intervals_to_csv(timestamps, filename="sensor_intervals.csv"): - """ - Save timestamp intervals to CSV file with metadata - - Args: - timestamps: List of timestamp values (from time.perf_counter()) - filename: Output CSV filename - """ - if len(timestamps) < 2: - print("Not enough timestamps to calculate intervals") - return - - # Calculate intervals in milliseconds - intervals_ms = np.diff(timestamps) * 1000 - - # Prepare data rows - rows = [] - for i, interval in enumerate(intervals_ms): - rows.append({ - "reading_number": i+1, - "interval_ms": f"{interval:.4f}", - "timestamp": timestamps[i], - "expected_interval_ms": (1000/250) if i==0 else "", # For 250Hz - "deviation_ms": f"{interval - (1000/250):.4f}" if i>0 else "" - }) - - # CSV header - fieldnames = ["reading_number", "interval_ms", "timestamp", - "expected_interval_ms", "deviation_ms"] - - # Write to file - with open(filename, 'w', newline='') as csvfile: - writer = csv.DictWriter(csvfile, fieldnames=fieldnames) - writer.writeheader() - writer.writerows(rows) - - print(f"Saved {len(intervals_ms)} intervals to {filename}") - if __name__ == "__main__": - # 替换为你的传感器 IP 和端口 - sensor = XjcSensor("192.168.5.1", 60000) - sensor.connect() - # 示例操作 - - sensor.disable_active_transmission() - time.sleep(0.5) - sensor.disable_active_transmission() - time.sleep(0.5) - sensor.disable_active_transmission() - time.sleep(0.5) - sensor.set_zero() - - # sensor.set_zero() - # sensor.disable_active_transmission() - - # while True: - # data = sensor.read_data_f32() - # if data is not None: - # print(f"Force data: {data}") - # sensor.enable_active_transmission() - # while True: - # sensor_data = sensor.read() - # if sensor_data is None: - # print('failed to get force sensor data!') - # print(sensor_data) - test_sensor_frequency(sensor) - # compare_modes(sensor) \ No newline at end of file + import sys + import pathlib + sys.path.append(str(pathlib.Path(__file__).parent.parent)) + from tools.Rate import Rate + import signal + import sys + rate = Rate(250) + xjc_sensor = XjcSensor() + def signal_handler(sig, frame): + print('Received Ctrl+C, exiting gracefully...') + sys.exit(0) + signal.signal(signal.SIGINT, lambda signum, frame: sys.exit(0)) + # atexit.register(xjc_sensor.disconnect) + time.sleep(1) + xjc_sensor.disable_active_transmission() + xjc_sensor.disable_active_transmission() + xjc_sensor.disable_active_transmission() + print(xjc_sensor.read_data_f32()) + print(xjc_sensor.read_data_f32()) + print(xjc_sensor.read_data_f32()) + # print(xjc_sensor.read()) + time.sleep(1) + xjc_sensor.set_zero() + xjc_sensor.set_zero() + xjc_sensor.set_zero() + # time.sleep(1) + # xjc_sensor.enable_active_transmission() + while True: + # sensor_data = xjc_sensor.read() + sensor_data = xjc_sensor.read_data_f32() + current_time = time.strftime("%Y-%m-%d_%H-%M-%S") + if sensor_data is None: + print('failed to get force sensor data!') + print(f"{current_time}-----{sensor_data}") + rate.sleep(False) + # break diff --git a/Massage/MassageControl/logs/MassageRobot_nova5_test.log b/Massage/MassageControl/logs/MassageRobot_nova5_test.log new file mode 100644 index 0000000..e69de29 diff --git a/Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/serial_tools.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..990b67d9063d0e74b4de5472c6103e920c01b613 GIT binary patch literal 2513 zcmb_e|8EpU6rcTid)K?`wO|XS;6@F}nS-i{iD|$XsR<-r-H936l@%W^b-R^@*&^Ad3e*Z_aP^0aVCQ3xAYF07C#^5OIc zs#A0ZL0)9V8-xzBL0Z~NfExmCSaQ37+by|gfE$tAC~$iuw->lEvd8aC%!f@jaVDCZP02|wioDZoI-CuKJB4(jC)(_f zg@NEz?q#fSWj_@jOmtkXT622`NFdlF@C}Z>~low;K+5PU_IMK0FmSl|elY zOfRU!$G~DjzOtxN1w*?)cnJdwO^0ScGofXmSyWvzJ5uWiIalgn3Taix6yn0WGjEp7 zG?vQ+c6efU>C0{TALd$v3^AJ?GU$n=@X{KJkY1fn3jvU>+YtA{|mf4vQ2# zgwAoY?PxYluO0ogPp03Nj(gr0^Dbq=^Bd7HtdI5pArN9+g)pnSvh{mh%yQJIqqjCc zOiw&kdTtsR^b(XQlmJkzqd;kyi<%&3l#WJq=v8dgxT-Je9bJGQj}W0#@FD1D`d7&^ zw7`ei_OGyU&W5jh?N0ZzTWhNq*H*u`BTNwc(JwbPZe3Zw`EB>p8{PZ&9({3r{mPeX zt3P)?|N7y@kL9Y#mdCxO%~>-P%=KH7uxJSut*zc)U%F?XsM+16yWN`?cTmQPds=(|asBe6pB{9-`J{X8>c)3><(habHM@4|{R;QaS6F>MJo-|l z;Rk1r#)f_BSZ(sLGI^@3B}O#Q8jZw^1P$9l_Kfp*Y({uoFxtC*i-&a2%v@gfhZeJcFR4^K##@CV}c?S$?G|V zbUDBlWGYQEy?wS9VuYhB{2M>$J84K;2Q6wG{wa=ymK-^C%GprckifUGjx zcz}#a_oJotAHUimtF|RjpC|r1(2mhM>o*#)dT0(~7PeYD$L#rPX!CXu_`&Iag|r<| z8Q3a-#~4`4M}Z`I-GjWy3dWm$;0c!K=RIHWeK1#6`8ewBN3snecST^twwSyMdJ#eh zF)h_nhaoDKYAPj_KL@hJa^0rq2d*2B1A^H#e6(x`ye;OL&+VxcHYQ_hO6JI<9S>FP z8TpdgRr!)l+7h+mOvQfh#1xDWzHn5NBgC;dU$NirVLd1ys>Op9*nA5YL5eoUrHXyB z4i8GB}Ch3)YMzmM57W=57aR88rE#Sx1^7Z8zBK l`1d1v0mv`}$I8HO7{GbZ+HEN&0JNo)l&WfF0kR=*{{YzMuVnxL literal 0 HcmV?d00001 diff --git a/Massage/MassageControl/tools/__pycache__/ssh_tools.cpython-39.pyc b/Massage/MassageControl/tools/__pycache__/ssh_tools.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..35fde28f971aaaefdc543dcc4f18628e7fe285b0 GIT binary patch literal 671 zcmYjPON-P%5bj5k>6vvF@de^t$U(=W2N9R?;6azgIS4@r*>ulNW|P#=-BD-hxq0>M zb#nAC^-VnOUl0VVz`pyXX706BDNM50u2MCf#T9b^( zSaKPCr5TlULDJ+BbL99kl$3GsujBx&K%JwSPY9OmX-{^si}wtD-jkkEa&h9~b>xVP z9Cb|6ZxJO}5j)<~k9?QxMmym~`=lrPkz_sHjh(o`CT{E|OZFl3h@Mcg`0qW#k#RtN zoJL-(z}U8El#e!`trwk9df;MH81oslyofuyeDXXUdkpEKsm>uAFBWg+b){OHJu=GX zO=V2gp5@`R{9JAG4b)Y+&8A14g(E3X2I@y8w5=*_c1wLxWnMzl6s`1;R)zG5R3%8| z$(gPob({y+`ogjqnp|mAFK0&fU9wPz$gXp{1@%!y; z1&x}m&GPggyu%HPGxZ)$!RD_6&aOh&b`Gu8P|qwtZDx35K6p