From a6e9d0d73445f4aa41152606c346b2501d82933e Mon Sep 17 00:00:00 2001 From: pengqi Date: Mon, 5 Jan 2026 18:11:56 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BA=BF=E6=9D=A1=E5=8E=82=E5=90=84=E8=AE=BE?= =?UTF-8?q?=E5=A4=87=E6=8E=A7=E5=88=B6=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- DM/DM_CAN.py | 631 ++++++++++++++++ DM/DM_Motor.py | 210 ++++++ RK1106/.idea/.gitignore | 3 + RK1106/.idea/Python.iml | 8 + .../inspectionProfiles/profiles_settings.xml | 6 + RK1106/.idea/misc.xml | 4 + RK1106/.idea/modules.xml | 8 + RK1106/ip.sh | 18 + RK1106/readme.md | 5 + RK1106/stepper_motor.py | 210 ++++++ RK1106/stepper_motor_test.py | 159 ++++ RK1106/stepper_motor_test1.py | 198 +++++ RK1106/test.py | 292 +++++++ UI/test.py | 564 ++++++++++++++ UI/ui.py | 568 ++++++++++++++ UI/线条参数记录_20251208_142616.txt | 6 + requirements.txt | 5 + servo/servo.py | 183 +++++ servo/servo_sdk/__init__.py | 3 + servo/servo_sdk/hx_30_hm.py | 187 +++++ servo/servo_sdk/port_handler.py | 113 +++ servo/servo_sdk/protocol_paket_handler.py | 713 ++++++++++++++++++ servo/servo_test.py | 195 +++++ 23 files changed, 4289 insertions(+) create mode 100644 DM/DM_CAN.py create mode 100644 DM/DM_Motor.py create mode 100644 RK1106/.idea/.gitignore create mode 100644 RK1106/.idea/Python.iml create mode 100644 RK1106/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 RK1106/.idea/misc.xml create mode 100644 RK1106/.idea/modules.xml create mode 100644 RK1106/ip.sh create mode 100644 RK1106/readme.md create mode 100644 RK1106/stepper_motor.py create mode 100644 RK1106/stepper_motor_test.py create mode 100644 RK1106/stepper_motor_test1.py create mode 100644 RK1106/test.py create mode 100644 UI/test.py create mode 100644 UI/ui.py create mode 100644 UI/线条参数记录_20251208_142616.txt create mode 100644 requirements.txt create mode 100644 servo/servo.py create mode 100644 servo/servo_sdk/__init__.py create mode 100644 servo/servo_sdk/hx_30_hm.py create mode 100644 servo/servo_sdk/port_handler.py create mode 100644 servo/servo_sdk/protocol_paket_handler.py create mode 100644 servo/servo_test.py diff --git a/DM/DM_CAN.py b/DM/DM_CAN.py new file mode 100644 index 0000000..ba0cfb1 --- /dev/null +++ b/DM/DM_CAN.py @@ -0,0 +1,631 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# Description：电机库 +''' + +from time import sleep +import numpy as np +from enum import IntEnum +from struct import unpack +from struct import pack + + +class Motor: + def __init__(self, MotorType, SlaveID, MasterID): + """ + define Motor object 定义电机对象 + :param MotorType: Motor type 电机类型 + :param SlaveID: CANID 电机ID + :param MasterID: MasterID 主机ID 建议不要设为0 + """ + self.Pd = float(0) + self.Vd = float(0) + self.state_q = float(0) + self.state_dq = float(0) + self.state_tau = float(0) + self.SlaveID = SlaveID + self.MasterID = MasterID + self.MotorType = MotorType + self.isEnable = False + self.NowControlMode = Control_Type.MIT + self.temp_param_dict = {} + + def recv_data(self, q: float, dq: float, tau: float): + self.state_q = q + self.state_dq = dq + self.state_tau = tau + + def getPosition(self): + """ + get the position of the motor 获取电机位置 + :return: the position of the motor 电机位置 + """ + return self.state_q + + def getVelocity(self): + """ + get the velocity of the motor 获取电机速度 + :return: the velocity of the motor 电机速度 + """ + return self.state_dq + + def getTorque(self): + """ + get the torque of the motor 获取电机力矩 + :return: the torque of the motor 电机力矩 + """ + return self.state_tau + + def getParam(self, RID): + """ + get the parameter of the motor 获取电机内部的参数，需要提前读取 + :param RID: DM_variable 电机参数 + :return: the parameter of the motor 电机参数 + """ + if RID in self.temp_param_dict: + return self.temp_param_dict[RID] + else: + return None + + +class MotorControl: + send_data_frame = np.array( + [0x55, 0xAA, 0x1e, 0x03, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08, 0x00, + 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0x00], np.uint8) + # 4310 4310_48 4340 4340_48 + Limit_Param = [[300, 30, 10], [12.5, 50, 10], [12.5, 8, 28], [12.5, 10, 28], + # 6006 8006 8009 10010L 10010 + [12.5, 45, 20], [12.5, 45, 40], [12.5, 45, 54], [12.5, 25, 200], [12.5, 20, 200], + # H3510 DMG6215 DMH6220 + [12.5 , 280 , 1],[12.5 , 45 , 10],[12.5 , 45 , 10]] + + def __init__(self, serial_device): + """ + define MotorControl object 定义电机控制对象 + :param serial_device: serial object 串口对象 + """ + self.serial_ = serial_device + self.motors_map = dict() + self.data_save = bytes() # save data + if self.serial_.is_open: # open the serial port + print("Serial port is open") + serial_device.close() + self.serial_.open() + + def controlMIT(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float): + """ + MIT Control Mode Function 达妙电机MIT控制模式函数 + :param DM_Motor: Motor object 电机对象 + :param kp: kp + :param kd: kd + :param q: position 期望位置 + :param dq: velocity 期望速度 + :param tau: torque 期望力矩 + :return: None + """ + if DM_Motor.SlaveID not in self.motors_map: + print("controlMIT ERROR : Motor ID not found") + return + kp_uint = float_to_uint(kp, 0, 500, 12) + kd_uint = float_to_uint(kd, 0, 5, 12) + MotorType = DM_Motor.MotorType + Q_MAX = self.Limit_Param[MotorType][0] + DQ_MAX = self.Limit_Param[MotorType][1] + TAU_MAX = self.Limit_Param[MotorType][2] + q_uint = float_to_uint(q, -Q_MAX, Q_MAX, 16) + dq_uint = float_to_uint(dq, -DQ_MAX, DQ_MAX, 12) + tau_uint = float_to_uint(tau, -TAU_MAX, TAU_MAX, 12) + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + data_buf[0] = (q_uint >> 8) & 0xff + data_buf[1] = q_uint & 0xff + data_buf[2] = dq_uint >> 4 + data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf) + data_buf[4] = kp_uint & 0xff + data_buf[5] = kd_uint >> 4 + data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf) + data_buf[7] = tau_uint & 0xff + self.__send_data(DM_Motor.SlaveID, data_buf) + self.recv() # receive the data from serial port + + def control_delay(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float, delay: float): + """ + MIT Control Mode Function with delay 达妙电机MIT控制模式函数带延迟 + :param DM_Motor: Motor object 电机对象 + :param kp: kp + :param kd: kd + :param q: position 期望位置 + :param dq: velocity 期望速度 + :param tau: torque 期望力矩 + :param delay: delay time 延迟时间 单位秒 + """ + self.controlMIT(DM_Motor, kp, kd, q, dq, tau) + sleep(delay) + + def control_Pos_Vel(self, Motor, P_desired: float, V_desired: float): + """ + control the motor in position and velocity control mode 电机位置速度控制模式 + :param Motor: Motor object 电机对象 + :param P_desired: desired position 期望位置 + :param V_desired: desired velocity 期望速度 + :return: None + """ + if Motor.SlaveID not in self.motors_map: + print("Control Pos_Vel Error : Motor ID not found") + return + motorid = 0x100 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + P_desired_uint8s = float_to_uint8s(P_desired) + V_desired_uint8s = float_to_uint8s(V_desired) + data_buf[0:4] = P_desired_uint8s + data_buf[4:8] = V_desired_uint8s + self.__send_data(motorid, data_buf) + # time.sleep(0.001) + self.recv() # receive the data from serial port + + def control_Vel(self, Motor, Vel_desired): + """ + control the motor in velocity control mode 电机速度控制模式 + :param Motor: Motor object 电机对象 + :param Vel_desired: desired velocity 期望速度 + """ + if Motor.SlaveID not in self.motors_map: + print("control_VEL ERROR : Motor ID not found") + return + motorid = 0x200 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + Vel_desired_uint8s = float_to_uint8s(Vel_desired) + data_buf[0:4] = Vel_desired_uint8s + self.__send_data(motorid, data_buf) + self.recv() # receive the data from serial port + + def control_pos_force(self, Motor, Pos_des: float, Vel_des, i_des): + """ + control the motor in EMIT control mode 电机力位混合模式 + :param Pos_des: desired position rad 期望位置 单位为rad + :param Vel_des: desired velocity rad/s 期望速度 为放大100倍 + :param i_des: desired current rang 0-10000 期望电流标幺值放大10000倍 + 电流标幺值：实际电流值除以最大电流值，最大电流见上电打印 + """ + if Motor.SlaveID not in self.motors_map: + print("control_pos_vel ERROR : Motor ID not found") + return + motorid = 0x300 + Motor.SlaveID + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + Pos_desired_uint8s = float_to_uint8s(Pos_des) + data_buf[0:4] = Pos_desired_uint8s + Vel_uint = np.uint16(Vel_des) + ides_uint = np.uint16(i_des) + data_buf[4] = Vel_uint & 0xff + data_buf[5] = Vel_uint >> 8 + data_buf[6] = ides_uint & 0xff + data_buf[7] = ides_uint >> 8 + self.__send_data(motorid, data_buf) + self.recv() # receive the data from serial port + + def enable(self, Motor): + """ + enable motor 使能电机 + 最好在上电后几秒后再使能电机 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFC)) + sleep(0.1) + self.recv() # receive the data from serial port + + def enable_old(self, Motor ,ControlMode): + """ + enable motor old firmware 使能电机旧版本固件，这个是为了旧版本电机固件的兼容性 + 可恶的旧版本固件使能需要加上偏移量 + 最好在上电后几秒后再使能电机 + :param Motor: Motor object 电机对象 + """ + data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc], np.uint8) + enable_id = ((int(ControlMode)-1) << 2) + Motor.SlaveID + self.__send_data(enable_id, data_buf) + sleep(0.1) + self.recv() # receive the data from serial port + + def disable(self, Motor): + """ + disable motor 失能电机 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFD)) + sleep(0.01) + + def set_zero_position(self, Motor): + """ + set the zero position of the motor 设置电机0位 + :param Motor: Motor object 电机对象 + """ + self.__control_cmd(Motor, np.uint8(0xFE)) + sleep(0.1) + self.recv() # receive the data from serial port + + def recv(self): + # 把上次没有解析完的剩下的也放进来 + data_recv = b''.join([self.data_save, self.serial_.read_all()]) + packets = self.__extract_packets(data_recv) + for packet in packets: + data = packet[7:15] + CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3] + CMD = packet[1] + self.__process_packet(data, CANID, CMD) + + def recv_set_param_data(self): + data_recv = self.serial_.read_all() + packets = self.__extract_packets(data_recv) + for packet in packets: + data = packet[7:15] + CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3] + CMD = packet[1] + self.__process_set_param_packet(data, CANID, CMD) + + def __process_packet(self, data, CANID, CMD): + if CMD == 0x11: + if CANID != 0x00: + if CANID in self.motors_map: + q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2]) + dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4)) + tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5]) + MotorType_recv = self.motors_map[CANID].MotorType + Q_MAX = self.Limit_Param[MotorType_recv][0] + DQ_MAX = self.Limit_Param[MotorType_recv][1] + TAU_MAX = self.Limit_Param[MotorType_recv][2] + recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16) + recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12) + recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12) + self.motors_map[CANID].recv_data(recv_q, recv_dq, recv_tau) + else: + MasterID=data[0] & 0x0f + if MasterID in self.motors_map: + q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2]) + dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4)) + tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5]) + MotorType_recv = self.motors_map[MasterID].MotorType + Q_MAX = self.Limit_Param[MotorType_recv][0] + DQ_MAX = self.Limit_Param[MotorType_recv][1] + TAU_MAX = self.Limit_Param[MotorType_recv][2] + recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16) + recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12) + recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12) + self.motors_map[MasterID].recv_data(recv_q, recv_dq, recv_tau) + + + def __process_set_param_packet(self, data, CANID, CMD): + if CMD == 0x11 and (data[2] == 0x33 or data[2] == 0x55): + masterid=CANID + slaveId = ((data[1] << 8) | data[0]) + if CANID==0x00: #防止有人把MasterID设为0稳一手 + masterid=slaveId + + if masterid not in self.motors_map: + if slaveId not in self.motors_map: + return + else: + masterid=slaveId + + RID = data[3] + # 读取参数得到的数据 + if is_in_ranges(RID): + #uint32类型 + num = uint8s_to_uint32(data[4], data[5], data[6], data[7]) + self.motors_map[masterid].temp_param_dict[RID] = num + + else: + #float类型 + num = uint8s_to_float(data[4], data[5], data[6], data[7]) + self.motors_map[masterid].temp_param_dict[RID] = num + + + def addMotor(self, Motor): + """ + add motor to the motor control object 添加电机到电机控制对象 + :param Motor: Motor object 电机对象 + """ + self.motors_map[Motor.SlaveID] = Motor + if Motor.MasterID != 0: + self.motors_map[Motor.MasterID] = Motor + return True + + def __control_cmd(self, Motor, cmd: np.uint8): + data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, cmd], np.uint8) + self.__send_data(Motor.SlaveID, data_buf) + + def __send_data(self, motor_id, data): + """ + send data to the motor 发送数据到电机 + :param motor_id: + :param data: + :return: + """ + self.send_data_frame[13] = motor_id & 0xff + self.send_data_frame[14] = (motor_id >> 8)& 0xff #id high 8 bits + self.send_data_frame[21:29] = data + self.serial_.write(bytes(self.send_data_frame.T)) + + def __read_RID_param(self, Motor, RID): + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x33, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8) + self.__send_data(0x7FF, data_buf) + + def __write_motor_param(self, Motor, RID, data): + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x55, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8) + if not is_in_ranges(RID): + # data is float + data_buf[4:8] = float_to_uint8s(data) + else: + # data is int + data_buf[4:8] = data_to_uint8s(int(data)) + self.__send_data(0x7FF, data_buf) + + def switchControlMode(self, Motor, ControlMode): + """ + switch the control mode of the motor 切换电机控制模式 + :param Motor: Motor object 电机对象 + :param ControlMode: Control_Type 电机控制模式 example:MIT:Control_Type.MIT MIT模式 + """ + max_retries = 10 + retry_interval = 0.05 #retry times + RID = 10 + self.__write_motor_param(Motor, RID, np.uint8(ControlMode)) + for _ in range(max_retries): + sleep(retry_interval) + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map: + if RID in self.motors_map[Motor.SlaveID].temp_param_dict: + if self.motors_map[Motor.SlaveID].temp_param_dict[RID] == ControlMode: + return True + else: + return False + return False + + def save_motor_param(self, Motor): + """ + save the all parameter to flash 保存所有电机参数 + :param Motor: Motor object 电机对象 + :return: + """ + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + self.disable(Motor) # before save disable the motor + self.__send_data(0x7FF, data_buf) + sleep(0.001) + + def change_limit_param(self, Motor_Type, PMAX, VMAX, TMAX): + """ + change the PMAX VMAX TMAX of the motor 改变电机的PMAX VMAX TMAX + :param Motor_Type: + :param PMAX: 电机的PMAX + :param VMAX: 电机的VMAX + :param TMAX: 电机的TMAX + :return: + """ + self.Limit_Param[Motor_Type][0] = PMAX + self.Limit_Param[Motor_Type][1] = VMAX + self.Limit_Param[Motor_Type][2] = TMAX + + def refresh_motor_status(self, Motor): + """ + get the motor status 获得电机状态 + """ + can_id_l = Motor.SlaveID & 0xff #id low 8 bits + can_id_h = (Motor.SlaveID >> 8) & 0xff #id high 8 bits + data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xCC, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + self.__send_data(0x7FF, data_buf) + self.recv() # receive the data from serial port + + def change_motor_param(self, Motor, RID, data): + """ + change the RID of the motor 改变电机的参数 + :param Motor: Motor object 电机对象 + :param RID: DM_variable 电机参数 + :param data: 电机参数的值 + :return: True or False ,True means success, False means fail + """ + max_retries = 20 + retry_interval = 0.05 #retry times + + self.__write_motor_param(Motor, RID, data) + for _ in range(max_retries): + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map and RID in self.motors_map[Motor.SlaveID].temp_param_dict: + if abs(self.motors_map[Motor.SlaveID].temp_param_dict[RID] - data) < 0.1: + return True + else: + return False + sleep(retry_interval) + return False + + def read_motor_param(self, Motor, RID): + """ + read only the RID of the motor 读取电机的内部信息例如 版本号等 + :param Motor: Motor object 电机对象 + :param RID: DM_variable 电机参数 + :return: 电机参数的值 + """ + max_retries = 20 + retry_interval = 0.05 #retry times + self.__read_RID_param(Motor, RID) + for _ in range(max_retries): + sleep(retry_interval) + self.recv_set_param_data() + if Motor.SlaveID in self.motors_map: + if RID in self.motors_map[Motor.SlaveID].temp_param_dict: + return self.motors_map[Motor.SlaveID].temp_param_dict[RID] + else: + return None + return None + + # ------------------------------------------------- + # Extract packets from the serial data + def __extract_packets(self, data): + frames = [] + header = 0xAA + tail = 0x55 + frame_length = 16 + i = 0 + remainder_pos = 0 + + while i <= len(data) - frame_length: + if data[i] == header and data[i + frame_length - 1] == tail: + frame = data[i:i + frame_length] + frames.append(frame) + i += frame_length + remainder_pos = i + else: + i += 1 + self.data_save = data[remainder_pos:] + return frames + + +def LIMIT_MIN_MAX(x, min, max): + if x <= min: + x = min + elif x > max: + x = max + + +def float_to_uint(x: float, x_min: float, x_max: float, bits): + LIMIT_MIN_MAX(x, x_min, x_max) + span = x_max - x_min + data_norm = (x - x_min) / span + return np.uint16(data_norm * ((1 << bits) - 1)) + + +def uint_to_float(x: np.uint16, min: float, max: float, bits): + span = max - min + data_norm = float(x) / ((1 << bits) - 1) + temp = data_norm * span + min + return np.float32(temp) + + +def float_to_uint8s(value): + # Pack the float into 4 bytes + packed = pack('f', value) + # Unpack the bytes into four uint8 values + return unpack('4B', packed) + + +def data_to_uint8s(value): + # Check if the value is within the range of uint32 + if isinstance(value, int) and (0 <= value <= 0xFFFFFFFF): + # Pack the uint32 into 4 bytes + packed = pack('I', value) + else: + raise ValueError("Value must be an integer within the range of uint32") + + # Unpack the bytes into four uint8 values + return unpack('4B', packed) + + +def is_in_ranges(number): + """ + check if the number is in the range of uint32 + :param number: + :return: + """ + if (7 <= number <= 10) or (13 <= number <= 16) or (35 <= number <= 36): + return True + return False + + +def uint8s_to_uint32(byte1, byte2, byte3, byte4): + # Pack the four uint8 values into a single uint32 value in little-endian order + packed = pack('<4B', byte1, byte2, byte3, byte4) + # Unpack the packed bytes into a uint32 value + return unpack(' + + + + + + + \ No newline at end of file diff --git a/RK1106/.idea/inspectionProfiles/profiles_settings.xml b/RK1106/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/RK1106/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/RK1106/.idea/misc.xml b/RK1106/.idea/misc.xml new file mode 100644 index 0000000..d56657a --- /dev/null +++ b/RK1106/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/RK1106/.idea/modules.xml b/RK1106/.idea/modules.xml new file mode 100644 index 0000000..3097039 --- /dev/null +++ b/RK1106/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/RK1106/ip.sh b/RK1106/ip.sh new file mode 100644 index 0000000..23353a0 --- /dev/null +++ b/RK1106/ip.sh @@ -0,0 +1,18 @@ +#!/bin/sh + +# 网络配置脚本：仅设置eth0静态IP（无网关/DNS） +case "$1" in + start) + # 配置eth0静态IP地址和子网掩码（移除多余的反斜杠） + ifconfig eth0 192.168.5.100 netmask 255.255.255.0 + # 可选：添加执行成功提示，方便确认 + echo "✅ eth0已配置静态IP：192.168.5.100/24" + ;; + stop) + ;; + *) + # 补充提示信息，告诉用户正确用法 + echo "❌ 使用方法：$0 {start|stop}" + exit 1 + ;; +esac \ No newline at end of file diff --git a/RK1106/readme.md b/RK1106/readme.md new file mode 100644 index 0000000..30a7bb0 --- /dev/null +++ b/RK1106/readme.md @@ -0,0 +1,5 @@ +# RK1106的IP地址 +192.168.5.100，自启动设置了IP + +# 测试： +先使用stepper_motor_test1进行单独测试，看会不会掉步 \ No newline at end of file diff --git a/RK1106/stepper_motor.py b/RK1106/stepper_motor.py new file mode 100644 index 0000000..2098eff --- /dev/null +++ b/RK1106/stepper_motor.py @@ -0,0 +1,210 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2026/1/5 15:50 +# @Author : reenrr +# @File : stepper_motor.py +# @Desc : 控制步进电机从初始位置移动10cm,移动后回到初始位置 +''' +import time +from periphery import GPIO +import logging + +# ------------参数配置------------- +# 1. 脉冲（PUL）引脚配置 → GPIO32 +PUL_Pin = 32 + +# 2. 方向（DIR）引脚配置 → GPIO33 +DIR_Pin = 33 + +# 3. 驱动器参数（根据拨码调整，默认不变） +PULSES_PER_ROUND = 400 # 每圈脉冲数（SW5~SW8拨码，默认400） +PULSE_FREQUENCY = 2500 # 脉冲频率（Hz） + +# ------------ 日志+参数配置 ------------ +script_dir = os.path.dirname(os.path.abspath(__file__)) +log_file_path = os.path.join(script_dir, "stepper_motor.log") + +logging.basicConfig( + level=logging.INFO, + format='[%(asctime)s.%(msecs)03d] [%(levelname)s] %(message)s', + datefmt='%Y-%m-%d %H:%M:%S', + handlers=[ + logging.StreamHandler(), + logging.FileHandler(log_file_path, encoding='utf-8') + ] +) + +class StepperMotor: + """新力川MA860H驱动器步进电机控制类""" + # 方向常量定义 + CLOCKWISE = "clockwise" # 顺时针 + COUNTER_CLOCKWISE = "counterclockwise" # 逆时针 + + def __init__(self, + pul_pin: int = PUL_Pin, + dir_pin: int = DIR_Pin, + pulses_per_round: int = PULSES_PER_ROUND, + pulse_frequency: int = PULSE_FREQUENCY, + clockwise_level: bool = True, + counter_clockwise_level: bool = False): + """ + 初始化步进电机控制器 + :param pul_pin: 脉冲引脚 + :param dir_pin: 方向引脚 + :param pulses_per_round: 每圈脉冲数（SW5~SW8拨码，默认400） + :param pulse_frequency: 脉冲频率（Hz，） + :param clockwise_level: 顺时针对应的DIR电平 + :param counter_clockwise_level: 逆时针对应的DIR电平 + """ + # 硬件配置参数 + self.pul_pin = pul_pin + self.dir_pin = dir_pin + + # 驱动器参数 + self.pulses_per_round = pulses_per_round + self.pulse_frequency = pulse_frequency + self.clockwise_level = clockwise_level + self.counter_clockwise_level = counter_clockwise_level + + # GPIO对象初始化 + self.pul_gpio = None + self.dir_gpio = None + + # 初始化GPIO + self._init_gpio() + + def _init_gpio(self): + """初始化PUL和DIR引脚""" + try: + # 初始化脉冲引脚（输出模式） + self.pul_gpio = GPIO(self.pul_pin, "out") + # 初始化方向引脚（输出模式） + self.dir_gpio = GPIO(self.dir_pin, "out") + + # 初始电平置低（避免电机误动作） + self.pul_gpio.write(False) + self.dir_gpio.write(False) + + logging.info(f"PUL引脚初始化完成：{self.pul_pin} 引脚") + logging.info(f"DIR引脚初始化完成：{self.dir_pin} 引脚") + + except PermissionError: + raise RuntimeError("权限不足！请用sudo运行程序（sudo python xxx.py）") + except Exception as e: + raise RuntimeError(f"GPIO初始化失败：{str(e)}") from e + + def _validate_rounds(self, rounds: float) -> bool: + """验证圈数是否合法（内部方法）""" + if rounds <= 0: + logging.info("圈数必须为正数") + return False + return True + + def _validate_direction(self, direction: str) -> bool: + """验证方向参数是否合法（内部方法）""" + if direction not in [self.CLOCKWISE, self.COUNTER_CLOCKWISE]: + logging.info(f"方向参数错误：仅支持 {self.CLOCKWISE}/{self.COUNTER_CLOCKWISE}") + return False + return True + + def rotate(self, rounds: float, direction: str = CLOCKWISE): + """ + 控制电机旋转（支持正反转） + :param rounds: 旋转圈数（可小数，如0.5=半圈） + :param direction: 方向（clockwise=顺时针，counterclockwise=逆时针） + """ + # 参数验证 + if not self._validate_rounds(rounds) or not self._validate_direction(direction): + return + + # 设置旋转方向（DIR电平） + if direction == self.CLOCKWISE: # 顺时针 + self.dir_gpio.write(self.clockwise_level) + logging.info(f"\n=== 顺时针旋转 {rounds} 圈 ===") + else: # 逆时针 + self.dir_gpio.write(self.counter_clockwise_level) + logging.info(f"\n=== 逆时针旋转 {rounds} 圈 ===") + + # 计算总脉冲数和时序（精准控制，避免丢步） + total_pulses = int(rounds * self.pulses_per_round) + pulse_period = 1.0 / self.pulse_frequency # 脉冲周期（秒） + half_period = pulse_period / 2 # 占空比50%（MA860H最优） + + logging.info(f"总脉冲数：{total_pulses} | 频率：{self.pulse_frequency}Hz | 周期：{pulse_period * 1000:.2f}ms") + start_time = time.perf_counter() # 高精度计时（避免丢步） + + # 发送脉冲序列（核心：占空比50%的方波） + for _ in range(total_pulses): + # 高电平 + self.pul_gpio.write(True) + # 精准延时（比time.sleep稳定，适配高频脉冲） + while time.perf_counter() - start_time < half_period: + pass + # 低电平 + self.pul_gpio.write(False) + # 更新下一个脉冲的起始时间 + start_time += pulse_period + + logging.info("旋转完成") + + def stop(self): + """紧急停止（置低脉冲引脚）""" + if self.pul_gpio: + self.pul_gpio.write(False) + logging.info("电机已停止") + + def close(self): + """释放GPIO资源""" + # 安全释放GPIO资源（关键：避免引脚电平残留） + if self.pul_gpio: + self.pul_gpio.write(False) # 脉冲引脚置低 + self.pul_gpio.close() + logging.info("\n PUL引脚已关闭（电平置低）") + + if self.dir_gpio: + self.dir_gpio.write(False) # 方向引脚置低 + self.dir_gpio.close() + logging.info("DIR引脚已关闭（电平置低）") + + # 重置GPIO对象 + self.pul_gpio = None + self.dir_gpio = None + + def __del__(self): + """析构函数：确保资源释放""" + self.close() + + +#---------控制步进电机外部接口-------------- +def stepper_motor_control(): + motor = None + try: + # 创建电机实例（使用默认配置） + motor = StepperMotor() + logging.info("\n=== 步进电机控制程序启动 ===") + + # 靠近电机方向 逆时针 + motor.rotate(rounds=10.0, direction=motor.COUNTER_CLOCKWISE) + time.sleep(5) # 暂停5秒 + # 远离电机方向 顺时针 + motor.rotate(rounds=10.0, direction=motor.CLOCKWISE) + time.sleep(5) # 暂停5秒 + + except PermissionError: + logging.info("\n 权限不足：请用 sudo 运行！") + logging.info("命令：sudo python3 double_direction_motor.py") + except ImportError: + logging.info("\n 缺少依赖：请安装python-periphery") + logging.info("命令：pip install python-periphery") + except Exception as e: + logging.info(f"\n 程序异常：{str(e)}") + finally: + if motor: + motor.close() + logging.info("程序退出完成") + + +if __name__ == '__main__': + stepper_motor_control() + diff --git a/RK1106/stepper_motor_test.py b/RK1106/stepper_motor_test.py new file mode 100644 index 0000000..3c113c6 --- /dev/null +++ b/RK1106/stepper_motor_test.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +# @Time : 2026/1/4 15:06 +# @Author : reenrr +# @File : stepper_motor_test.py +# @Desc : 线条厂控制步进电机 +""" +import time +from periphery import GPIO +import sys +sys.setswitchinterval(0.000001) # 减小线程切换间隔 + +# -------------- +# 硬件参数配置（必须与你的设备匹配！） +# -------------- +# 1. 脉冲（PUL）引脚配置 → GPIO32 +PUL_Pin = 32 + +# 2. 方向（DIR）引脚配置 → GPIO33 +DIR_Pin = 33 + +# 3. LCDA257C驱动器核心参数（关键！与拨码/软件设置一致） +PULSES_PER_ROUND = 400 # 每圈脉冲数（按驱动器细分拨码调整） +PULSE_FREQUENCY = 5000 # 初始测试频率（建议从500开始逐步调高） + +# 4. LCDA257C时序参数（遵循手册要求，不可低于最小值） +MIN_DIR_DELAY_US = 10 # DIR提前PUL的最小延迟（>8us） +PULSE_LOW_MIN_US = 2 # 脉冲低电平最小宽度（≥2us） +PULSE_HIGH_MIN_US = 2 # 脉冲高电平最小宽度（≥2us） + +# 5. 方向电平定义（可根据实际测试调整） +CLOCKWISE_LEVEL = True # 顺时针→DIR高电平 +COUNTER_CLOCKWISE_LEVEL = False # 逆时针→DIR低电平 + + +def init_stepper() -> tuple[GPIO, GPIO]: + """初始化PUL和DIR引脚（输出模式，适配LCDA257C）""" + try: + # 初始化脉冲引脚（输出模式） + pul_gpio = GPIO(PUL_Pin, "out") + # 初始化方向引脚（输出模式） + dir_gpio = GPIO(DIR_Pin, "out") + + # 初始状态：脉冲低电平，方向低电平（驱动器空闲状态） + pul_gpio.write(False) + dir_gpio.write(False) + + print(f"✅ PUL引脚初始化完成：{PUL_Pin}引脚") + print(f"✅ DIR引脚初始化完成：{DIR_Pin}引脚") + return pul_gpio, dir_gpio + + except FileNotFoundError: + raise RuntimeError(f"GPIO芯片不存在！检查 {PUL_Pin} 是否存在（命令：ls /dev/gpiochip*）") + except PermissionError: + raise RuntimeError("GPIO权限不足！请用 sudo 运行程序（sudo python test.py）") + except Exception as e: + raise RuntimeError(f"GPIO初始化失败：{str(e)}") from e + + +def rotate(pul_gpio: GPIO, dir_gpio: GPIO, rounds: float, direction: str = "clockwise", + test_freq: int = PULSE_FREQUENCY): + """ + 控制LCDA257C驱动器旋转（优化版脉冲发送，解决频率不生效问题） + :param pul_gpio: PUL引脚GPIO对象 + :param dir_gpio: DIR引脚GPIO对象 + :param rounds: 旋转圈数（正数，可小数） + :param direction: 方向（clockwise/counterclockwise） + :param test_freq: 本次旋转使用的脉冲频率（覆盖全局默认值） + """ + # 1. 参数合法性校验 + if rounds <= 0: + print("❌ 圈数必须为正数！") + return + if test_freq < 100: + print("❌ 频率过低（≥100Hz），请调整test_freq参数！") + return + + # 2. 设置旋转方向（严格遵循DIR提前时序） + if direction == "clockwise": + dir_gpio.write(CLOCKWISE_LEVEL) + print(f"\n=== 顺时针旋转 {rounds} 圈（目标频率：{test_freq}Hz）===") + elif direction == "counterclockwise": + dir_gpio.write(COUNTER_CLOCKWISE_LEVEL) + print(f"\n=== 逆时针旋转 {rounds} 圈（目标频率：{test_freq}Hz）===") + else: + print("❌ 方向参数错误！仅支持 clockwise/counterclockwise") + return + + # DIR电平设置后，延迟≥8us（满足LCDA257C时序要求） + time.sleep(MIN_DIR_DELAY_US / 1_000_000) + + # 3. 计算脉冲参数（确保高低电平≥最小宽度） + total_pulses = int(rounds * PULSES_PER_ROUND) + pulse_period = 1.0 / test_freq # 脉冲周期（秒） + + # 确保高低电平宽度不低于驱动器要求（避免脉冲识别失败） + high_period = max(pulse_period / 2, PULSE_HIGH_MIN_US / 1_000_000) + low_period = max(pulse_period / 2, PULSE_LOW_MIN_US / 1_000_000) + + # 打印参数（便于调试） + print(f"总脉冲数：{total_pulses} | 理论高电平：{high_period * 1e6:.1f}us | 理论低电平：{low_period * 1e6:.1f}us") + + # 4. 优化版脉冲发送（解决Python高频延时不准问题） + start_total = time.perf_counter() # 高精度计时（统计实际频率） + for _ in range(total_pulses): + # 高电平（直接sleep，避免while循环的调度延迟） + pul_gpio.write(True) + time.sleep(high_period) + + # 低电平 + pul_gpio.write(False) + time.sleep(low_period) + end_total = time.perf_counter() + + # 5. 计算实际频率（验证是否达到目标） + actual_duration = end_total - start_total + actual_freq = total_pulses / actual_duration if actual_duration > 0 else 0 + print(f"✅ 旋转完成 | 实际频率：{actual_freq:.0f}Hz | 耗时：{actual_duration:.2f}秒") + + +def motor_test_demo(): + """电机测试示例（逐步提升频率，验证转速变化）""" + pul_gpio = None + dir_gpio = None + try: + # 初始化引脚 + pul_gpio, dir_gpio = init_stepper() + print("\n=== 步进电机频率测试程序启动 ===") + + while True: + print(f"\n===== 测试频率：{PULSE_FREQUENCY}Hz =====") + + # 远离电机方向 顺时针 + rotate(pul_gpio, dir_gpio, rounds=10.0, direction="clockwise") + time.sleep(5) # + # 靠近电机方向 逆时针 + rotate(pul_gpio, dir_gpio, rounds=10.0, direction="counterclockwise") + time.sleep(5) # + + except Exception as e: + print(f"\n❌ 程序异常：{str(e)}") + finally: + # 安全释放GPIO资源（必须执行，避免引脚电平残留） + if pul_gpio: + pul_gpio.write(False) + pul_gpio.close() + print("\n✅ PUL引脚已关闭（低电平）") + if dir_gpio: + dir_gpio.write(False) + dir_gpio.close() + print("✅ DIR引脚已关闭（低电平）") + print("✅ 程序退出完成") + + +if __name__ == '__main__': + # 运行测试demo（必须sudo执行） + motor_test_demo() + diff --git a/RK1106/stepper_motor_test1.py b/RK1106/stepper_motor_test1.py new file mode 100644 index 0000000..38a4c1e --- /dev/null +++ b/RK1106/stepper_motor_test1.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +# @Time : 2026/1/4 19:13 +# @Author : reenrr +# @File : stepper_motor_test1.py +# @Desc : 线条厂控制步进电机测试 应该不会丢步 +""" +import time +from periphery import GPIO # 若未安装：pip install python-periphery + +# ------------参数配置------------- +# 1. 脉冲（PUL）引脚配置 → GPIO32 +PUL_Pin = 32 + +# 2. 方向（DIR）引脚配置 → GPIO33 +DIR_Pin = 33 + +# 3. 驱动器参数（根据拨码调整，默认不变） +PULSES_PER_ROUND = 400 # 每圈脉冲数（SW5~SW8拨码，默认400） +PULSE_FREQUENCY = 2500 # 脉冲频率（Hz，新手建议500~2000，最大200KHz） + +class StepperMotor: + """新力川MA860H驱动器步进电机控制类""" + # 方向常量定义 + CLOCKWISE = "clockwise" # 顺时针 + COUNTER_CLOCKWISE = "counterclockwise" # 逆时针 + + def __init__(self, + pul_pin: int = PUL_Pin, + dir_pin: int = DIR_Pin, + pulses_per_round: int = PULSES_PER_ROUND, + pulse_frequency: int = PULSE_FREQUENCY, + clockwise_level: bool = True, + counter_clockwise_level: bool = False): + """ + 初始化步进电机控制器 + :param pul_pin: 脉冲引脚 + :param dir_pin: 方向引脚 + :param pulses_per_round: 每圈脉冲数（SW5~SW8拨码，默认400） + :param pulse_frequency: 脉冲频率（Hz，新手建议500~2000，最大200KHz） + :param clockwise_level: 顺时针对应的DIR电平 + :param counter_clockwise_level: 逆时针对应的DIR电平 + """ + # 硬件配置参数 + self.pul_pin = pul_pin + self.dir_pin = dir_pin + + # 驱动器参数 + self.pulses_per_round = pulses_per_round + self.pulse_frequency = pulse_frequency + self.clockwise_level = clockwise_level + self.counter_clockwise_level = counter_clockwise_level + + # GPIO对象初始化 + self.pul_gpio = None + self.dir_gpio = None + + # 初始化GPIO + self._init_gpio() + + def _init_gpio(self): + """初始化PUL和DIR引脚（内部方法）""" + try: + # 初始化脉冲引脚（输出模式） + self.pul_gpio = GPIO(self.pul_pin, "out") + # 初始化方向引脚（输出模式） + self.dir_gpio = GPIO(self.dir_pin, "out") + + # 初始电平置低（避免电机误动作） + self.pul_gpio.write(False) + self.dir_gpio.write(False) + + print(f"✅ PUL引脚初始化完成：{self.pul_pin} 引脚") + print(f"✅ DIR引脚初始化完成：{self.dir_pin} 引脚") + + except PermissionError: + raise RuntimeError("权限不足！请用sudo运行程序（sudo python xxx.py）") + except Exception as e: + raise RuntimeError(f"GPIO初始化失败：{str(e)}") from e + + def _validate_rounds(self, rounds: float) -> bool: + """验证圈数是否合法（内部方法）""" + if rounds <= 0: + print("❌ 圈数必须为正数") + return False + return True + + def _validate_direction(self, direction: str) -> bool: + """验证方向参数是否合法（内部方法）""" + if direction not in [self.CLOCKWISE, self.COUNTER_CLOCKWISE]: + print(f"❌ 方向参数错误：仅支持 {self.CLOCKWISE}/{self.COUNTER_CLOCKWISE}") + return False + return True + + def rotate(self, rounds: float, direction: str = CLOCKWISE): + """ + 控制电机旋转（支持正反转） + :param rounds: 旋转圈数（可小数，如0.5=半圈） + :param direction: 方向（clockwise=顺时针，counterclockwise=逆时针） + """ + # 参数验证 + if not self._validate_rounds(rounds) or not self._validate_direction(direction): + return + + # 设置旋转方向（DIR电平） + if direction == self.CLOCKWISE: # 顺时针 + self.dir_gpio.write(self.clockwise_level) + print(f"\n=== 顺时针旋转 {rounds} 圈 ===") + else: # 逆时针 + self.dir_gpio.write(self.counter_clockwise_level) + print(f"\n=== 逆时针旋转 {rounds} 圈 ===") + + # 计算总脉冲数和时序（精准控制，避免丢步） + total_pulses = int(rounds * self.pulses_per_round) + pulse_period = 1.0 / self.pulse_frequency # 脉冲周期（秒） + half_period = pulse_period / 2 # 占空比50%（MA860H最优） + + print(f"总脉冲数：{total_pulses} | 频率：{self.pulse_frequency}Hz | 周期：{pulse_period * 1000:.2f}ms") + start_time = time.perf_counter() # 高精度计时（避免丢步） + + # 发送脉冲序列（核心：占空比50%的方波） + for _ in range(total_pulses): + # 高电平 + self.pul_gpio.write(True) + # 精准延时（比time.sleep稳定，适配高频脉冲） + while time.perf_counter() - start_time < half_period: + pass + # 低电平 + self.pul_gpio.write(False) + # 更新下一个脉冲的起始时间 + start_time += pulse_period + + print("✅ 旋转完成") + + def stop(self): + """紧急停止（置低脉冲引脚）""" + if self.pul_gpio: + self.pul_gpio.write(False) + print("🛑 电机已停止") + + def close(self): + """释放GPIO资源""" + # 安全释放GPIO资源（关键：避免引脚电平残留） + if self.pul_gpio: + self.pul_gpio.write(False) # 脉冲引脚置低 + self.pul_gpio.close() + print("\n✅ PUL引脚已关闭（电平置低）") + + if self.dir_gpio: + self.dir_gpio.write(False) # 方向引脚置低 + self.dir_gpio.close() + print("✅ DIR引脚已关闭（电平置低）") + + # 重置GPIO对象 + self.pul_gpio = None + self.dir_gpio = None + + def __del__(self): + """析构函数：确保资源释放""" + self.close() + + +# 使用示例 +def motor_demo(): + """电机控制示例""" + motor = None + try: + # 创建电机实例（使用默认配置） + motor = StepperMotor() + print("\n=== 步进电机控制程序启动 ===") + + while True: + # 靠近电机方向 逆时针 + motor.rotate(rounds=10.0, direction=motor.COUNTER_CLOCKWISE) + time.sleep(5) # 暂停5秒 + # 远离电机方向 顺时针 + motor.rotate(rounds=10.0, direction=motor.CLOCKWISE) + time.sleep(5) # 暂停5秒 + + + except PermissionError: + print("\n❌ 权限不足：请用 sudo 运行！") + print("命令：sudo python3 double_direction_motor.py") + except ImportError: + print("\n❌ 缺少依赖：请安装python-periphery") + print("命令：pip install python-periphery") + except Exception as e: + print(f"\n❌ 程序异常：{str(e)}") + finally: + if motor: + motor.close() + print("✅ 程序退出完成") + + +if __name__ == '__main__': + motor_demo() + diff --git a/RK1106/test.py b/RK1106/test.py new file mode 100644 index 0000000..0b3e378 --- /dev/null +++ b/RK1106/test.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2026/1/5 16:18 +# @Author : reenrr +# @File : test.py +# @Desc : 步进电机添加记录脉冲数量（防止断电情况） +''' +import time +import json +import os +from periphery import GPIO + +# ------------参数配置------------- +# 1. 脉冲（PUL）引脚配置 → GPIO32 +PUL_Pin = 32 + +# 2. 方向（DIR）引脚配置 → GPIO33 +DIR_Pin = 33 + +# 3. 驱动器参数（根据拨码调整，默认不变） +PULSES_PER_ROUND = 400 # 每圈脉冲数（SW5~SW8拨码，默认400） +PULSE_FREQUENCY = 2500 # 脉冲频率（Hz，新手建议500~2000，最大200KHz） + +# 4. 计数持久化配置 +COUNT_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)), "motor_pulse_count.json") +SAVE_INTERVAL = 10 # 每发送10个脉冲保存一次计数（减少IO开销，可根据需求调整） + +class StepperMotor: + """新力川MA860H驱动器步进电机控制类""" + # 方向常量定义 + CLOCKWISE = "clockwise" # 顺时针 + COUNTER_CLOCKWISE = "counterclockwise" # 逆时针 + + def __init__(self, + pul_pin: int = PUL_Pin, + dir_pin: int = DIR_Pin, + pulses_per_round: int = PULSES_PER_ROUND, + pulse_frequency: int = PULSE_FREQUENCY, + clockwise_level: bool = True, + counter_clockwise_level: bool = False): + """ + 初始化步进电机控制器 + :param pul_pin: 脉冲引脚 + :param dir_pin: 方向引脚 + :param pulses_per_round: 每圈脉冲数（SW5~SW8拨码，默认400） + :param pulse_frequency: 脉冲频率（Hz，新手建议500~2000，最大200KHz） + :param clockwise_level: 顺时针对应的DIR电平 + :param counter_clockwise_level: 逆时针对应的DIR电平 + """ + # 硬件配置参数 + self.pul_pin = pul_pin + self.dir_pin = dir_pin + + # 驱动器参数 + self.pulses_per_round = pulses_per_round + self.pulse_frequency = pulse_frequency + self.clockwise_level = clockwise_level + self.counter_clockwise_level = counter_clockwise_level + + # GPIO对象初始化 + self.pul_gpio = None + self.dir_gpio = None + + # 脉冲计数相关（核心：记录已发送的脉冲数） + self.current_pulse_count = 0 # 本次旋转已发送的脉冲数 + self.total_pulse_history = self._load_pulse_count() # 历史总脉冲数（从文件加载） + self.current_rotate_target = 0 # 本次旋转的目标脉冲数 + + # 初始化GPIO + self._init_gpio() + + def _load_pulse_count(self): + """加载历史脉冲计数（程序启动时执行）""" + try: + if os.path.exists(COUNT_FILE): + with open(COUNT_FILE, "r", encoding="utf-8") as f: + data = json.load(f) + # 返回历史总脉冲数（默认0） + return int(data.get("total_pulses", 0)) + except Exception as e: + print(f"[WARN] 加载历史计数失败：{e}，将从0开始计数") + return 0 + + def _save_pulse_count(self): + """保存当前总脉冲数到文件（持久化）""" + try: + with open(COUNT_FILE, "w", encoding="utf-8") as f: + json.dump({ + "total_pulses": self.total_pulse_history, + "update_time": time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()) + }, f, ensure_ascii=False, indent=2) + except Exception as e: + print(f"[ERROR] 保存计数失败：{e}") + + def _init_gpio(self): + """初始化PUL和DIR引脚（内部方法）""" + try: + # 初始化脉冲引脚（输出模式） + self.pul_gpio = GPIO(self.pul_pin, "out") + # 初始化方向引脚（输出模式） + self.dir_gpio = GPIO(self.dir_pin, "out") + + # 初始电平置低（避免电机误动作） + self.pul_gpio.write(False) + self.dir_gpio.write(False) + + print(f"[OK] PUL引脚初始化完成：{self.pul_pin} 引脚") + print(f"[OK] DIR引脚初始化完成：{self.dir_pin} 引脚") + print( + f"[INFO] 历史累计脉冲数：{self.total_pulse_history} → 对应圈数：{self.total_pulse_history / self.pulses_per_round:.2f} 圈") + + except PermissionError: + raise RuntimeError("权限不足！请用sudo运行程序（sudo python xxx.py）") + except Exception as e: + raise RuntimeError(f"GPIO初始化失败：{str(e)}") from e + + def _validate_rounds(self, rounds: float) -> bool: + """验证圈数是否合法（内部方法）""" + if rounds <= 0: + print("[ERROR] 圈数必须为正数") + return False + return True + + def _validate_direction(self, direction: str) -> bool: + """验证方向参数是否合法（内部方法）""" + if direction not in [self.CLOCKWISE, self.COUNTER_CLOCKWISE]: + print(f"[ERROR] 方向参数错误：仅支持 {self.CLOCKWISE}/{self.COUNTER_CLOCKWISE}") + return False + return True + + def rotate(self, rounds: float, direction: str = CLOCKWISE): + """ + 控制电机旋转（支持正反转，实时记录脉冲数） + :param rounds: 旋转圈数（可小数，如0.5=半圈） + :param direction: 方向（clockwise=顺时针，counterclockwise=逆时针） + """ + # 参数验证 + if not self._validate_rounds(rounds) or not self._validate_direction(direction): + return + + # 重置本次旋转的计数 + self.current_pulse_count = 0 + # 计算本次旋转的目标脉冲数 + self.current_rotate_target = int(rounds * self.pulses_per_round) + + # 设置旋转方向（DIR电平） + if direction == self.CLOCKWISE: # 顺时针 + self.dir_gpio.write(self.clockwise_level) + print(f"\n=== 顺时针旋转 {rounds} 圈 ===") + else: # 逆时针 + self.dir_gpio.write(self.counter_clockwise_level) + print(f"\n=== 逆时针旋转 {rounds} 圈 ===") + + # 计算总脉冲数和时序（精准控制，避免丢步） + total_pulses = self.current_rotate_target + pulse_period = 1.0 / self.pulse_frequency # 脉冲周期（秒） + half_period = pulse_period / 2 # 占空比50%（MA860H最优） + + print(f"目标脉冲数：{total_pulses} | 频率：{self.pulse_frequency}Hz | 周期：{pulse_period * 1000:.2f}ms") + start_time = time.perf_counter() # 高精度计时（避免丢步） + + try: + # 发送脉冲序列（核心：占空比50%的方波，实时计数） + for _ in range(total_pulses): + # 高电平 + self.pul_gpio.write(True) + # 精准延时（比time.sleep稳定，适配高频脉冲） + while time.perf_counter() - start_time < half_period: + pass + # 低电平 + self.pul_gpio.write(False) + # 更新下一个脉冲的起始时间 + start_time += pulse_period + + # 🌟 核心：累加本次脉冲计数 + self.current_pulse_count += 1 + self.total_pulse_history += 1 + + # 每发送SAVE_INTERVAL个脉冲，保存一次计数（减少IO开销） + if self.current_pulse_count % SAVE_INTERVAL == 0: + self._save_pulse_count() + + print( + f"[OK] 旋转完成 → 本次发送脉冲：{self.current_pulse_count} | 累计脉冲：{self.total_pulse_history} | 累计圈数：{self.total_pulse_history / self.pulses_per_round:.2f}") + + except Exception as e: + # 🌟 关键：异常发生时（如断电前的程序崩溃），立即保存当前计数 + self._save_pulse_count() + # 计算已完成的圈数 + completed_rounds = self.current_pulse_count / self.pulses_per_round + remaining_pulses = self.current_rotate_target - self.current_pulse_count + remaining_rounds = remaining_pulses / self.pulses_per_round + print(f"\n[ERROR] 旋转过程中异常：{e}") + print(f"[INFO] 异常时已发送脉冲：{self.current_pulse_count} → 已完成圈数：{completed_rounds:.2f}") + print(f"[INFO] 剩余未发送脉冲：{remaining_pulses} → 剩余圈数：{remaining_rounds:.2f}") + print( + f"[INFO] 累计总脉冲：{self.total_pulse_history} → 累计总圈数：{self.total_pulse_history / self.pulses_per_round:.2f}") + raise # 抛出异常，让上层处理 + + def get_current_status(self): + """获取当前电机状态（脉冲数、圈数）""" + return { + "total_pulses": self.total_pulse_history, + "total_rounds": self.total_pulse_history / self.pulses_per_round, + "last_rotate_completed_pulses": self.current_pulse_count, + "last_rotate_completed_rounds": self.current_pulse_count / self.pulses_per_round, + "last_rotate_target_pulses": self.current_rotate_target, + "last_rotate_target_rounds": self.current_rotate_target / self.pulses_per_round + } + + def reset_count(self): + """重置累计计数（按需使用，如电机归位后）""" + self.total_pulse_history = 0 + self._save_pulse_count() + print("[INFO] 累计脉冲计数已重置为0") + + def stop(self): + """紧急停止（置低脉冲引脚）""" + if self.pul_gpio: + self.pul_gpio.write(False) + # 停止时保存当前计数 + self._save_pulse_count() + print("[STOP] 电机已停止，当前计数已保存") + + def close(self): + """释放GPIO资源""" + # 安全释放GPIO资源（关键：避免引脚电平残留） + if self.pul_gpio: + self.pul_gpio.write(False) # 脉冲引脚置低 + self.pul_gpio.close() + print("\n[OK] PUL引脚已关闭（电平置低）") + + if self.dir_gpio: + self.dir_gpio.write(False) # 方向引脚置低 + self.dir_gpio.close() + print("[OK] DIR引脚已关闭（电平置低）") + + # 关闭时保存最终计数 + self._save_pulse_count() + print( + f"[INFO] 最终累计脉冲：{self.total_pulse_history} → 累计圈数：{self.total_pulse_history / self.pulses_per_round:.2f}") + + # 重置GPIO对象 + self.pul_gpio = None + self.dir_gpio = None + + def __del__(self): + """析构函数：确保资源释放""" + self.close() + +# 使用示例 +def motor_demo(): + """电机控制示例""" + motor = None + try: + # 创建电机实例（使用默认配置） + motor = StepperMotor() + print("\n=== 步进电机控制程序启动 ===") + # 打印初始状态 + init_status = motor.get_current_status() + print(f"[INIT] 初始状态 → 累计圈数：{init_status['total_rounds']:.2f} 圈") + + while True: + # 靠近电机方向 逆时针 + motor.rotate(rounds=10.0, direction=motor.COUNTER_CLOCKWISE) + time.sleep(5) # 暂停5秒 + # 远离电机方向 顺时针 + motor.rotate(rounds=10.0, direction=motor.CLOCKWISE) + time.sleep(5) # 暂停5秒 + + except PermissionError: + print("\n[ERROR] 权限不足：请用 sudo 运行！") + print("命令：sudo python3 double_direction_motor.py") + except ImportError: + print("\n[ERROR] 缺少依赖：请安装python-periphery") + print("命令：pip install python-periphery") + except KeyboardInterrupt: + print("\n[INFO] 用户手动停止程序") + except Exception as e: + print(f"\n[ERROR] 程序异常：{str(e)}") + finally: + if motor: + # 打印最终状态 + final_status = motor.get_current_status() + print(f"\n[FINAL] 程序退出状态 → 累计脉冲：{final_status['total_pulses']} | 累计圈数：{final_status['total_rounds']:.2f} 圈") + motor.close() + print("[OK] 程序退出完成") + + +if __name__ == '__main__': + motor_demo() \ No newline at end of file diff --git a/UI/test.py b/UI/test.py new file mode 100644 index 0000000..81166e2 --- /dev/null +++ b/UI/test.py @@ -0,0 +1,564 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/12/8 14:19 +# @Author : reenrr +# @File : ui.py +# @Description: 线条长度标记与电机位置控制系统主界面--测试修改代码 +''' +import sys +import time +from PySide6.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, + QHBoxLayout, QLabel, QPushButton, + QGroupBox, QMessageBox, QComboBox, QTextEdit, + QTableWidget, QTableWidgetItem) +from PySide6.QtCore import QTimer, Qt, Signal, Slot +from PySide6.QtGui import (QFont, QPalette, QColor) + + +class LengthMotorController(QMainWindow): + """线条长度标记与电机位置控制系统主界面""" + update_time_signal = Signal(str) + record_data_signal = Signal(float, float, float, float) + line_select_signal = Signal(str, float, float, float, float) + + def __init__(self): + super().__init__() + self.setWindowTitle("线条长度标记与电机位置控制系统") + self.setMinimumSize(1200, 700) + + self.font1 = QFont("Microsoft YaHei", 14) + self.font2 = QFont("Microsoft YaHei", 12) + self.recorded_data = [] + # 线条A参数 + self.line_a_params = { + "类型1": (0.5, 5, 0.1, 25), + "类型2": (1.0, 10, 0.2, 30), + "类型3": (1.2, 8, 0.15, 28), + "类型4": (1.8, 12, 0.25, 32), + "类型5": (0.8, 7, 0.12, 26), + "类型6": (1.4, 14, 0.18, 31), + "类型7": (1.6, 16, 0.22, 33), + "类型8": (2.0, 18, 0.28, 34) + } + # 线条B参数 + self.line_b_params = { + "类型1": (1.5, 15, 0.3, 35), + "类型2": (2.0, 20, 0.4, 40), + "类型3": (1.6, 18, 0.35, 38), + "类型4": (2.2, 22, 0.45, 42), + "类型5": (1.7, 25, 0.32, 36), + "类型6": (1.9, 28, 0.38, 39), + "类型7": (2.1, 30, 0.42, 41), + "类型8": (2.3, 32, 0.48, 43) + } + # 当前选中的线条类型 + self.selected_line_a = "类型1" + self.selected_line_b = "类型1" + + self._setup_style() + central_widget = QWidget() + self.setCentralWidget(central_widget) + main_layout = QVBoxLayout(central_widget) + main_layout.setSpacing(20) + main_layout.setContentsMargins(30, 30, 30, 30) + + # 1. 顶部标题（替代原实时时间） + self._create_title(main_layout) + + # 2. 下方左右分栏：左侧线条选择区，右侧（实时时间 + 参数 + 3D） + bottom_h_layout = QHBoxLayout() + bottom_h_layout.setSpacing(20) + + # 左侧：线条选择区（表格） + self._create_line_select_table(bottom_h_layout) + + # 右侧：实时时间 + 参数设置 + 3D显示区 + self._create_right_area(bottom_h_layout) + + main_layout.addLayout(bottom_h_layout) + self._init_timer() + self._connect_signals() + + def _setup_style(self): + """设置界面样式""" + palette = QPalette() + palette.setColor(QPalette.Window, QColor(245, 245, 247)) + palette.setColor(QPalette.WindowText, QColor(30, 30, 30)) + palette.setColor(QPalette.Button, QColor(66, 133, 244)) + palette.setColor(QPalette.ButtonText, Qt.white) + self.setPalette(palette) + + self.setStyleSheet(""" + QGroupBox { + font-weight: bold; + border: 2px solid #4285F4; + border-radius: 8px; + margin-top: 10px; + padding-top: 10px; + } + QGroupBox::title { + subcontrol-origin: margin; + left: 10px; + padding: 0 8px 0 8px; + } + QPushButton { + border-radius: 6px; + padding: 8px 16px; + font-size: 12pt; + } + QPushButton:hover { + background-color: #5294FF; + } + QComboBox { + border: 2px solid #DDDDDD; + border-radius: 6px; + padding: 8px; + font-size: 10pt; + } + QComboBox:focus { + border-color: #4285F4; + outline: none; + } + QTextEdit { + border: 2px solid #DDDDDD; + border-radius: 6px; + background-color: white; + font-size: 11pt; + } + QTableWidget { + border: 2px solid #DDDDDD; + border-radius: 6px; + font-size: 11pt; + } + QTableWidget::item:selected { + background-color: #E1F5FE; + color: #212121; + } + """) + + def _create_title(self, parent_layout): + """顶部标题（替代原实时时间）""" + title_label = QLabel("线条长度标记与电机位置控制系统") + title_label.setFont(QFont("Microsoft YaHei", 20, QFont.Bold)) + title_label.setAlignment(Qt.AlignCenter) + parent_layout.addWidget(title_label) + + def _create_line_select_table(self, parent_layout): + """左侧线条选择区（表格）""" + line_select_group = QGroupBox("线条选择（参数预览）") + line_select_group.setFont(self.font1) + line_select_group.setMinimumWidth(self.width() * 0.5) + select_layout = QVBoxLayout(line_select_group) + select_layout.setSpacing(20) + select_layout.setContentsMargins(20, 20, 20, 20) + + # 线条A表格 + a_label = QLabel("线条A参数表：") + a_label.setFont(self.font2) + self.table_line_a = QTableWidget() + self.table_line_a.setRowCount(len(self.line_a_params)) + self.table_line_a.setColumnCount(5) + self.table_line_a.setHorizontalHeaderLabels(["类型", "参数a", "参数b", "参数c", "参数d"]) + for row, (type_name, params) in enumerate(self.line_a_params.items()): + self.table_line_a.setItem(row, 0, QTableWidgetItem(type_name)) + self.table_line_a.setItem(row, 1, QTableWidgetItem(str(params[0]))) + self.table_line_a.setItem(row, 2, QTableWidgetItem(str(params[1]))) + self.table_line_a.setItem(row, 3, QTableWidgetItem(str(params[2]))) + self.table_line_a.setItem(row, 4, QTableWidgetItem(str(params[3]))) + self.table_line_a.setFixedHeight(220) + self.table_line_a.setEditTriggers(QTableWidget.NoEditTriggers) + self.table_line_a.setSelectionBehavior(QTableWidget.SelectRows) + self.table_line_a.selectRow(0) + self.table_line_a.itemSelectionChanged.connect(self._on_line_a_select) + + # 线条B表格 + b_label = QLabel("线条B参数表：") + b_label.setFont(self.font2) + self.table_line_b = QTableWidget() + self.table_line_b.setRowCount(len(self.line_b_params)) + self.table_line_b.setColumnCount(5) + self.table_line_b.setHorizontalHeaderLabels(["类型", "参数a", "参数b", "参数c", "参数d"]) + for row, (type_name, params) in enumerate(self.line_b_params.items()): + self.table_line_b.setItem(row, 0, QTableWidgetItem(type_name)) + self.table_line_b.setItem(row, 1, QTableWidgetItem(str(params[0]))) + self.table_line_b.setItem(row, 2, QTableWidgetItem(str(params[1]))) + self.table_line_b.setItem(row, 3, QTableWidgetItem(str(params[2]))) + self.table_line_b.setItem(row, 4, QTableWidgetItem(str(params[3]))) + self.table_line_b.setFixedHeight(220) + self.table_line_b.setEditTriggers(QTableWidget.NoEditTriggers) + self.table_line_b.setSelectionBehavior(QTableWidget.SelectRows) + self.table_line_b.selectRow(0) + self.table_line_b.itemSelectionChanged.connect(self._on_line_b_select) + + # 导入按钮 + import_layout = QHBoxLayout() + import_a_btn = QPushButton("导入线条A选中参数") + import_a_btn.setFont(self.font2) + import_a_btn.clicked.connect(lambda: self._import_line_param("A")) + + import_b_btn = QPushButton("导入线条B选中参数") + import_b_btn.setFont(self.font2) + import_b_btn.clicked.connect(lambda: self._import_line_param("B")) + + import_layout.addWidget(import_a_btn) + import_layout.addWidget(import_b_btn) + + select_layout.addWidget(a_label) + select_layout.addWidget(self.table_line_a) + select_layout.addWidget(b_label) + select_layout.addWidget(self.table_line_b) + select_layout.addLayout(import_layout) + select_layout.addStretch() + + parent_layout.addWidget(line_select_group) + + def _on_line_a_select(self): + """线条A表格选中行变化""" + selected_rows = self.table_line_a.selectedItems() + if selected_rows: + # 获取选中行的第一列（类型列）文本 + row = selected_rows[0].row() + self.selected_line_a = self.table_line_a.item(row, 0).text() + + def _on_line_b_select(self): + """线条B表格选中行变化""" + selected_rows = self.table_line_b.selectedItems() + if selected_rows: + row = selected_rows[0].row() + self.selected_line_b = self.table_line_b.item(row, 0).text() + + def _create_right_area(self, parent_layout): + """右侧区域：实时时间 → 线条类型设置 → 3D显示""" + right_v_layout = QVBoxLayout() + right_v_layout.setSpacing(20) + + # 1. 实时时间（移到右侧顶部） + time_group = QGroupBox("实时时间") + time_group.setFont(self.font1) + time_layout = QHBoxLayout(time_group) + self.time_label = QLabel() + self.time_label.setAlignment(Qt.AlignCenter) + self.time_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold)) + time_layout.addWidget(self.time_label) + right_v_layout.addWidget(time_group) + + # 2. 线条类型设置区 + control_group = QGroupBox("线条类型设置") + control_group.setFont(self.font1) + control_layout = QVBoxLayout(control_group) + control_layout.setSpacing(15) + control_layout.setContentsMargins(20, 20, 20, 20) + + # 提取所有参数a/b/c/d的可能值 + # 收集参数a的所有值 + # 提取所有参数a/b/c/d的可能值（统一格式） + # 收集参数a的所有值（保留2位小数，去除末尾0） + param_a_list = [] + for params in self.line_a_params.values(): + a = params[0] + a_str = f"{a:.2f}".rstrip('0').rstrip('.') if '.' in f"{a:.2f}" else f"{a:.2f}" + param_a_list.append(a_str) + for params in self.line_b_params.values(): + a = params[0] + a_str = f"{a:.2f}".rstrip('0').rstrip('.') if '.' in f"{a:.2f}" else f"{a:.2f}" + param_a_list.append(a_str) + param_a_list = sorted(list(set(param_a_list)), key=lambda x: float(x)) + + # 收集参数b的所有值（整数格式） + param_b_list = [] + for params in self.line_a_params.values(): + b_str = f"{params[1]:.0f}" + param_b_list.append(b_str) + for params in self.line_b_params.values(): + b_str = f"{params[1]:.0f}" + param_b_list.append(b_str) + param_b_list = sorted(list(set(param_b_list)), key=lambda x: int(x)) + + # 收集参数c的所有值（保留2位小数，去除末尾0） + param_c_list = [] + for params in self.line_a_params.values(): + c = params[2] + c_str = f"{c:.2f}".rstrip('0').rstrip('.') if '.' in f"{c:.2f}" else f"{c:.2f}" + param_c_list.append(c_str) + for params in self.line_b_params.values(): + c = params[2] + c_str = f"{c:.2f}".rstrip('0').rstrip('.') if '.' in f"{c:.2f}" else f"{c:.2f}" + param_c_list.append(c_str) + param_c_list = sorted(list(set(param_c_list)), key=lambda x: float(x)) + + # 收集参数d的所有值（整数格式） + param_d_list = [] + for params in self.line_a_params.values(): + d_str = f"{params[3]:.0f}" + param_d_list.append(d_str) + for params in self.line_b_params.values(): + d_str = f"{params[3]:.0f}" + param_d_list.append(d_str) + param_d_list = sorted(list(set(param_d_list)), key=lambda x: int(x)) + + # 4个参数下拉框 + param_layout = QHBoxLayout() + param_layout.setSpacing(20) + + # 参数a + a_param_layout = QVBoxLayout() + label_a = QLabel("参数a:") + label_a.setFont(self.font2) + label_a.setAlignment(Qt.AlignCenter) + self.combo_a = QComboBox() + self.combo_a.addItems(param_a_list) + self.combo_a.setFixedWidth(100) + self.combo_a.setFont(self.font2) + a_param_layout.addWidget(label_a) + a_param_layout.addWidget(self.combo_a) + param_layout.addLayout(a_param_layout) + + # 参数b + b_param_layout = QVBoxLayout() + label_b = QLabel("参数b:") + label_b.setFont(self.font2) + label_b.setAlignment(Qt.AlignCenter) + self.combo_b = QComboBox() + self.combo_b.addItems(param_b_list) + self.combo_b.setFixedWidth(100) + self.combo_b.setFont(self.font2) + b_param_layout.addWidget(label_b) + b_param_layout.addWidget(self.combo_b) + param_layout.addLayout(b_param_layout) + + # 参数c + c_param_layout = QVBoxLayout() + label_c = QLabel("参数c:") + label_c.setFont(self.font2) + label_c.setAlignment(Qt.AlignCenter) + self.combo_c = QComboBox() + self.combo_c.addItems(param_c_list) + self.combo_c.setFixedWidth(100) + self.combo_c.setFont(self.font2) + c_param_layout.addWidget(label_c) + c_param_layout.addWidget(self.combo_c) + param_layout.addLayout(c_param_layout) + + # 参数d + d_param_layout = QVBoxLayout() + label_d = QLabel("参数d:") + label_d.setFont(self.font2) + label_d.setAlignment(Qt.AlignCenter) + self.combo_d = QComboBox() + self.combo_d.addItems(param_d_list) + self.combo_d.setFixedWidth(100) + self.combo_d.setFont(self.font2) + d_param_layout.addWidget(label_d) + d_param_layout.addWidget(self.combo_d) + param_layout.addLayout(d_param_layout) + + # 按钮行 + button_layout = QHBoxLayout() + self.save_btn = QPushButton("保存设置") + self.save_btn.setFont(self.font2) + self.save_btn.setFixedSize(140, 45) + + self.clear_btn = QPushButton("清空选择") + self.clear_btn.setFont(self.font2) + self.clear_btn.setFixedSize(140, 45) + + self.export_btn = QPushButton("导出记录") + self.export_btn.setFont(self.font2) + self.export_btn.setFixedSize(140, 45) + + button_layout.addStretch() + button_layout.addWidget(self.save_btn) + button_layout.addWidget(self.clear_btn) + button_layout.addWidget(self.export_btn) + button_layout.addStretch() + + control_layout.addLayout(param_layout) + control_layout.addLayout(button_layout) + right_v_layout.addWidget(control_group) + + # 3. 3D效果显示占位区 + self._create_3d_placeholder(right_v_layout) + + parent_layout.addLayout(right_v_layout) + + def _create_3d_placeholder(self, parent_layout): + """3D效果显示占位区（缩小留白）""" + display_group = QGroupBox("线条3D效果显示（后续接口）") + display_group.setFont(self.font1) + display_layout = QVBoxLayout(display_group) + display_layout.setContentsMargins(20, 20, 20, 20) + + placeholder_text = QTextEdit() + placeholder_text.setReadOnly(True) + placeholder_text.setMaximumHeight(180) # 进一步缩小高度 + placeholder_text.setText("""3D接口说明： +1. 后续集成3D模块（如Qt3DExtras） +2. 参数a→半径，参数b→长度，参数c/d→颜色 +3. 保存参数触发record_data_signal更新3D""") + placeholder_text.setFont(QFont("Microsoft YaHei", 10)) + + test_btn = QPushButton("测试参数传递") + test_btn.setFont(self.font2) + test_btn.setFixedWidth(150) + test_btn.clicked.connect(self._test_3d_param_pass) + + display_layout.addWidget(placeholder_text) + display_layout.addWidget(test_btn, alignment=Qt.AlignCenter) + parent_layout.addWidget(display_group) + + def _init_timer(self): + """初始化定时器""" + self.timer = QTimer(self) + self.timer.timeout.connect(self._update_current_time) + self.timer.start(1000) + self._update_current_time() + + def _connect_signals(self): + """连接信号与槽函数""" + self.save_btn.clicked.connect(self._save_settings) + self.clear_btn.clicked.connect(self._clear_inputs) + self.export_btn.clicked.connect(self._export_records) + self.update_time_signal.connect(self.time_label.setText) + self.record_data_signal.connect(self._log_param_change) + self.line_select_signal.connect(self._set_line_params) + + def _update_current_time(self): + """更新当前时间""" + current_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()) + self.update_time_signal.emit(current_time) + + @Slot(str) + def _import_line_param(self, line_type): + """导入选中线条的参数""" + if line_type == "A": + selected_type = self.selected_line_a + params = self.line_a_params[selected_type] + line_name = f"线条A-{selected_type}" + else: + selected_type = self.selected_line_b + params = self.line_b_params[selected_type] + line_name = f"线条B-{selected_type}" + + self.line_select_signal.emit(line_name, *params) + QMessageBox.information(self, "导入成功", f"已导入{line_name}的参数") + + @Slot(str, float, float, float, float) + def _set_line_params(self, line_name, a, b, c, d): + """设置参数到下拉框（统一浮点数格式）""" + # 根据参数特性设置小数位：a/c保留2位，b/d保留0位（整数） + a_str = f"{a:.2f}".rstrip('0').rstrip('.') if '.' in f"{a:.2f}" else f"{a:.2f}" + b_str = f"{b:.0f}" # 参数b是整数（5/7/8等） + c_str = f"{c:.2f}".rstrip('0').rstrip('.') if '.' in f"{c:.2f}" else f"{c:.2f}" + d_str = f"{d:.0f}" # 参数d是整数（25/26/28等） + + """设置参数到下拉框""" + self.combo_a.setCurrentText(str(a)) + self.combo_b.setCurrentText(str(b)) + self.combo_c.setCurrentText(str(c)) + self.combo_d.setCurrentText(str(d)) + + @Slot() + def _save_settings(self): + """保存参数并触发信号""" + try: + param_a = float(self.combo_a.currentText()) + param_b = float(self.combo_b.currentText()) + param_c = float(self.combo_c.currentText()) + param_d = float(self.combo_d.currentText()) + except ValueError: + QMessageBox.critical(self, "输入错误", "参数格式无效！") + return + + if param_a <= 0 or param_b <= 0 or param_c < 0 or param_d < 0: + QMessageBox.warning(self, "参数错误", "参数a/b必须大于0，c/d不能小于0！") + return + + self.record_data_signal.emit(param_a, param_b, param_c, param_d) + QMessageBox.information(self, "保存成功", + f"已保存设置：\n参数a：{param_a}\n参数b：{param_b}\n参数c：{param_c}\n参数d：{param_d}") + + @Slot(float, float, float, float) + def _log_param_change(self, a, b, c, d): + """记录参数变化""" + self.recorded_data.append({ + "a": a, "b": b, "c": c, "d": d, + "time": time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()) + }) + + @Slot() + def _test_3d_param_pass(self): + """测试参数传递""" + try: + param_a = float(self.combo_a.currentText()) + param_b = float(self.combo_b.currentText()) + param_c = float(self.combo_c.currentText()) + param_d = float(self.combo_d.currentText()) + except ValueError: + QMessageBox.critical(self, "测试失败", "当前参数格式无效！") + return + + QMessageBox.information( + self, "参数传递测试成功", + f"当前参数已准备好传递给3D模块：\n" + f"参数a（半径）：{param_a}\n" + f"参数b（长度）：{param_b}\n" + f"参数c（颜色通道1）：{param_c}\n" + f"参数d（颜色通道2）：{param_d}" + ) + + @Slot() + def _clear_inputs(self): + """清空所有选择""" + self.combo_a.setCurrentIndex(0) + self.combo_b.setCurrentIndex(0) + self.combo_c.setCurrentIndex(0) + self.combo_d.setCurrentIndex(0) + self.table_line_a.selectRow(0) + self.table_line_b.selectRow(0) + + @Slot() + def _export_records(self): + """导出参数记录""" + if not self.recorded_data: + QMessageBox.warning(self, "导出提示", "暂无记录可导出！") + return + + try: + filename = f"线条参数记录_{time.strftime('%Y%m%d_%H%M%S')}.txt" + with open(filename, 'w', encoding='utf-8') as f: + f.write("线条参数记录\n") + f.write("=" * 60 + "\n") + f.write(f"导出时间：{time.strftime('%Y-%m-%d %H:%M:%S')}\n") + f.write("参数a\t参数b\t参数c\t参数d\t记录时间\n") + f.write("-" * 60 + "\n") + for data in self.recorded_data: + f.write(f"{data['a']:.2f}\t{data['b']:.2f}\t{data['c']:.2f}\t{data['d']:.2f}\t{data['time']}\n") + QMessageBox.information(self, "导出成功", f"记录已导出到：\n{filename}") + except Exception as e: + QMessageBox.critical(self, "导出失败", f"导出文件时出错：{str(e)}") + + def closeEvent(self, event): + """关闭窗口确认""" + reply = QMessageBox.question(self, "确认退出", + "是否确定退出程序？", + QMessageBox.Yes | QMessageBox.No, + QMessageBox.No) + if reply == QMessageBox.Yes: + self.timer.stop() + event.accept() + else: + event.ignore() + +# ----------界面对外接口----------- +def ui_main(): + app = QApplication(sys.argv) + app.setApplicationName("长度电机控制器") + app.setApplicationVersion("1.0.0") + window = LengthMotorController() + window.show() + sys.exit(app.exec()) + +# ----------测试接口-------------- +if __name__ == "__main__": + ui_main() \ No newline at end of file diff --git a/UI/ui.py b/UI/ui.py new file mode 100644 index 0000000..16fec2c --- /dev/null +++ b/UI/ui.py @@ -0,0 +1,568 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/12/8 14:19 +# @Author : reenrr +# @File : ui.py +# @Description: 线条长度标记与电机位置控制系统主界面 +''' +import sys +import time +from PySide6.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, + QHBoxLayout, QLabel, QPushButton, + QGroupBox, QMessageBox, QComboBox, QTextEdit, + QTableWidget, QTableWidgetItem) # 新增表格组件 +from PySide6.QtCore import QTimer, Qt, Signal, Slot +from PySide6.QtGui import (QFont, QPalette, QColor) + + +class LengthMotorController(QMainWindow): + """线条长度标记与电机位置控制系统主界面""" + update_time_signal = Signal(str) + record_data_signal = Signal(float, float, float, float) + line_select_signal = Signal(str, float, float, float, float) + + def __init__(self): + super().__init__() + self.setWindowTitle("线条长度标记与电机位置控制系统") + self.setMinimumSize(1200, 700) + + self.font1 = QFont("Microsoft YaHei", 12) + self.font2 = QFont("Microsoft YaHei", 12) + + self.recorded_data = [] + # 线条A参数 + self.line_a_params = { + "类型1": (0.5, 5, 0.1, 25), + "类型2": (1.0, 10, 0.2, 30), + "类型3": (1.2, 8, 0.15, 28), + "类型4": (1.8, 12, 0.25, 32), + "类型5": (0.8, 7, 0.12, 26), + "类型6": (1.4, 14, 0.18, 31), + "类型7": (1.6, 16, 0.22, 33), + "类型8": (2.0, 18, 0.28, 34) + } + # 线条B参数 + self.line_b_params = { + "类型1": (1.5, 15, 0.3, 35), + "类型2": (2.0, 20, 0.4, 40), + "类型3": (1.6, 18, 0.35, 38), + "类型4": (2.2, 22, 0.45, 42), + "类型5": (1.7, 25, 0.32, 36), + "类型6": (1.9, 28, 0.38, 39), + "类型7": (2.1, 30, 0.42, 41), + "类型8": (2.3, 32, 0.48, 43) + } + # 当前选中的线条类型 + self.selected_line_a = "类型1" + self.selected_line_b = "类型1" + + self._setup_style() + central_widget = QWidget() + self.setCentralWidget(central_widget) + main_layout = QVBoxLayout(central_widget) + main_layout.setSpacing(20) + main_layout.setContentsMargins(30, 30, 30, 30) + + # 1. 顶部标题（替代原实时时间） + self._create_title(main_layout) + + # 2. 下方左右分栏：左侧线条选择区（表格），右侧参数+3D区 + bottom_h_layout = QHBoxLayout() + bottom_h_layout.setSpacing(20) + + # 左侧：线条选择区（表格形式） + self._create_line_select_table(bottom_h_layout) + + # 右侧：参数设置 + 3D显示区 + self._create_right_area(bottom_h_layout) + + main_layout.addLayout(bottom_h_layout) + self._init_timer() + self._connect_signals() + + def _setup_style(self): + """设置界面样式""" + palette = QPalette() + palette.setColor(QPalette.Window, QColor(245, 245, 247)) + palette.setColor(QPalette.WindowText, QColor(30, 30, 30)) + palette.setColor(QPalette.Button, QColor(66, 133, 244)) + palette.setColor(QPalette.ButtonText, Qt.white) + self.setPalette(palette) + + self.setStyleSheet(""" + QGroupBox { + font-weight: bold; + border: 2px solid #4285F4; + border-radius: 8px; + margin-top: 10px; + padding-top: 10px; + } + QGroupBox::title { + subcontrol-origin: margin; + left: 10px; + padding: 0 8px 0 8px; + } + QPushButton { + border-radius: 6px; + padding: 8px 16px; + font-size: 12pt; + } + QPushButton:hover { + background-color: #5294FF; + } + QComboBox { + border: 2px solid #DDDDDD; + border-radius: 6px; + padding: 8px; + font-size: 10pt; + } + QComboBox:focus { + border-color: #4285F4; + outline: none; + } + QTextEdit { + border: 2px solid #DDDDDD; + border-radius: 6px; + background-color: white; + font-size: 11pt; + } + QTableWidget { + border: 2px solid #DDDDDD; + border-radius: 6px; + font-size: 11pt; + } + QTableWidget::item:selected { + background-color: #E1F5FE; + color: #212121; + } + """) + + def _create_title(self, parent_layout): + """顶部标题（替代原实时时间）""" + title_label = QLabel("线条长度标记与电机位置控制系统") + title_label.setFont(QFont("Microsoft YaHei", 20, QFont.Bold)) + title_label.setAlignment(Qt.AlignCenter) + parent_layout.addWidget(title_label) + + def _create_line_select_table(self, parent_layout): + """左侧线条选择区（表格形式，显示线条A/B的参数）""" + line_select_group = QGroupBox("线条选择（参数预览）") + line_select_group.setFont(self.font1) + line_select_group.setMinimumWidth(self.width() * 0.5) + select_layout = QVBoxLayout(line_select_group) + select_layout.setSpacing(20) + select_layout.setContentsMargins(20, 20, 20, 20) + + # 线条A表格 + a_label = QLabel("线条A参数表：") + a_label.setFont(self.font2) + self.table_line_a = QTableWidget() + self.table_line_a.setRowCount(len(self.line_a_params)) + self.table_line_a.setColumnCount(5) # 类型 + a/b/c/d + self.table_line_a.setHorizontalHeaderLabels(["类型", "参数a", "参数b", "参数c", "参数d"]) + # 填充线条A数据 + for row, (type_name, params) in enumerate(self.line_a_params.items()): + self.table_line_a.setItem(row, 0, QTableWidgetItem(type_name)) + self.table_line_a.setItem(row, 1, QTableWidgetItem(str(params[0]))) + self.table_line_a.setItem(row, 2, QTableWidgetItem(str(params[1]))) + self.table_line_a.setItem(row, 3, QTableWidgetItem(str(params[2]))) + self.table_line_a.setItem(row, 4, QTableWidgetItem(str(params[3]))) + self.table_line_a.setFixedHeight(220) + self.table_line_a.setEditTriggers(QTableWidget.NoEditTriggers) # 禁止编辑 + self.table_line_a.setSelectionBehavior(QTableWidget.SelectRows) # 按行选择 + self.table_line_a.selectRow(0) # 默认选中第一行 + self.table_line_a.itemSelectionChanged.connect(self._on_line_a_select) + + # 线条B表格 + b_label = QLabel("线条B参数表：") + b_label.setFont(self.font2) + self.table_line_b = QTableWidget() + self.table_line_b.setRowCount(len(self.line_b_params)) + self.table_line_b.setColumnCount(5) + self.table_line_b.setHorizontalHeaderLabels(["类型", "参数a", "参数b", "参数c", "参数d"]) + # 填充线条B数据 + for row, (type_name, params) in enumerate(self.line_b_params.items()): + self.table_line_b.setItem(row, 0, QTableWidgetItem(type_name)) + self.table_line_b.setItem(row, 1, QTableWidgetItem(str(params[0]))) + self.table_line_b.setItem(row, 2, QTableWidgetItem(str(params[1]))) + self.table_line_b.setItem(row, 3, QTableWidgetItem(str(params[2]))) + self.table_line_b.setItem(row, 4, QTableWidgetItem(str(params[3]))) + self.table_line_b.setFixedHeight(220) + self.table_line_b.setEditTriggers(QTableWidget.NoEditTriggers) + self.table_line_b.setSelectionBehavior(QTableWidget.SelectRows) + self.table_line_b.selectRow(0) + self.table_line_b.itemSelectionChanged.connect(self._on_line_b_select) + + # 导入按钮 + import_layout = QHBoxLayout() + import_a_btn = QPushButton("导入线条A选中参数") + import_a_btn.setFont(self.font2) + import_a_btn.clicked.connect(lambda: self._import_line_param("A")) + + import_b_btn = QPushButton("导入线条B选中参数") + import_b_btn.setFont(self.font2) + import_b_btn.clicked.connect(lambda: self._import_line_param("B")) + + import_layout.addWidget(import_a_btn) + import_layout.addWidget(import_b_btn) + + # 布局 + select_layout.addWidget(a_label) + select_layout.addWidget(self.table_line_a) + select_layout.addWidget(b_label) + select_layout.addWidget(self.table_line_b) + select_layout.addLayout(import_layout) + select_layout.addStretch() # 填充高度 + + parent_layout.addWidget(line_select_group) + + def _on_line_a_select(self): + """线条A表格选中行变化时，记录选中类型""" + selected_rows = self.table_line_a.selectionModel().selectedRows() + if selected_rows: + # 获取选中行的第一列（类型列）文本 + row = selected_rows[0].row() + self.selected_line_a = self.table_line_a.item(row, 0).text() + + def _on_line_b_select(self): + """线条B表格选中行变化时，记录选中类型""" + selected_rows = self.table_line_b.selectionModel().selectedRows() + if selected_rows: + row = selected_rows[0].row() + self.selected_line_b = self.table_line_b.item(row, 0).text() + + def _create_right_area(self, parent_layout): + """右侧区域：参数设置 + 3D显示区""" + right_v_layout = QVBoxLayout() + right_v_layout.setSpacing(20) + + # 1. 实时时间（移到右侧顶部） + time_group = QGroupBox("实时时间") + time_group.setFont(self.font1) + time_layout = QHBoxLayout(time_group) + self.time_label = QLabel() + self.time_label.setAlignment(Qt.AlignCenter) + self.time_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold)) + time_layout.addWidget(self.time_label) + right_v_layout.addWidget(time_group) + + # 2. 线条类型设置区 + control_group = QGroupBox("线条类型设置") + control_group.setFont(self.font1) + control_layout = QVBoxLayout(control_group) + control_layout.setSpacing(15) + control_layout.setContentsMargins(20, 20, 20, 20) + + # 提取所有参数a/b/c/d的可能值 + # 收集参数a的所有值 + # 提取所有参数a/b/c/d的可能值（统一格式） + # 收集参数a的所有值（保留2位小数，去除末尾0） + param_a_list = [] + for params in self.line_a_params.values(): + a = params[0] + a_str = f"{a:.2f}".rstrip('0').rstrip('.') if '.' in f"{a:.2f}" else f"{a:.2f}" + param_a_list.append(a_str) + for params in self.line_b_params.values(): + a = params[0] + a_str = f"{a:.2f}".rstrip('0').rstrip('.') if '.' in f"{a:.2f}" else f"{a:.2f}" + param_a_list.append(a_str) + param_a_list = sorted(list(set(param_a_list)), key=lambda x: float(x)) + + # 收集参数b的所有值（整数格式） + param_b_list = [] + for params in self.line_a_params.values(): + b_str = f"{params[1]:.0f}" + param_b_list.append(b_str) + for params in self.line_b_params.values(): + b_str = f"{params[1]:.0f}" + param_b_list.append(b_str) + param_b_list = sorted(list(set(param_b_list)), key=lambda x: int(x)) + + # 收集参数c的所有值（保留2位小数，去除末尾0） + param_c_list = [] + for params in self.line_a_params.values(): + c = params[2] + c_str = f"{c:.2f}".rstrip('0').rstrip('.') if '.' in f"{c:.2f}" else f"{c:.2f}" + param_c_list.append(c_str) + for params in self.line_b_params.values(): + c = params[2] + c_str = f"{c:.2f}".rstrip('0').rstrip('.') if '.' in f"{c:.2f}" else f"{c:.2f}" + param_c_list.append(c_str) + param_c_list = sorted(list(set(param_c_list)), key=lambda x: float(x)) + + # 收集参数d的所有值（整数格式） + param_d_list = [] + for params in self.line_a_params.values(): + d_str = f"{params[3]:.0f}" + param_d_list.append(d_str) + for params in self.line_b_params.values(): + d_str = f"{params[3]:.0f}" + param_d_list.append(d_str) + param_d_list = sorted(list(set(param_d_list)), key=lambda x: int(x)) + + # 4个参数下拉框（横向布局） + param_layout = QHBoxLayout() + param_layout.setSpacing(20) + + # 参数a + a_param_layout = QVBoxLayout() + label_a = QLabel("参数a:") + label_a.setFont(self.font2) + label_a.setAlignment(Qt.AlignCenter) + self.combo_a = QComboBox() + self.combo_a.addItems(param_a_list) + self.combo_a.setFixedWidth(100) + self.combo_a.setFont(self.font2) + a_param_layout.addWidget(label_a) + a_param_layout.addWidget(self.combo_a) + param_layout.addLayout(a_param_layout) + + # 参数b + b_param_layout = QVBoxLayout() + label_b = QLabel("参数b:") + label_b.setFont(self.font2) + label_b.setAlignment(Qt.AlignCenter) + self.combo_b = QComboBox() + self.combo_b.addItems(param_b_list) + self.combo_b.setFixedWidth(100) + self.combo_b.setFont(self.font2) + b_param_layout.addWidget(label_b) + b_param_layout.addWidget(self.combo_b) + param_layout.addLayout(b_param_layout) + + # 参数c + c_param_layout = QVBoxLayout() + label_c = QLabel("参数c:") + label_c.setFont(self.font2) + label_c.setAlignment(Qt.AlignCenter) + self.combo_c = QComboBox() + self.combo_c.addItems(param_c_list) + self.combo_c.setFixedWidth(100) + self.combo_c.setFont(self.font2) + c_param_layout.addWidget(label_c) + c_param_layout.addWidget(self.combo_c) + param_layout.addLayout(c_param_layout) + + # 参数d + d_param_layout = QVBoxLayout() + label_d = QLabel("参数d:") + label_d.setFont(self.font2) + label_d.setAlignment(Qt.AlignCenter) + self.combo_d = QComboBox() + self.combo_d.addItems(param_d_list) + self.combo_d.setFixedWidth(100) + self.combo_d.setFont(self.font2) + d_param_layout.addWidget(label_d) + d_param_layout.addWidget(self.combo_d) + param_layout.addLayout(d_param_layout) + + # 按钮行 + button_layout = QHBoxLayout() + self.save_btn = QPushButton("保存设置") + self.save_btn.setFont(self.font2) + self.save_btn.setFixedSize(140, 45) # 宽140px，高45px + + self.clear_btn = QPushButton("清空选择") + self.clear_btn.setFont(self.font2) + self.clear_btn.setFixedSize(140, 45) # 宽140px，高45px + + self.export_btn = QPushButton("导出记录") + self.export_btn.setFont(self.font2) + self.export_btn.setFixedSize(140, 45) # 宽140px，高45px + + button_layout.addStretch() + button_layout.addWidget(self.save_btn) + button_layout.addWidget(self.clear_btn) + button_layout.addWidget(self.export_btn) + button_layout.addStretch() + + control_layout.addLayout(param_layout) + control_layout.addLayout(button_layout) + right_v_layout.addWidget(control_group) + + # 2. 3D效果显示占位区 + self._create_3d_placeholder(right_v_layout) + + parent_layout.addLayout(right_v_layout) + + def _create_3d_placeholder(self, parent_layout): + """3D效果显示占位区（缩小版）""" + display_group = QGroupBox("线条3D效果显示（后续接口）") + display_group.setFont(self.font1) + display_layout = QVBoxLayout(display_group) + display_layout.setContentsMargins(20, 20, 20, 20) + + placeholder_text = QTextEdit() + placeholder_text.setReadOnly(True) + placeholder_text.setMaximumHeight(180) + placeholder_text.setText("""3D接口说明： +1. 后续集成3D模块（如Qt3DExtras） +2. 参数a→半径，参数b→长度，参数c/d→颜色 +3. 保存参数触发record_data_signal更新3D""") + placeholder_text.setFont(QFont("Microsoft YaHei", 10)) + + test_btn = QPushButton("测试参数传递") + test_btn.setFont(self.font2) + test_btn.setFixedWidth(150) + test_btn.clicked.connect(self._test_3d_param_pass) + + display_layout.addWidget(placeholder_text) + display_layout.addWidget(test_btn, alignment=Qt.AlignCenter) + parent_layout.addWidget(display_group) + + def _init_timer(self): + """初始化定时器""" + self.timer = QTimer(self) + self.timer.timeout.connect(self._update_current_time) + self.timer.start(1000) + self._update_current_time() + + def _connect_signals(self): + """连接信号与槽函数""" + self.save_btn.clicked.connect(self._save_settings) + self.clear_btn.clicked.connect(self._clear_inputs) + self.export_btn.clicked.connect(self._export_records) + self.update_time_signal.connect(self.time_label.setText) + self.record_data_signal.connect(self._log_param_change) + self.line_select_signal.connect(self._set_line_params) + + def _update_current_time(self): + """更新当前时间""" + current_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()) + self.update_time_signal.emit(current_time) + + @Slot(str) + def _import_line_param(self, line_type): + """导入选中线条的参数""" + if line_type == "A": + selected_type = self.selected_line_a + params = self.line_a_params[selected_type] + line_name = f"线条A-{selected_type}" + else: + selected_type = self.selected_line_b + params = self.line_b_params[selected_type] + line_name = f"线条B-{selected_type}" + + self.line_select_signal.emit(line_name, *params) + QMessageBox.information(self, "导入成功", f"已导入{line_name}的参数") + + @Slot(str, float, float, float, float) + def _set_line_params(self, line_name, a, b, c, d): + """设置参数到下拉框（统一浮点数格式）""" + # 根据参数特性设置小数位：a/c保留2位，b/d保留0位（整数） + a_str = f"{a:.2f}".rstrip('0').rstrip('.') if '.' in f"{a:.2f}" else f"{a:.2f}" + b_str = f"{b:.0f}" # 参数b是整数（5/7/8等） + c_str = f"{c:.2f}".rstrip('0').rstrip('.') if '.' in f"{c:.2f}" else f"{c:.2f}" + d_str = f"{d:.0f}" # 参数d是整数（25/26/28等） + + """设置参数到下拉框""" + self.combo_a.setCurrentText(a_str) + self.combo_b.setCurrentText(b_str) + self.combo_c.setCurrentText(c_str) + self.combo_d.setCurrentText(d_str) + + @Slot() + def _save_settings(self): + """保存参数并触发信号""" + try: + param_a = float(self.combo_a.currentText()) + param_b = float(self.combo_b.currentText()) + param_c = float(self.combo_c.currentText()) + param_d = float(self.combo_d.currentText()) + except ValueError: + QMessageBox.critical(self, "输入错误", "参数格式无效！") + return + + if param_a <= 0 or param_b <= 0 or param_c < 0 or param_d < 0: + QMessageBox.warning(self, "参数错误", "参数a/b必须大于0，c/d不能小于0！") + return + + self.record_data_signal.emit(param_a, param_b, param_c, param_d) + QMessageBox.information(self, "保存成功", + f"已保存设置：\n参数a：{param_a}\n参数b：{param_b}\n参数c：{param_c}\n参数d：{param_d}") + + @Slot(float, float, float, float) + def _log_param_change(self, a, b, c, d): + """记录参数变化""" + self.recorded_data.append({ + "a": a, "b": b, "c": c, "d": d, + "time": time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()) + }) + + @Slot() + def _test_3d_param_pass(self): + """测试参数传递""" + try: + param_a = float(self.combo_a.currentText()) + param_b = float(self.combo_b.currentText()) + param_c = float(self.combo_c.currentText()) + param_d = float(self.combo_d.currentText()) + except ValueError: + QMessageBox.critical(self, "测试失败", "当前参数格式无效！") + return + + QMessageBox.information( + self, "参数传递测试成功", + f"当前参数已准备好传递给3D模块：\n" + f"参数a（半径）：{param_a}\n" + f"参数b（长度）：{param_b}\n" + f"参数c（颜色通道1）：{param_c}\n" + f"参数d（颜色通道2）：{param_d}" + ) + + @Slot() + def _clear_inputs(self): + """清空所有选择""" + self.combo_a.setCurrentIndex(0) + self.combo_b.setCurrentIndex(0) + self.combo_c.setCurrentIndex(0) + self.combo_d.setCurrentIndex(0) + self.table_line_a.selectRow(0) + self.table_line_b.selectRow(0) + + @Slot() + def _export_records(self): + """导出参数记录""" + if not self.recorded_data: + QMessageBox.warning(self, "导出提示", "暂无记录可导出！") + return + + try: + filename = f"线条参数记录_{time.strftime('%Y%m%d_%H%M%S')}.txt" + with open(filename, 'w', encoding='utf-8') as f: + f.write("线条参数记录\n") + f.write("=" * 60 + "\n") + f.write(f"导出时间：{time.strftime('%Y-%m-%d %H:%M:%S')}\n") + f.write("参数a\t参数b\t参数c\t参数d\t记录时间\n") + f.write("-" * 60 + "\n") + for data in self.recorded_data: + f.write(f"{data['a']:.2f}\t{data['b']:.2f}\t{data['c']:.2f}\t{data['d']:.2f}\t{data['time']}\n") + QMessageBox.information(self, "导出成功", f"记录已导出到：\n{filename}") + except Exception as e: + QMessageBox.critical(self, "导出失败", f"导出文件时出错：{str(e)}") + + def closeEvent(self, event): + """关闭窗口确认""" + reply = QMessageBox.question(self, "确认退出", + "是否确定退出程序？", + QMessageBox.Yes | QMessageBox.No, + QMessageBox.No) + if reply == QMessageBox.Yes: + self.timer.stop() + event.accept() + else: + event.ignore() + +# ----------界面对外接口----------- +def ui_main(): + app = QApplication(sys.argv) + app.setApplicationName("长度电机控制器") + app.setApplicationVersion("1.0.0") + window = LengthMotorController() + window.show() + sys.exit(app.exec()) + +# ----------测试接口-------------- +if __name__ == "__main__": + ui_main() \ No newline at end of file diff --git a/UI/线条参数记录_20251208_142616.txt b/UI/线条参数记录_20251208_142616.txt new file mode 100644 index 0000000..a20f8f7 --- /dev/null +++ b/UI/线条参数记录_20251208_142616.txt @@ -0,0 +1,6 @@ +线条参数记录 +============================================================ +导出时间：2025-12-08 14:26:16 +序号 参数a 参数b 参数c 参数d 记录时间 +------------------------------------------------------------ +1 0.50 5.00 0.10 25.00 2025-12-08 14:26:13 diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..b3e5942 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,5 @@ +numpy>=2.0.2 +pyserial>=3.5 +pyside6==6.9.1 +pymodbus==3.8.6 +python-periphery \ No newline at end of file diff --git a/servo/servo.py b/servo/servo.py new file mode 100644 index 0000000..30436fc --- /dev/null +++ b/servo/servo.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/12/2 14:05 +# @Author : reenrr +# @File :servo.py +# @Description : 控制舵机正反转 +''' + +from servo_sdk import * +import logging + +# -------------参数配置-------------- +BAUDRATE = 115200 # 舵机的波特率 +PORT = 'COM4' +SERVO_IDS = [1, 2, 3, 4, 5] # 舵机们的 ID 号 +POS_START = 2047 +POS_END = 0 +SPEED = 1500 +ACC = 0 +TIME_INTERVAL1 = ((2047-0) / 1500) + 2 # 翻转回来的时间 +TIME_INTERVAL2 = 10 # 不用翻转运行的时间 + +class ServoController: + def __init__(self, config=None): + """ + 初始化舵机控制器 + """ + self.config = config + self.time_interval1 = self.config['time_interval1'] + self.time_interval2 = self.config['time_interval2'] + + # 初始化串口和舵机处理器 + self.port_handler = PortHandler(self.config['port']) + self.servo_handler = HxServoHandler(self.port_handler) + + # 初始化状态 + self.is_running = False + + def setup_port(self): + """配置串口参数""" + # 打开串口 + if self.port_handler.openPort(): + logging.info("成功打开串口") + else: + raise RuntimeError("打开串口失败") + + # 设置波特率 + if self.port_handler.setBaudRate(self.config['baudrate']): + logging.info("设置波特率成功") + else: + self.port_handler.closePort() + raise RuntimeError("设置波特率失败") + + def enable_all_servos(self): + """使能所有舵机""" + enable_success = True + for servo_id in self.config['servo_ids']: + enable_result = self.servo_handler.torqueEnable(servo_id) + if enable_result[1] != 0: + logging.info(f"[ID:{servo_id:02d}] 舵机使能失败，错误信息：{enable_result[1]}") + enable_success = False + else: + logging.info(f"[ID:{servo_id:02d}] 舵机使能成功") + + if not enable_success: + self.cleanup() + raise RuntimeError("使能舵机失败,程序退出") + + logging.info("所有舵机使能成功") + + def disable_all_servos(self): + """失能所有舵机""" + disable_success = True + for servo_id in self.config['servo_ids']: + disable_result = self.servo_handler.torqueDisable(servo_id) + if disable_result[1] != 0: + logging.info(f"[ID:{servo_id:02d}] 舵机失能失败，错误信息：{disable_result[1]}") + disable_success = False + else: + logging.info(f"[ID:{servo_id:02d}] 舵机失能成功") + + if not disable_success: + self.cleanup() + raise RuntimeError("失能舵机失败,程序退出") + + logging.info("所有舵机失能成功") + + def write_position(self, position, speed, acc): + """ + 写入目标位置到所有舵机 + :param position: 目标位置 + :param speed: 速度 + :param acc: 加速度 + """ + for servo_id in self.config['servo_ids']: + add_param_result = self.servo_handler.syncWritePosEx( + servo_id, position, speed, acc + ) + if not add_param_result: + logging.info(f"[ID:{servo_id:02d}] 添加参数失败") + continue + + result = self.servo_handler.GroupSyncWrite.txPacket() + if result != COMM_SUCCESS: + logging.info(f"[ID:{servo_id:02d}] 发送指令失败：{result.getTxRxResult(result)}") + logging.info("复位成功") + + # 清空参数缓存 + self.servo_handler.GroupSyncWrite.clearParam() + + def stop(self): + """停止舵机运行""" + self.is_running = False + + def cleanup(self): + """清理资源""" + self.disable_all_servos() + if self.port_handler.is_open(): + self.port_handler.closePort() + logging.info("串口已关闭") + + +# ---------舵机对外接口---------- +def servo_rotate(): + """复位位置之后，先转到180度，延时TIME_INTERVAL1一段时间，再转到0度""" + custom_config = { + 'port': PORT, + 'baudrate': BAUDRATE, + 'servo_ids': SERVO_IDS, + 'pos_start': POS_START, + 'pos_end': POS_END, + 'speed': SPEED, + 'acc': ACC, + 'time_interval1': TIME_INTERVAL1, + 'time_interval2': TIME_INTERVAL2 + } + + controller = ServoController(custom_config) + + try: + # 初始化串口 + controller.setup_port() + # 使能所有舵机 + controller.enable_all_servos() + # 复位位置 + controller.write_position(controller.config['pos_end'], + controller.config['speed'], + controller.config['acc']) + + """运行舵机运动""" + logging.info("舵机开始循环运动（按Ctrl+C终止）...") + + # 运动到起始位置（180度） + controller.write_position(controller.config['pos_start'], + controller.config['speed'], + controller.config['acc']) + logging.info("运动到180度") + time.sleep(controller.time_interval1) + + # 运动到结束位置（0度） + controller.write_position(controller.config['pos_end'], + controller.config['speed'], + controller.config['acc']) + logging.info("运动到0度") + time.sleep(controller.time_interval2) + + except KeyboardInterrupt: + logging.info("\n用户终止程序...") + controller.stop() + except RuntimeError as e: + logging.info(f"\n运行错误：{str(e)}") + except Exception as e: + logging.info(f"\n程序运行异常：{str(e)}") + finally: + controller.cleanup() + + +# -----------测试接口------------ +if __name__ == '__main__': + servo_rotate() + + diff --git a/servo/servo_sdk/__init__.py b/servo/servo_sdk/__init__.py new file mode 100644 index 0000000..3bbc947 --- /dev/null +++ b/servo/servo_sdk/__init__.py @@ -0,0 +1,3 @@ +from .hx_30_hm import * +from .port_handler import * +from .protocol_paket_handler import * \ No newline at end of file diff --git a/servo/servo_sdk/hx_30_hm.py b/servo/servo_sdk/hx_30_hm.py new file mode 100644 index 0000000..8e0d96e --- /dev/null +++ b/servo/servo_sdk/hx_30_hm.py @@ -0,0 +1,187 @@ +from .protocol_paket_handler import * + +#baudrate define +HX_30HM_1M = 0 +HX_30HM_0_5M = 1 +HX_30HM_250K = 2 +HX_30HM_128K = 3 +HX_30HM_115200 = 4 +HX_30HM_76800 = 5 +HX_30HM_57600 = 6 +HX_30HM_38400 = 7 + +#Memory table + +#NVS +HX_30HM_SERVO_MAIN_VERSION = 3 +HX_30HM_SERVO_SEC_VERSION = 4 +HX_30HM_ID = 5 +HX_30HM_BAUD_RATE = 6 +HX_30HM_CW_DEAD = 26 +HX_30HM_CCW_DEAD = 27 +HX_30HM_POS_OFFSET_L = 31 +HX_30HM_POS_OFFSET_H = 32 +HX_30HM_MODE = 33 + +#SRAM + +#Write read +HX_30HM_TORQUE_ENABLE = 40 +HX_30HM_ACC = 41 +HX_30HM_GOAL_POSITION_L = 42 +HX_30HM_GOAL_POSITION_H = 43 +HX_30HM_PWM_SPEED_L = 44 +HX_30HM_PWM_SPEED_H = 45 +HX_30HM_GOAL_SPEED_L = 46 +HX_30HM_GOAL_SPEED_H = 47 +HX_30HM_MAX_TORQUE_L = 48 +HX_30HM_MAX_TORQUE_H = 49 + +#Only read +HX_30HM_PRESENT_POSITION_L = 56 +HX_30HM_PRESENT_POSITION_H = 57 +HX_30HM_PRESENT_SPEED_L = 58 +HX_30HM_PRESENT_SPEED_H = 59 +HX_30HM_PRESENT_LOAD_L = 60 +HX_30HM_PRESENT_LOAD_H = 61 +HX_30HM_PRESENT_VOLTAGE = 62 +HX_30HM_PRESENT_TEMPERATURE = 63 +HX_30HM_MOVING_STATUS = 66 +HX_30HM_PRESENT_CURRENT_L = 69 +HX_30HM_PRESENT_CURRENT_H = 70 + + +class HxServoHandler(PacketHandler): + def __init__(self, PortHandler): + PacketHandler.__init__(self, PortHandler, 0) + self.GroupSyncWrite = GroupSyncWrite(self, HX_30HM_ACC, 7) + + def selectMode(self, id, mode): + _mode = [max(0, min(mode, 3))] + return self.writeReadData(id, HX_30HM_MODE, 1, _mode) + + def torqueEnable(self, id): + state = [1] + return self.writeReadData(id, HX_30HM_TORQUE_ENABLE, 1, state) + + def torqueDisable(self, id): + state = [0] + return self.writeReadData(id, HX_30HM_TORQUE_ENABLE, 1, state) + + def writeCurPosOffset(self, id): + state = [128] + return self.writeReadData(id, HX_30HM_TORQUE_ENABLE, 1, state) + + def writePosOffset(self, id, offset): + _offset = max(-2047, min(offset, 2047)) + _offset = self.toServo(_offset, 11) + txpacket = [self.getLowByte(_offset), self.getHighByte(_offset)] + + return self.writeReadData(id, HX_30HM_POS_OFFSET_L, len(txpacket), txpacket) + + def writeAcc(self, id, acc): + _acc = [max(0, min(acc, 254))] + return self.writeReadData(id, HX_30HM_ACC, 1, _acc) + + def writeSpeed(self, id, speed): + _speed = max(0, min(speed, 3400)) + txpacket = [self.getLowByte(_speed), self.getHighByte(_speed)] + return self.writeReadData(id, HX_30HM_GOAL_SPEED_L, len(txpacket), txpacket) + + def writePos(self, id, pos): + _pos = max(-30719, min(pos, 30719)) + _pos = self.toServo(_pos, 15) + txpacket = [self.getLowByte(_pos), self.getHighByte(_pos)] + return self.writeReadData(id, HX_30HM_GOAL_POSITION_L, len(txpacket), txpacket) + + def writePosEx(self, id, pos, speed, acc): + _pos = max(-30719, min(pos, 30719)) + _speed = max(0, min(speed, 3400)) + _acc = max(0, min(acc, 254)) + + _pos = self.toServo(_pos, 15) + + txpacket = [_acc, + self.getLowByte(_pos), self.getHighByte(_pos), + 0,0, + self.getLowByte(_speed), self.getHighByte(_speed)] + + return self.writeReadData(id, HX_30HM_ACC, len(txpacket), txpacket) + + def writePwmSpeed(self, id, speed): + _speed = max(-1000, min(speed, 1000)) + _speed = self.toServo(_speed, 10) + txpacket = [self.getLowByte(_speed), self.getHighByte(_speed)] + return self.writeReadData(id, HX_30HM_PWM_SPEED_L, len(txpacket), txpacket) + + def writeMaxTorque(self, id, max_torque): + _max_torque = max(0, min(max_torque, 1000)) + print(_max_torque) + txpacket = [self.getLowByte(_max_torque), self.getHighByte(_max_torque)] + return self.writeReadData(id, HX_30HM_MAX_TORQUE_L, len(txpacket), txpacket) + + def writeRegPosEx(self, id, pos, speed, acc): + _pos = max(-30719, min(pos, 30719)) + _speed = max(0, min(speed, 3400)) + _acc = max(0, min(acc, 254)) + _pos = self.toHost(_pos, 15) + txpacket = [_acc, + self.getLowByte(_pos), self.getHighByte(_pos), + 0,0, + self.getLowByte(_speed), self.getHighByte(_speed)] + return self.regWriteTxRx(id, HX_30HM_ACC, len(txpacket), txpacket) + + def regAction(self): + return self.action(BROADCAST_ID) + + def syncWritePosEx(self, id, pos, speed, acc): + _pos = max(-30719, min(pos, 30719)) + _speed = max(0, min(speed, 3400)) + _acc = max(0, min(acc, 254)) + txpacket = [_acc, + self.getLowByte(_pos), self.getHighByte(_pos), + 0,0, + self.getLowByte(_speed), self.getHighByte(_speed)] + return self.GroupSyncWrite.addParam(id, txpacket) + + def readPosOffset(self, id): + offset, result, error = self.read2ByteData(id, HX_30HM_POS_OFFSET_L) + # The representation of negative numbers + return -(~(offset - 1) & 0xFFFF) if offset > 2047 else offset, result, error + + def readPos(self, id): + present_position, result, error = self.read2ByteData(id, HX_30HM_PRESENT_POSITION_L) + return self.toHost(present_position, 15), result, error + + def readSpeed(self, id): + present_speed, result, error = self.read2ByteData(id, HX_30HM_PRESENT_SPEED_L) + return self.toHost(present_speed, 15), result, error + + def readPosSpeed(self, id): + present_position_speed, result, error = self.read4ByteData(id, HX_30HM_PRESENT_POSITION_L) + present_position = self.getLowWord32(present_position_speed) + present_speed = self.get_Highword32(present_position_speed) + return self.toHost(present_position, 15), self.toHost(present_speed, 15), result, error + + def readTemperature(self, id): + present_temp, result, error = self.read1ByteData(id, HX_30HM_PRESENT_TEMPERATURE) + return present_temp, result, error + + def readVoltage(self, id): + present_vol, result, error = self.read1ByteData(id, HX_30HM_PRESENT_VOLTAGE) + return present_vol, result, error + + def readCurrent(self, id): + present_cur, result, error = self.read2ByteData(id, HX_30HM_PRESENT_CURRENT_L) + return present_cur, result, error + + def readLoad(self, id): + load, result, error = self.read2ByteData(id, HX_30HM_PRESENT_LOAD_L) + return self.toHost(load, 10), result, error + + def readMoving(self, id): + moving, result, error = self.read1ByteData(id, HX_30HM_MOVING_STATUS) + return moving, result, error + + + diff --git a/servo/servo_sdk/port_handler.py b/servo/servo_sdk/port_handler.py new file mode 100644 index 0000000..842cd9b --- /dev/null +++ b/servo/servo_sdk/port_handler.py @@ -0,0 +1,113 @@ +import time +import serial +import sys +import platform + +DEFAULT_BAUDRATE = 1000000 +LATENCY_TIMER = 50 + +class PortHandler(object): + def __init__(self, port_name): + self.is_open = False + self.baudrate = DEFAULT_BAUDRATE + self.packet_start_time = 0.0 + self.packet_timeout = 0.0 + self.tx_time_per_byte = 0.0 + + self.is_using = False + self.port_name = port_name + self.ser = None + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + self.is_open = False + + def clearPort(self): + self.ser.flush() + + def setPortName(self, port_name): + self.port_name = port_name + + def getPortName(self): + return self.port_name + + def setBaudRate(self, baudrate): + baud = self.getCFlagBaud(baudrate) + + if baud <= 0: + # self.setupPort(38400) + # self.baudrate = baudrate + return False # TODO: setCustomBaudrate(baudrate) + else: + self.baudrate = baudrate + return self.setupPort(baud) + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): + return self.ser.in_waiting + + def readPort(self, length): + if (sys.version_info > (3, 0)): + return self.ser.read(length) + else: + return [ord(ch) for ch in self.ser.read(length)] + + def writePort(self, packet): + return self.ser.write(packet) + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER + + def setPacketTimeoutMillis(self, msec): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = msec + + def isPacketTimeout(self): + if self.getTimeSinceStart() > self.packet_timeout: + self.packet_timeout = 0 + return True + + return False + + def getCurrentTime(self): + return round(time.time() * 1000000000) / 1000000.0 + + def getTimeSinceStart(self): + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: + self.packet_start_time = self.getCurrentTime() + + return time_since + + def setupPort(self, cflag_baud): + if self.is_open: + self.closePort() + + self.ser = serial.Serial( + port=self.port_name, + baudrate=self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize=serial.EIGHTBITS, + timeout=0 + ) + + self.is_open = True + + self.ser.reset_input_buffer() + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def getCFlagBaud(self, baudrate): + if baudrate in [4800, 9600, 14400, 19200, 38400, 57600, 115200, 128000, 250000, 500000, 1000000]: + return baudrate + else: + return -1 diff --git a/servo/servo_sdk/protocol_paket_handler.py b/servo/servo_sdk/protocol_paket_handler.py new file mode 100644 index 0000000..bed1856 --- /dev/null +++ b/servo/servo_sdk/protocol_paket_handler.py @@ -0,0 +1,713 @@ + +TXPACKET_MAX_LEN = 250 +RXPACKET_MAX_LEN = 250 + +# for Protocol Packet +PKT_HEADER0 = 0 +PKT_HEADER1 = 1 +PKT_ID = 2 +PKT_LENGTH = 3 +PKT_INSTRUCTION = 4 +PKT_ERROR = 4 +PKT_PARAMETER0 = 5 + +# Protocol Error bit +ERRBIT_VOLTAGE = 1 #0x01 +ERRBIT_SENSOR = 2 #0x02 +ERRBIT_OVERHEAT = 4 #0x04 +ERRBIT_CURRENT = 8 #0x08 +ERRBIT_ANGLE = 16 #0x10 +ERRBIT_OVERLOAD = 32 #0x20 + +BROADCAST_ID = 254 #0xFE + +# Instruction for Servo Protocol +INST_PING = 1 #0x01 +INST_READ = 2 #0x02 +INST_WRITE = 3 #0x03 +INST_REG_WRITE = 4 #0x04 +INST_ACTION = 5 #0x05 +INST_RESET = 6 #0x06 +INST_SYNC_READ = 130 #0x82 +INST_SYNC_WRITE = 131 #0x83 + + +# Communication Result +COMM_SUCCESS = 0 # tx or rx packet communication success +COMM_PORT_BUSY = -1 # Port is busy (in use) +COMM_TX_FAIL = -2 # Failed transmit instruction packet +COMM_RX_FAIL = -3 # Failed get status packet +COMM_TX_ERROR = -4 # Incorrect instruction packet +COMM_RX_WAITING = -5 # Now receiving status packet +COMM_RX_TIMEOUT = -6 # There is no status packet +COMM_RX_CORRUPT = -7 # Incorrect status packet +COMM_NOT_AVAILABLE = -9 #Protocol does not support this function + +class PacketHandler(object): + def __init__(self, PortHandler, endianness): + self.PortHandler = PortHandler + self.endianness = endianness # 0: little-endian 1:big-endian + + def getEndian(self): + return self.endianness + + def setEndian(self, e): + self.endianness = e + + def toHost(self, a, b): + if (a & (1 << b)): + return -(a & ~(1 << b)) + else: + return a + + def toServo(self, a, b): + if (a < 0): + return (-a | (1 << b)) + else: + return a + + def makeWord16(self, a, b): + if self.endianness == 0: + return (a & 0xFF) | ((b & 0xFF) << 8) + else: + return (b & 0xFF) | ((a & 0xFF) << 8) + + def makeWord32(self, a, b): + return (a & 0xFFFF) | (b & 0xFFFF) << 16 + + def getLowWord32(self, l): + return l & 0xFFFF + + def get_Highword32(self, h): + return (h >> 16) & 0xFFFF + + def getLowByte(self, w): + if self.endianness == 0: + return w & 0xFF + else: + return (w >> 8) & 0xFF + + def getHighByte(self, w): + if self.endianness == 0: + return (w >> 8) & 0xFF + else: + return w & 0xFF + + def getTxRxResult(self, result): + if result == COMM_SUCCESS: + return "[TxRxResult] Communication success!" + elif result == COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!" + elif result == COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!" + elif result == COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!" + elif result == COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!" + elif result == COMM_RX_WAITING: + return "[TxRxResult] Now receiving status packet!" + elif result == COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!" + elif result == COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!" + elif result == COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support this function!" + else: + return "" + + def getRxPacketError(self, error): + if error & ERRBIT_VOLTAGE: + return "[ServoStatus] Input voltage error!" + + if error & ERRBIT_SENSOR: + return "[ServoStatus] Sensor error!" + + if error & ERRBIT_OVERHEAT: + return "[ServoStatus] Overheat error!" + + if error & ERRBIT_CURRENT: + return "[ServoStatus] Current error!" + + if error & ERRBIT_ANGLE: + return "[ServoStatus] Angle error!" + + if error & ERRBIT_OVERLOAD: + return "[ServoStatus] Overload error!" + + return "" + + def txPacket(self, txpacket): + checksum = 0 + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + + if self.PortHandler.is_using: + return COMM_PORT_BUSY + self.PortHandler.is_using = True + + # check max packet length + if total_packet_length > TXPACKET_MAX_LEN: + self.PortHandler.is_using = False + return COMM_TX_ERROR + + # make packet header + txpacket[PKT_HEADER0] = 0xFF + txpacket[PKT_HEADER1] = 0xFF + + # add a checksum to the packet + for index in range(2, total_packet_length - 1): # except header, checksum + checksum += txpacket[index] + + txpacket[total_packet_length - 1] = ~checksum & 0xFF + + # tx packet + self.PortHandler.clearPort() + written_packet_length = self.PortHandler.writePort(txpacket) + if total_packet_length != written_packet_length: + self.PortHandler.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self): + rxpacket = [] + + result = COMM_TX_FAIL + checksum = 0 + rx_length = 0 + wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + + while True: + rxpacket.extend(self.PortHandler.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + # find packet header + index = 0 + for index in range(0, (rx_length - 1)): + if (rxpacket[index] == 0xFF) and (rxpacket[index + 1] == 0xFF): + break + + if index == 0: # found at the beginning of the packet + if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or ( + rxpacket[PKT_ERROR] > 0x7F): + # unavailable ID or unavailable Length or unavailable Error + # remove the first byte in the packet + del rxpacket[0] + rx_length -= 1 + continue + + # re-calculate the exact length of the rx packet + if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1): + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1 + continue + + if rx_length < wait_length: + # check timeout + if self.PortHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + else: + continue + + # calculate checksum + for i in range(2, wait_length - 1): # except header, checksum + checksum += rxpacket[i] + checksum = ~checksum & 0xFF + + # verify checksum + if rxpacket[wait_length - 1] == checksum: + result = COMM_SUCCESS + else: + result = COMM_RX_CORRUPT + break + + else: + # remove unnecessary packets + del rxpacket[0: index] + rx_length -= index + + else: + # check timeout + if self.PortHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + self.PortHandler.is_using = False + return rxpacket, result + + def txRxPacket(self, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(txpacket) + if result != COMM_SUCCESS: + return rxpacket, result, error + + # (ID == Broadcast ID) == no need to wait for status packet or not available + if txpacket[PKT_ID] == BROADCAST_ID: + self.PortHandler.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + self.PortHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + self.PortHandler.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket() + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, id): + + error = 0 + + txpacket = [0] * 6 + + if id > BROADCAST_ID: + return COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(txpacket) + + return rxpacket, result, error + + def action(self, id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(txpacket) + + return result + + def readData(self, id, address, length): + txpacket = [0] * 8 + data = [] + + if id > BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, 0 + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + rxpacket, result, error = self.txRxPacket(txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def read1ByteData(self, id, address): + data, result, error = self.readData(id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteData(self, id, address): + data, result, error = self.readData(id, address, 2) + data_read = self.makeWord16(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteData(self, id, address): + data, result, error = self.readData(id, address, 4) + data_read = self.makeWord32(self.makeWord16(data[0], data[1]), + self.makeWord16(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeDataOnly(self, id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(txpacket) + self.PortHandler.is_using = False + + return result + + def writeReadData(self, id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + rxpacket, result, error = self.txRxPacket(txpacket) + + return result, error + + def write1ByteDataOnly(self, id, address, data): + data_write = [data] + return self.writeDataOnly(id, address, 1, data_write) + + def writeRead1ByteData(self, id, address, data): + data_write = [data] + return self.writeReadData(id, address, 1, data_write) + + def write2ByteDataOnly(self, id, address, data): + data_write = [self.getLowByte(data), self.getHighByte(data)] + return self.writeDataOnly(id, address, 2, data_write) + + def writeRead2ByteData(self, id, address, data): + data_write = [self.getLowByte(data), self.getHighByte(data)] + return self.writeReadData(id, address, 2, data_write) + + def write4ByteDataOnly(self, id, address, data): + data_write = [self.getLowByte(self.getLowWord32(data)), + self.getHighByte(self.getLowWord32(data)), + self.getLowByte(self.get_Highword32(data)), + self.getHighByte(self.get_Highword32(data))] + return self.writeDataOnly(id, address, 4, data_write) + + def writeRead4ByteData(self, id, address, data): + data_write = [self.getLowByte(self.getLowWord32(data)), + self.getHighByte(self.getLowWord32(data)), + self.getLowByte(self.get_Highword32(data)), + self.getHighByte(self.get_Highword32(data))] + return self.writeReadData(id, address, 4, data_write) + + def regWriteTxOnly(self, id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(txpacket) + self.PortHandler.is_using = False + + return result + + def regWriteTxRx(self, id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + _, result, error = self.txRxPacket(txpacket) + + return result, error + + def syncReadTx(self, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 7: INST START_ADDR DATA_LEN CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + result = self.txPacket(txpacket) + + return result + + def syncReadRx(self, data_length, param_length): + wait_length = (6 + data_length) * param_length + self.PortHandler.setPacketTimeout(wait_length) + rxpacket = [] + rx_length = 0 + while True: + rxpacket.extend(self.PortHandler.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + result = COMM_SUCCESS + break + else: + # check timeout + if self.PortHandler.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + self.PortHandler.is_using = False + return result, rxpacket + + def syncWriteTxOnly(self, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + + _, result, _ = self.txRxPacket(txpacket) + + return result + + def reset(self, id): + error = 0 + + txpacket = [0] * 6 + + if id > BROADCAST_ID: + return COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_RESET + + rxpacket, result, error = self.txRxPacket(txpacket) + + return result, error + +class GroupSyncRead: + def __init__(self, handler, start_address, data_length): + self.handler = handler + self.start_address = start_address + self.data_length = data_length + + self.last_result = False + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: # len(self.data_dict.keys()) == 0: + return + + self.param = [] + + for scs_id in self.data_dict: + self.param.append(scs_id) + + def addParam(self, id): + if id in self.data_dict: # scs_id already exist + return False + + self.data_dict[id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, id): + if id not in self.data_dict: # NOT exist + return + + del self.data_dict[id] + + self.is_param_changed = True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.handler.syncReadTx(self.start_address, + self.data_length, + self.param, + len(self.data_dict.keys())) + + def rxPacket(self): + self.last_result = True + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + result, rxpacket = self.handler.syncReadRx(self.data_length, len(self.data_dict.keys())) + + if len(rxpacket) >= (self.data_length + 6): + for id in self.data_dict: + self.data_dict[id], result = self.readRx(rxpacket, id, self.data_length) + if result != COMM_SUCCESS: + self.last_result = False + + else: + self.last_result = False + + return result + + def txRxPacket(self): + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def readRx(self, rxpacket, id, data_length): + data = [] + rx_length = len(rxpacket) + + rx_index = 0 + while (rx_index + 6 + data_length) <= rx_length: + headpacket = [0x00, 0x00, 0x00] + while rx_index < rx_length: + headpacket[2] = headpacket[1] + headpacket[1] = headpacket[0] + headpacket[0] = rxpacket[rx_index] + rx_index += 1 + if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == id: + + break + + if (rx_index+3+data_length) > rx_length: + break + if rxpacket[rx_index] != (data_length+2): + rx_index += 1 + + continue + rx_index += 1 + Error = rxpacket[rx_index] + rx_index += 1 + calSum = id + (data_length+2) + Error + data = [Error] + data.extend(rxpacket[rx_index : rx_index+data_length]) + for i in range(0, data_length): + calSum += rxpacket[rx_index] + rx_index += 1 + calSum = ~calSum & 0xFF + + if calSum != rxpacket[rx_index]: + return None, COMM_RX_CORRUPT + return data, COMM_SUCCESS + + return None, COMM_RX_CORRUPT + + def isAvailable(self, id, address, data_length): + #if self.last_result is False or scs_id not in self.data_dict: + if id not in self.data_dict: + return False, 0 + + if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): + return False, 0 + + if not self.data_dict[id]: + return False, 0 + + if len(self.data_dict[id])<(data_length+1): + return False, 0 + + return True, self.data_dict[id][0] + + def getData(self, id, address, data_length): + if data_length == 1: + return self.data_dict[id][address-self.start_address+1] + elif data_length == 2: + return self.handler.makeWord16(self.data_dict[id][address-self.start_address+1], + self.data_dict[id][address-self.start_address+2]) + elif data_length == 4: + return self.handler.makeWord32(self.handler.makeWord16(self.data_dict[id][address-self.start_address+1], + self.data_dict[id][address-self.start_address+2]), + self.handler.makeWord16(self.data_dict[id][address-self.start_address+3], + self.data_dict[id][address-self.start_address+4])) + else: + return 0 + +class GroupSyncWrite: + def __init__(self, handler, start_address, data_length): + self.handler = handler + self.start_address = start_address + self.data_length = data_length + + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: + return + + self.param = [] + + for id in self.data_dict: + if not self.data_dict[id]: + return + + self.param.append(id) + self.param.extend(self.data_dict[id]) + + def addParam(self, id, data): + if id in self.data_dict: # id already exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[id] = data + + self.is_param_changed = True + return True + + def removeParam(self, id): + if id not in self.data_dict: # NOT exist + return + + del self.data_dict[id] + + self.is_param_changed = True + + def changeParam(self, id, data): + if id not in self.data_dict: # NOT exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[id] = data + + self.is_param_changed = True + return True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + # print(self.data_dict) + return self.handler.syncWriteTxOnly(self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * (1 + self.data_length)) \ No newline at end of file diff --git a/servo/servo_test.py b/servo/servo_test.py new file mode 100644 index 0000000..6d14203 --- /dev/null +++ b/servo/servo_test.py @@ -0,0 +1,195 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2026/1/5 17:01 +# @Author : reenrr +# @File : servo_test.py +# @Desc : 测试舵机 +''' +#!/usr/bin/env python +# -*- coding: utf-8 -*- +''' +# @Time : 2025/12/2 14:05 +# @Author : reenrr +# @File :servo.py +# @Description : 控制舵机正反转 +''' + +from servo_sdk import * +import logging + +# -------------参数配置-------------- +BAUDRATE = 115200 # 舵机的波特率 +PORT = 'COM4' +SERVO_IDS = [1, 2, 3, 4, 5] # 舵机们的 ID 号 +POS_START = 2047 +POS_END = 0 +SPEED = 1500 +ACC = 0 +TIME_INTERVAL1 = ((2047-0) / 1500) + 2 # 翻转回来的时间 +TIME_INTERVAL2 = 10 # 不用翻转运行的时间 + +class ServoController: + def __init__(self, config=None): + """ + 初始化舵机控制器 + """ + self.config = config + self.time_interval1 = self.config['time_interval1'] + self.time_interval2 = self.config['time_interval2'] + + # 初始化串口和舵机处理器 + self.port_handler = PortHandler(self.config['port']) + self.servo_handler = HxServoHandler(self.port_handler) + + # 初始化状态 + self.is_running = False + + def setup_port(self): + """配置串口参数""" + # 打开串口 + if self.port_handler.openPort(): + logging.info("成功打开串口") + else: + raise RuntimeError("打开串口失败") + + # 设置波特率 + if self.port_handler.setBaudRate(self.config['baudrate']): + logging.info("设置波特率成功") + else: + self.port_handler.closePort() + raise RuntimeError("设置波特率失败") + + def enable_all_servos(self): + """使能所有舵机""" + enable_success = True + for servo_id in self.config['servo_ids']: + enable_result = self.servo_handler.torqueEnable(servo_id) + if enable_result[1] != 0: + logging.info(f"[ID:{servo_id:02d}] 舵机使能失败，错误信息：{enable_result[1]}") + enable_success = False + else: + logging.info(f"[ID:{servo_id:02d}] 舵机使能成功") + + if not enable_success: + self.cleanup() + raise RuntimeError("使能舵机失败,程序退出") + + logging.info("所有舵机使能成功") + + def disable_all_servos(self): + """失能所有舵机""" + disable_success = True + for servo_id in self.config['servo_ids']: + disable_result = self.servo_handler.torqueDisable(servo_id) + if disable_result[1] != 0: + logging.info(f"[ID:{servo_id:02d}] 舵机失能失败，错误信息：{disable_result[1]}") + disable_success = False + else: + logging.info(f"[ID:{servo_id:02d}] 舵机失能成功") + + if not disable_success: + self.cleanup() + raise RuntimeError("失能舵机失败,程序退出") + + logging.info("所有舵机失能成功") + + def write_position(self, position, speed, acc): + """ + 写入目标位置到所有舵机 + :param position: 目标位置 + :param speed: 速度 + :param acc: 加速度 + """ + for servo_id in self.config['servo_ids']: + add_param_result = self.servo_handler.syncWritePosEx( + servo_id, position, speed, acc + ) + if not add_param_result: + logging.info(f"[ID:{servo_id:02d}] 添加参数失败") + continue + + result = self.servo_handler.GroupSyncWrite.txPacket() + if result != COMM_SUCCESS: + logging.info(f"[ID:{servo_id:02d}] 发送指令失败：{result.getTxRxResult(result)}") + logging.info("复位成功") + + # 清空参数缓存 + self.servo_handler.GroupSyncWrite.clearParam() + + def stop(self): + """停止舵机运行""" + self.is_running = False + + def cleanup(self): + """清理资源""" + self.disable_all_servos() + if self.port_handler.is_open(): + self.port_handler.closePort() + logging.info("串口已关闭") + + def run_cycle(self): + """运行舵机循环运动""" + self.is_running = True + logging.info("舵机开始循环运动（按Ctrl+C终止）...") + + while self.is_running: + # 运动到起始位置（180度） + self.write_position(self.config['pos_start'], + self.config['speed'], + self.config['acc']) + logging.info("运动到180度") + time.sleep(self.time_interval1) + + # 运动到结束位置（0度） + self.write_position(self.config['pos_end'], + self.config['speed'], + self.config['acc']) + logging.info("运动到0度") + time.sleep(self.time_interval2) + + def run(self): + """循环运行：复位位置之后，先转到180度，延时TIME_INTERVAL1一段时间，再转到0度""" + try: + # 初始化串口 + self.setup_port() + # 使能所有舵机 + self.enable_all_servos() + # 复位位置 + self.write_position(self.config['pos_end'], + self.config['speed'], + self.config['acc']) + # 运行循环 + self.run_cycle() + except KeyboardInterrupt: + logging.info("\n用户终止程序...") + self.stop() + except RuntimeError as e: + logging.info(f"\n运行错误：{str(e)}") + except Exception as e: + logging.info(f"\n程序运行异常：{str(e)}") + finally: + self.cleanup() + +# ---------舵机对外接口---------- +def servo_rotate(): + custom_config = { + 'port': PORT, + 'baudrate': BAUDRATE, + 'servo_ids': SERVO_IDS, + 'pos_start': POS_START, + 'pos_end': POS_END, + 'speed': SPEED, + 'acc': ACC, + 'time_interval1': TIME_INTERVAL1, + 'time_interval2': TIME_INTERVAL2 + } + + controller = ServoController(custom_config) + + controller.run() + + +if __name__ == '__main__': + servo_rotate() +