Last active
June 12, 2026 23:13
-
-
Save nro-bot/585683b26c474a21695c2ec5fb3c01bd to your computer and use it in GitHub Desktop.
steadywin_motor_test.py
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import can | |
| import time | |
| import struct | |
| import math | |
| RAD_TO_SIGNAL = 2607.5946 | |
| # 定义电机命令 | |
| CMD_PING = 0x01 | |
| CMD_READ_STATUS = 0x02 # “02查询” | |
| # 寄存器地址常量 | |
| PRESENT_POSITION = 0x228 # 当前位置寄存器地址552 = 0x228 | |
| GOAL_POSITION = 0x214 # 目标位置寄存器地址532 = 0x214 | |
| TORQUE_ON_OFF_REGISTER = 0x200 # 扭矩开关寄存器地址512 = 0x200 | |
| OPENTING_MODEL_REGISTER = 0x21 # Operating Mode寄存器地址33 = 0x21 | |
| SET_ZERO_POSITION_REGISTER = 0x34 # 设置零位寄存器52= 0x34 | |
| SET_ZERO_POSITION_VALUE = 0x01 # 设置零位的值 | |
| TORQUE_ON_VALUE = 0x01 # 打开扭矩的值 | |
| TORQUE_OFF_VALUE = 0x00 # 关闭扭矩的值 | |
| # 设置 OPENTING_MODEL 的值 (0：电流模式；1：速度模式；3:位置模式；6：MIT模式 | |
| OPENTING_MODEL_VALUE = 0x03 | |
| # MIT 控制参数的映射范围（示例，请根据实际文档调整） | |
| P_DES_MIN = -12.5 | |
| P_DES_MAX = 12.5 | |
| V_DES_MIN = -65.0 | |
| V_DES_MAX = 65.0 | |
| T_FF_MIN = -18.0 | |
| T_FF_MAX = 18.0 | |
| KP_MIN = 0.0 | |
| KP_MAX = 500.0 | |
| KD_MIN = 0.0 | |
| KD_MAX = 5.0 | |
| # 位置/速度/扭矩的实际浮点数范围 (用于 uint_to_float 转换) | |
| # 这些需要根据电机的实际规格来定义 | |
| POS_MIN_RANGE = -12.5 * math.pi # 假设是弧度，根据电机文档调整 | |
| POS_MAX_RANGE = 12.5 * math.pi | |
| VEL_MIN_RANGE = -65.0 * math.pi # 假设是弧度/秒 | |
| VEL_MAX_RANGE = 65.0 * math.pi | |
| TORQUE_MIN_RANGE = -18.0 | |
| TORQUE_MAX_RANGE = 18.0 | |
| # 将浮点数映射到定点数的函数 | |
| def float_to_uint(value, min_val, max_val, num_bits): | |
| if value < min_val: | |
| value = min_val | |
| if value > max_val: | |
| value = max_val | |
| scaled_value = (value - min_val) / (max_val - min_val) | |
| return int(scaled_value * ((1 << num_bits) - 1)) | |
| # 将定点数映射回浮点数的函数 | |
| def uint_to_float(value, min_val, max_val, num_bits): | |
| scaled_value = value / ((1 << num_bits) - 1) | |
| return scaled_value * (max_val - min_val) + min_val | |
| # 对于带符号的定点数，需要做额外的处理 | |
| def signed_uint_to_float(value, min_val, max_val, num_bits): | |
| if value & (1 << (num_bits - 1)): | |
| value = value - (1 << num_bits) | |
| range_span = max_val - min_val | |
| integer_span = (1 << num_bits) - 1 # 注意：对于带符号数，范围可能不是 (1<<num_bits)-1，而是 (1<<(num_bits-1))-1 到 -(1<<(num_bits-1)) | |
| return min_val + (value / integer_span) * range_span | |
| class MotorSLCan: | |
| def __init__(self, port, baudrate=1000000): | |
| self.port = port | |
| self.baudrate = baudrate | |
| self.bus = None | |
| def connect(self): | |
| try: | |
| self.bus = can.interface.Bus(interface='socketcan', | |
| channel=self.port, | |
| ) | |
| # bitrate=self.baudrate) | |
| print(f"Connected to SLCan on {self.port} with bitrate {self.baudrate}") | |
| return True | |
| except Exception as e: | |
| print(f"Error connecting to SLCan: {e}") | |
| return False | |
| def disconnect(self): | |
| if self.bus: | |
| self.bus.shutdown() | |
| print("Disconnected from SLCan.") | |
| def send_command(self, arbitration_id, data, is_extended_id=False): | |
| try: | |
| message = can.Message( | |
| arbitration_id=arbitration_id, | |
| is_extended_id=is_extended_id, | |
| data=data | |
| ) | |
| self.bus.send(message) | |
| return True | |
| except Exception as e: | |
| print(f"Error sending CAN message: {e}") | |
| return False | |
| def receive_response(self, timeout=1.0): | |
| try: | |
| message = self.bus.recv(timeout) | |
| if message: | |
| return message | |
| else: | |
| return None | |
| except Exception as e: | |
| print(f"Error receiving CAN message: {e}") | |
| return None | |
| def ping_device(self, can_id): | |
| self.send_command(can_id, bytearray([CMD_PING])) | |
| response = self.receive_response(timeout=0.2) # PING 响应可能需要稍长一些 | |
| if response and len(response.data) == 8: | |
| _485_id = response.data[0] | |
| can_id_response = response.data[1] | |
| model_number = struct.unpack('<H', response.data[2:4])[0] | |
| major_fw = response.data[4] | |
| minor_fw = response.data[5] | |
| patch_fw = response.data[6] | |
| drive_mode = response.data[7] | |
| print(f"--- PING Response (ID: {hex(can_id)}) ---") | |
| print( | |
| f" 485 ID: {hex(_485_id)}, CAN ID: {hex(can_id_response)}, Model: {model_number}, FW: {major_fw}.{minor_fw}.{patch_fw}") | |
| return { | |
| "485_ID": _485_id, "CAN_ID": can_id_response, "MODEL_NUMBER": model_number, | |
| "MAJOR_FIRMWARE_VERSION": major_fw, "MINOR_FIRMWARE_VERSION": minor_fw, | |
| "PATCH_FIRMWARE_VERSION": patch_fw, "DRIVE_MODE": drive_mode | |
| } | |
| else: | |
| print(f"PING failed or invalid response for CAN ID {hex(can_id)}.") | |
| return None | |
| def read_device_status(self, can_id): | |
| self.send_command(can_id, bytearray([CMD_READ_STATUS])) | |
| response = self.receive_response(timeout=0.05) # 状态查询响应时间可以短一些 | |
| if response and len(response.data) == 8: | |
| operator_mode = response.data[0] | |
| exencoder_bits = response.data[1] | |
| torque = response.data[2] | |
| input_voltage = struct.unpack('<H', response.data[3:5])[0] | |
| error_code = response.data[5] | |
| t_mos = response.data[6] | |
| t_rotor = response.data[7] | |
| # print(f"--- Device Status (ID: {hex(can_id)}) ---") | |
| # print(f" Mode: {operator_mode}, Voltage: {input_voltage / 100.0:.2f}V, Error: {hex(error_code)}, T_MOS: {t_mos}, T_Rotor: {t_rotor}") | |
| return { | |
| "OperatorMode": operator_mode, "ExencoderBits": exencoder_bits, "Torque": torque, | |
| "INPUT_VOLTAGE": input_voltage, "INPUT_VOLTAGE_V": input_voltage / 100.0, | |
| "ErrorCode": error_code, "T_MOS": t_mos, "T_Rotor": t_rotor | |
| } | |
| else: | |
| # print(f"Read device status failed or invalid response for CAN ID {hex(can_id)}.") | |
| return None | |
| def read_register(self, can_id, address, length): | |
| if not (1 <= length <= 8): | |
| print(f"Error: Register read length must be between 1 and 8. (CAN ID: {hex(can_id)})") | |
| return None | |
| data = bytearray([ | |
| (address & 0xFF), | |
| ((address >> 8) & 0xFF), | |
| length | |
| ]) | |
| self.send_command(can_id, data) | |
| response = self.receive_response(timeout=0.05) # 寄存器读取响应时间可以短一些 | |
| if response and len(response.data) == length: | |
| # print(f"--- Read Register (ID: {hex(can_id)}, Addr: {hex(address)}, Len: {length}) --- Data: {response.data.hex()}") | |
| return response.data | |
| else: | |
| # print(f"Read register failed or invalid response for CAN ID {hex(can_id)}, Address {hex(address)}.") | |
| # if response: | |
| # print(f" Received data length: {len(response.data)}, Expected: {length}") | |
| # print(f" Received data: {response.data.hex()}") | |
| return None | |
| def write_register(self, can_id, address, value_bytes): | |
| if not (1 <= len(value_bytes) <= 4): | |
| print( | |
| f"Error: Register write value length must be between 1 and 4 bytes. Got {len(value_bytes)} bytes. (CAN ID: {hex(can_id)})") | |
| return False | |
| padded_value_bytes = bytearray(value_bytes) | |
| while len(padded_value_bytes) < 4: | |
| padded_value_bytes.append(0xFF) # 文档说是 FF 填充，但实际中 00 更常见，请核实 | |
| data = bytearray([ | |
| (address & 0xFF), | |
| ((address >> 8) & 0xFF), | |
| len(value_bytes) | |
| ]) | |
| data.extend(padded_value_bytes) | |
| self.send_command(can_id, data) | |
| response = self.receive_response(timeout=0.05) # 写寄存器响应时间可以短一些 | |
| if response and len(response.data) >= 1: | |
| status = response.data[0] | |
| if status == 0x01: | |
| # print(f"Write register (ID: {hex(can_id)}, Addr: {hex(address)}, Val: {value_bytes.hex()}) successful (Status: {hex(status)}).") | |
| return True | |
| else: | |
| print( | |
| f"Write register (ID: {hex(can_id)}, Addr: {hex(address)}, Val: {value_bytes.hex()}) failed (Status: {hex(status)}).") | |
| return False | |
| else: | |
| print(f"Write register failed or invalid response for CAN ID {hex(can_id)}, Address {hex(address)}.") | |
| if response: | |
| print(f" Received response data: {response.data.hex()}") | |
| return False | |
| def open_torque(self, can_id): | |
| return self.write_register(can_id, TORQUE_ON_OFF_REGISTER, bytearray([TORQUE_ON_VALUE])) | |
| def close_torque(self, can_id): | |
| return self.write_register(can_id, TORQUE_ON_OFF_REGISTER, bytearray([TORQUE_OFF_VALUE])) | |
| def set_zero_position(self, can_id): | |
| return self.write_register(can_id, SET_ZERO_POSITION_REGISTER, bytearray([SET_ZERO_POSITION_VALUE])) | |
| def set_openting_model(self, can_id): | |
| return self.write_register(can_id, OPENTING_MODEL_REGISTER, bytearray([OPENTING_MODEL_VALUE])) | |
| def send_mit_control(self, can_id, p_des, v_des, kp, kd, t_ff): | |
| # 暂时不修改这部分，因为它不属于你当前的主要需求，但保留 | |
| p_des_int = float_to_uint(p_des, P_DES_MIN, P_DES_MAX, 16) | |
| v_des_int = float_to_uint(v_des, V_DES_MIN, V_DES_MAX, 12) | |
| kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12) | |
| kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12) | |
| t_ff_int = float_to_uint(t_ff, T_FF_MIN, T_FF_MAX, 12) | |
| data = bytearray(8) | |
| data[0] = (p_des_int >> 8) & 0xFF | |
| data[1] = p_des_int & 0xFF | |
| data[2] = (v_des_int >> 4) & 0xFF | |
| data[3] = ((v_des_int & 0x0F) << 4) | ((kp_int >> 8) & 0x0F) | |
| data[4] = kp_int & 0xFF | |
| data[5] = (kd_int >> 4) & 0xFF | |
| data[6] = ((kd_int & 0x0F) << 4) | ((t_ff_int >> 8) & 0x0F) | |
| data[7] = t_ff_int & 0xFF | |
| self.send_command(can_id, data) | |
| response = self.receive_response(timeout=0.05) | |
| if response and len(response.data) == 8: | |
| return self._parse_mit_response(response.data) | |
| else: | |
| print(f"MIT control failed or invalid response for CAN ID {hex(can_id)}.") | |
| return None | |
| def _parse_mit_response(self, data): | |
| # 暂时不修改这部分，因为它不属于你当前的主要需求，但保留 | |
| motor_id = data[0] & 0x0F | |
| err_code = (data[0] >> 4) & 0x0F | |
| pos_raw = (data[1] << 8) | data[2] | |
| vel_raw = ((data[3] << 4) | ((data[4] >> 4) & 0x0F)) | |
| torque_raw = (((data[4] & 0x0F) << 8) | data[5]) | |
| t_mos = data[6] | |
| t_rotor = data[7] | |
| # print(f"--- MIT/Torque Response (ID: {hex(motor_id)}) --- Err: {err_code}, Pos: {pos_raw}, Vel: {vel_raw}, T: {torque_raw}") | |
| return { | |
| "ID": motor_id, "ERR": err_code, "POS_RAW": pos_raw, | |
| "VEL_RAW": vel_raw, "T_RAW": torque_raw, "T_MOS": t_mos, "T_Rotor": t_rotor | |
| } | |
| if __name__ == "__main__": | |
| # SLAN_PORT = 'ttyACM0'/ | |
| SLAN_PORT = 'can0' | |
| # 四个驱动器的 CAN ID | |
| MOTOR_CAN_IDS = [ 0x01]#, 0x02,0x3, 0x04, 0x05,0x06] | |
| motor_comm = MotorSLCan(SLAN_PORT) | |
| if motor_comm.connect(): | |
| try: | |
| # 存储每个电机的最后操作时间，用于调度 | |
| last_status_query_time = {can_id: 0 for can_id in MOTOR_CAN_IDS} | |
| last_pos_query_time = {can_id: 0 for can_id in MOTOR_CAN_IDS} | |
| last_goal_update_time = {can_id: 0 for can_id in MOTOR_CAN_IDS} | |
| # 目标位置生成器的状态 | |
| current_goal_position_rad = {can_id: 0.0 for can_id in MOTOR_CAN_IDS} | |
| goal_position_direction = {can_id: 1 for can_id in MOTOR_CAN_IDS} # 1 for increasing, -1 for decreasing | |
| MAX_GOAL_POS_RAD = 1.0 * math.pi # 180 度，可根据需要调整 | |
| MIN_GOAL_POS_RAD = -1.0 * math.pi # -180 度 | |
| GOAL_POS_STEP_RAD = 0.001 * math.pi # 每次更新的步长 | |
| print("\n--- INITIALIZING MOTORS ---") | |
| for motor_id in MOTOR_CAN_IDS: | |
| print(f"\nInitializing Motor {hex(motor_id)}:") | |
| # 1. PING | |
| print(f" Pinging...") | |
| motor_info = motor_comm.ping_device(motor_id) | |
| if not motor_info: | |
| print(f" Failed to PING motor {hex(motor_id)}. Skipping initialization.") | |
| continue # 如果PING失败，跳过这个电机的初始化 | |
| # 2. 设置 OPENTING_MODEL | |
| print(f" Setting OPENTING_MODEL...") | |
| if not motor_comm.set_openting_model(motor_id): | |
| print(f" Failed to set OPENTING_MODEL for motor {hex(motor_id)}.") | |
| continue | |
| time.sleep(0.1) | |
| # 3. 打开扭矩开关 | |
| print(f" Opening torque...") | |
| if not motor_comm.open_torque(motor_id): | |
| print(f" Failed to open torque for motor {hex(motor_id)}.") | |
| continue | |
| print("\n--- STARTING CONTROL LOOP ---") | |
| # 主循环频率控制 | |
| LOOP_FREQUENCY = 1000 # Hz | |
| LOOP_PERIOD = 1.0 / LOOP_FREQUENCY # seconds | |
| # 指令发送频率 | |
| STATUS_QUERY_PERIOD = 0.100 # 100 ms | |
| POS_QUERY_PERIOD = 0.010 # 10 ms | |
| GOAL_UPDATE_PERIOD = 0.010 # 10 ms | |
| start_time = time.monotonic() | |
| loop_count = 0 | |
| while True: | |
| loop_start_time = time.monotonic() | |
| for motor_id in MOTOR_CAN_IDS: | |
| current_time = time.monotonic() | |
| # ① 0x02 查询 (100ms) | |
| if current_time - last_status_query_time[motor_id] >= STATUS_QUERY_PERIOD: | |
| status = motor_comm.read_device_status(motor_id) | |
| #if status: | |
| # print( | |
| # f"Loop {loop_count}: Motor {hex(motor_id)} Status: Mode={status['OperatorMode']}, Volt={status['INPUT_VOLTAGE_V']:.2f}V, Temp={status['T_MOS']}C/{status['T_Rotor']}C") | |
| last_status_query_time[motor_id] = current_time | |
| # ② 位置查询 (10ms) | |
| if current_time - last_pos_query_time[motor_id] >= POS_QUERY_PERIOD: | |
| # 假设 PRESENT_POSITION 寄存器返回 4 字节（32位）表示位置 | |
| pos_raw_bytes = motor_comm.read_register(motor_id, PRESENT_POSITION, 4) | |
| if pos_raw_bytes and len(pos_raw_bytes) == 4: | |
| pos_raw = struct.unpack('<i', pos_raw_bytes)[0] # 假设无符号16位小端 | |
| # 将原始整数位置转换为浮点数 (需要根据你的电机文档提供正确的映射范围和位数) | |
| # 这里我假设 PRESENT_POSITION 也是 16 位，范围与 MIT 的 P_DES 类似 | |
| #actual_pos_rad = uint_to_float(pos_raw, POS_MIN_RANGE, POS_MAX_RANGE, 16) | |
| #print( | |
| # f"Loop {loop_count}: Motor {hex(motor_id)} Pos: {pos_raw} ") | |
| last_pos_query_time[motor_id] = current_time | |
| # ③ 更新目标位置 (10ms) | |
| if current_time - last_goal_update_time[motor_id] >= GOAL_UPDATE_PERIOD: | |
| # 简单的目标位置生成：在 MIN_GOAL_POS_RAD 和 MAX_GOAL_POS_RAD 之间往复运动 | |
| if goal_position_direction[motor_id] == 1: | |
| current_goal_position_rad[motor_id] += GOAL_POS_STEP_RAD | |
| if current_goal_position_rad[motor_id] >= MAX_GOAL_POS_RAD: | |
| current_goal_position_rad[motor_id] = MAX_GOAL_POS_RAD | |
| goal_position_direction[motor_id] = -1 | |
| else: | |
| current_goal_position_rad[motor_id] -= GOAL_POS_STEP_RAD | |
| if current_goal_position_rad[motor_id] <= MIN_GOAL_POS_RAD: | |
| current_goal_position_rad[motor_id] = MIN_GOAL_POS_RAD | |
| goal_position_direction[motor_id] = 1 | |
| # 将浮点目标位置转换为电机期望的整数格式（假设是16位，与P_DES_MIN/MAX类似） | |
| #goal_pos_int = float_to_uint(current_goal_position_rad[motor_id], POS_MIN_RANGE, POS_MAX_RANGE, 16) | |
| goal_pos_int = int(RAD_TO_SIGNAL * current_goal_position_rad[motor_id]) | |
| goal_bytes = struct.pack('<i', goal_pos_int) # 假设有符号的4字节小端 | |
| # 写入 GOAL_POSITION 寄存器 | |
| motor_comm.write_register(motor_id, GOAL_POSITION, goal_bytes) | |
| #print(f"Loop {loop_count}: Motor {hex(motor_id)} Set Goal: {current_goal_position_rad[motor_id]:.4f} rad (raw: {goal_pos_int})") | |
| last_goal_update_time[motor_id] = current_time | |
| loop_count += 1 | |
| # 确保主循环以 3000Hz 运行 | |
| loop_end_time = time.monotonic() | |
| time_elapsed = loop_end_time - loop_start_time | |
| time_to_sleep = LOOP_PERIOD - time_elapsed | |
| if time_to_sleep > 0: | |
| time.sleep(time_to_sleep) | |
| else: | |
| # 如果循环执行时间超过了周期，打印警告 | |
| #print(f"Warning: Loop {loop_count} took {time_elapsed*1000:.2f} ms, exceeding {LOOP_PERIOD*1000:.2f} ms.") | |
| pass | |
| except KeyboardInterrupt: | |
| print("\nExiting program, closing torque for all motors.") | |
| import can | |
| import time | |
| import struct | |
| import math | |
| RAD_TO_SIGNAL = 2607.5946 | |
| # 定义电机命令 | |
| CMD_PING = 0x01 | |
| CMD_READ_STATUS = 0x02 # “02查询” | |
| # 寄存器地址常量 | |
| PRESENT_POSITION = 0x228 # 当前位置寄存器地址552 = 0x228 | |
| GOAL_POSITION = 0x214 # 目标位置寄存器地址532 = 0x214 | |
| TORQUE_ON_OFF_REGISTER = 0x200 # 扭矩开关寄存器地址512 = 0x200 | |
| OPENTING_MODEL_REGISTER = 0x21 # Operating Mode寄存器地址33 = 0x21 | |
| SET_ZERO_POSITION_REGISTER = 0x34 # 设置零位寄存器52= 0x34 | |
| SET_ZERO_POSITION_VALUE = 0x01 # 设置零位的值 | |
| TORQUE_ON_VALUE = 0x01 # 打开扭矩的值 | |
| TORQUE_OFF_VALUE = 0x00 # 关闭扭矩的值 | |
| # 设置 OPENTING_MODEL 的值 (0：电流模式；1：速度模式；3:位置模式；6：MIT模式 | |
| OPENTING_MODEL_VALUE = 0x03 | |
| # MIT 控制参数的映射范围（示例，请根据实际文档调整） | |
| P_DES_MIN = -12.5 | |
| P_DES_MAX = 12.5 | |
| V_DES_MIN = -65.0 | |
| V_DES_MAX = 65.0 | |
| T_FF_MIN = -18.0 | |
| T_FF_MAX = 18.0 | |
| KP_MIN = 0.0 | |
| KP_MAX = 500.0 | |
| KD_MIN = 0.0 | |
| KD_MAX = 5.0 | |
| # 位置/速度/扭矩的实际浮点数范围 (用于 uint_to_float 转换) | |
| # 这些需要根据电机的实际规格来定义 | |
| POS_MIN_RANGE = -12.5 * math.pi # 假设是弧度，根据电机文档调整 | |
| POS_MAX_RANGE = 12.5 * math.pi | |
| VEL_MIN_RANGE = -65.0 * math.pi # 假设是弧度/秒 | |
| VEL_MAX_RANGE = 65.0 * math.pi | |
| TORQUE_MIN_RANGE = -18.0 | |
| TORQUE_MAX_RANGE = 18.0 | |
| # 将浮点数映射到定点数的函数 | |
| def float_to_uint(value, min_val, max_val, num_bits): | |
| if value < min_val: | |
| value = min_val | |
| if value > max_val: | |
| value = max_val | |
| scaled_value = (value - min_val) / (max_val - min_val) | |
| return int(scaled_value * ((1 << num_bits) - 1)) | |
| # 将定点数映射回浮点数的函数 | |
| def uint_to_float(value, min_val, max_val, num_bits): | |
| scaled_value = value / ((1 << num_bits) - 1) | |
| return scaled_value * (max_val - min_val) + min_val | |
| # 对于带符号的定点数，需要做额外的处理 | |
| def signed_uint_to_float(value, min_val, max_val, num_bits): | |
| if value & (1 << (num_bits - 1)): | |
| value = value - (1 << num_bits) | |
| range_span = max_val - min_val | |
| integer_span = (1 << num_bits) - 1 # 注意：对于带符号数，范围可能不是 (1<<num_bits)-1，而是 (1<<(num_bits-1))-1 到 -(1<<(num_bits-1)) | |
| return min_val + (value / integer_span) * range_span | |
| class MotorSLCan: | |
| def __init__(self, port, baudrate=1000000): | |
| self.port = port | |
| self.baudrate = baudrate | |
| self.bus = None | |
| def connect(self): | |
| try: | |
| self.bus = can.interface.Bus(interface='socketcan', | |
| channel=self.port, | |
| ) | |
| # bitrate=self.baudrate) | |
| print(f"Connected to SLCan on {self.port} with bitrate {self.baudrate}") | |
| return True | |
| except Exception as e: | |
| print(f"Error connecting to SLCan: {e}") | |
| return False | |
| def disconnect(self): | |
| if self.bus: | |
| self.bus.shutdown() | |
| print("Disconnected from SLCan.") | |
| def send_command(self, arbitration_id, data, is_extended_id=False): | |
| try: | |
| message = can.Message( | |
| arbitration_id=arbitration_id, | |
| is_extended_id=is_extended_id, | |
| data=data | |
| ) | |
| self.bus.send(message) | |
| return True | |
| except Exception as e: | |
| print(f"Error sending CAN message: {e}") | |
| return False | |
| def receive_response(self, timeout=1.0): | |
| try: | |
| message = self.bus.recv(timeout) | |
| if message: | |
| return message | |
| else: | |
| return None | |
| except Exception as e: | |
| print(f"Error receiving CAN message: {e}") | |
| return None | |
| def ping_device(self, can_id): | |
| self.send_command(can_id, bytearray([CMD_PING])) | |
| response = self.receive_response(timeout=0.2) # PING 响应可能需要稍长一些 | |
| if response and len(response.data) == 8: | |
| _485_id = response.data[0] | |
| can_id_response = response.data[1] | |
| model_number = struct.unpack('<H', response.data[2:4])[0] | |
| major_fw = response.data[4] | |
| minor_fw = response.data[5] | |
| patch_fw = response.data[6] | |
| drive_mode = response.data[7] | |
| print(f"--- PING Response (ID: {hex(can_id)}) ---") | |
| print( | |
| f" 485 ID: {hex(_485_id)}, CAN ID: {hex(can_id_response)}, Model: {model_number}, FW: {major_fw}.{minor_fw}.{patch_fw}") | |
| return { | |
| "485_ID": _485_id, "CAN_ID": can_id_response, "MODEL_NUMBER": model_number, | |
| "MAJOR_FIRMWARE_VERSION": major_fw, "MINOR_FIRMWARE_VERSION": minor_fw, | |
| "PATCH_FIRMWARE_VERSION": patch_fw, "DRIVE_MODE": drive_mode | |
| } | |
| else: | |
| print(f"PING failed or invalid response for CAN ID {hex(can_id)}.") | |
| return None | |
| def read_device_status(self, can_id): | |
| self.send_command(can_id, bytearray([CMD_READ_STATUS])) | |
| response = self.receive_response(timeout=0.05) # 状态查询响应时间可以短一些 | |
| if response and len(response.data) == 8: | |
| operator_mode = response.data[0] | |
| exencoder_bits = response.data[1] | |
| torque = response.data[2] | |
| input_voltage = struct.unpack('<H', response.data[3:5])[0] | |
| error_code = response.data[5] | |
| t_mos = response.data[6] | |
| t_rotor = response.data[7] | |
| # print(f"--- Device Status (ID: {hex(can_id)}) ---") | |
| # print(f" Mode: {operator_mode}, Voltage: {input_voltage / 100.0:.2f}V, Error: {hex(error_code)}, T_MOS: {t_mos}, T_Rotor: {t_rotor}") | |
| return { | |
| "OperatorMode": operator_mode, "ExencoderBits": exencoder_bits, "Torque": torque, | |
| "INPUT_VOLTAGE": input_voltage, "INPUT_VOLTAGE_V": input_voltage / 100.0, | |
| "ErrorCode": error_code, "T_MOS": t_mos, "T_Rotor": t_rotor | |
| } | |
| else: | |
| # print(f"Read device status failed or invalid response for CAN ID {hex(can_id)}.") | |
| return None | |
| def read_register(self, can_id, address, length): | |
| if not (1 <= length <= 8): | |
| print(f"Error: Register read length must be between 1 and 8. (CAN ID: {hex(can_id)})") | |
| return None | |
| data = bytearray([ | |
| (address & 0xFF), | |
| ((address >> 8) & 0xFF), | |
| length | |
| ]) | |
| self.send_command(can_id, data) | |
| response = self.receive_response(timeout=0.05) # 寄存器读取响应时间可以短一些 | |
| if response and len(response.data) == length: | |
| # print(f"--- Read Register (ID: {hex(can_id)}, Addr: {hex(address)}, Len: {length}) --- Data: {response.data.hex()}") | |
| return response.data | |
| else: | |
| # print(f"Read register failed or invalid response for CAN ID {hex(can_id)}, Address {hex(address)}.") | |
| # if response: | |
| # print(f" Received data length: {len(response.data)}, Expected: {length}") | |
| # print(f" Received data: {response.data.hex()}") | |
| return None | |
| def write_register(self, can_id, address, value_bytes): | |
| if not (1 <= len(value_bytes) <= 4): | |
| print( | |
| f"Error: Register write value length must be between 1 and 4 bytes. Got {len(value_bytes)} bytes. (CAN ID: {hex(can_id)})") | |
| return False | |
| padded_value_bytes = bytearray(value_bytes) | |
| while len(padded_value_bytes) < 4: | |
| padded_value_bytes.append(0xFF) # 文档说是 FF 填充，但实际中 00 更常见，请核实 | |
| data = bytearray([ | |
| (address & 0xFF), | |
| ((address >> 8) & 0xFF), | |
| len(value_bytes) | |
| ]) | |
| data.extend(padded_value_bytes) | |
| self.send_command(can_id, data) | |
| response = self.receive_response(timeout=0.05) # 写寄存器响应时间可以短一些 | |
| if response and len(response.data) >= 1: | |
| status = response.data[0] | |
| if status == 0x01: | |
| # print(f"Write register (ID: {hex(can_id)}, Addr: {hex(address)}, Val: {value_bytes.hex()}) successful (Status: {hex(status)}).") | |
| return True | |
| else: | |
| print( | |
| f"Write register (ID: {hex(can_id)}, Addr: {hex(address)}, Val: {value_bytes.hex()}) failed (Status: {hex(status)}).") | |
| return False | |
| else: | |
| print(f"Write register failed or invalid response for CAN ID {hex(can_id)}, Address {hex(address)}.") | |
| if response: | |
| print(f" Received response data: {response.data.hex()}") | |
| return False | |
| def open_torque(self, can_id): | |
| return self.write_register(can_id, TORQUE_ON_OFF_REGISTER, bytearray([TORQUE_ON_VALUE])) | |
| def close_torque(self, can_id): | |
| return self.write_register(can_id, TORQUE_ON_OFF_REGISTER, bytearray([TORQUE_OFF_VALUE])) | |
| def set_zero_position(self, can_id): | |
| return self.write_register(can_id, SET_ZERO_POSITION_REGISTER, bytearray([SET_ZERO_POSITION_VALUE])) | |
| def set_openting_model(self, can_id): | |
| return self.write_register(can_id, OPENTING_MODEL_REGISTER, bytearray([OPENTING_MODEL_VALUE])) | |
| def send_mit_control(self, can_id, p_des, v_des, kp, kd, t_ff): | |
| # 暂时不修改这部分，因为它不属于你当前的主要需求，但保留 | |
| p_des_int = float_to_uint(p_des, P_DES_MIN, P_DES_MAX, 16) | |
| v_des_int = float_to_uint(v_des, V_DES_MIN, V_DES_MAX, 12) | |
| kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12) | |
| kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12) | |
| t_ff_int = float_to_uint(t_ff, T_FF_MIN, T_FF_MAX, 12) | |
| data = bytearray(8) | |
| data[0] = (p_des_int >> 8) & 0xFF | |
| data[1] = p_des_int & 0xFF | |
| data[2] = (v_des_int >> 4) & 0xFF | |
| data[3] = ((v_des_int & 0x0F) << 4) | ((kp_int >> 8) & 0x0F) | |
| data[4] = kp_int & 0xFF | |
| data[5] = (kd_int >> 4) & 0xFF | |
| data[6] = ((kd_int & 0x0F) << 4) | ((t_ff_int >> 8) & 0x0F) | |
| data[7] = t_ff_int & 0xFF | |
| self.send_command(can_id, data) | |
| response = self.receive_response(timeout=0.05) | |
| if response and len(response.data) == 8: | |
| return self._parse_mit_response(response.data) | |
| else: | |
| print(f"MIT control failed or invalid response for CAN ID {hex(can_id)}.") | |
| return None | |
| def _parse_mit_response(self, data): | |
| # 暂时不修改这部分，因为它不属于你当前的主要需求，但保留 | |
| motor_id = data[0] & 0x0F | |
| err_code = (data[0] >> 4) & 0x0F | |
| pos_raw = (data[1] << 8) | data[2] | |
| vel_raw = ((data[3] << 4) | ((data[4] >> 4) & 0x0F)) | |
| torque_raw = (((data[4] & 0x0F) << 8) | data[5]) | |
| t_mos = data[6] | |
| t_rotor = data[7] | |
| # print(f"--- MIT/Torque Response (ID: {hex(motor_id)}) --- Err: {err_code}, Pos: {pos_raw}, Vel: {vel_raw}, T: {torque_raw}") | |
| return { | |
| "ID": motor_id, "ERR": err_code, "POS_RAW": pos_raw, | |
| "VEL_RAW": vel_raw, "T_RAW": torque_raw, "T_MOS": t_mos, "T_Rotor": t_rotor | |
| } | |
| if __name__ == "__main__": | |
| # SLAN_PORT = 'ttyACM0'/ | |
| SLAN_PORT = 'can0' | |
| # 四个驱动器的 CAN ID | |
| MOTOR_CAN_IDS = [ 0x01]#, 0x02,0x3, 0x04, 0x05,0x06] | |
| motor_comm = MotorSLCan(SLAN_PORT) | |
| if motor_comm.connect(): | |
| try: | |
| # 存储每个电机的最后操作时间，用于调度 | |
| last_status_query_time = {can_id: 0 for can_id in MOTOR_CAN_IDS} | |
| last_pos_query_time = {can_id: 0 for can_id in MOTOR_CAN_IDS} | |
| last_goal_update_time = {can_id: 0 for can_id in MOTOR_CAN_IDS} | |
| # 目标位置生成器的状态 | |
| current_goal_position_rad = {can_id: 0.0 for can_id in MOTOR_CAN_IDS} | |
| goal_position_direction = {can_id: 1 for can_id in MOTOR_CAN_IDS} # 1 for increasing, -1 for decreasing | |
| MAX_GOAL_POS_RAD = 1.0 * math.pi # 180 度，可根据需要调整 | |
| MIN_GOAL_POS_RAD = -1.0 * math.pi # -180 度 | |
| GOAL_POS_STEP_RAD = 0.001 * math.pi # 每次更新的步长 | |
| print("\n--- INITIALIZING MOTORS ---") | |
| for motor_id in MOTOR_CAN_IDS: | |
| print(f"\nInitializing Motor {hex(motor_id)}:") | |
| # 1. PING | |
| print(f" Pinging...") | |
| motor_info = motor_comm.ping_device(motor_id) | |
| if not motor_info: | |
| print(f" Failed to PING motor {hex(motor_id)}. Skipping initialization.") | |
| continue # 如果PING失败，跳过这个电机的初始化 | |
| # 2. 设置 OPENTING_MODEL | |
| print(f" Setting OPENTING_MODEL...") | |
| if not motor_comm.set_openting_model(motor_id): | |
| print(f" Failed to set OPENTING_MODEL for motor {hex(motor_id)}.") | |
| continue | |
| time.sleep(0.1) | |
| # 3. 打开扭矩开关 | |
| print(f" Opening torque...") | |
| if not motor_comm.open_torque(motor_id): | |
| print(f" Failed to open torque for motor {hex(motor_id)}.") | |
| continue | |
| print("\n--- STARTING CONTROL LOOP ---") | |
| # 主循环频率控制 | |
| LOOP_FREQUENCY = 1000 # Hz | |
| LOOP_PERIOD = 1.0 / LOOP_FREQUENCY # seconds | |
| # 指令发送频率 | |
| STATUS_QUERY_PERIOD = 0.100 # 100 ms | |
| POS_QUERY_PERIOD = 0.010 # 10 ms | |
| GOAL_UPDATE_PERIOD = 0.010 # 10 ms | |
| start_time = time.monotonic() | |
| loop_count = 0 | |
| while True: | |
| loop_start_time = time.monotonic() | |
| for motor_id in MOTOR_CAN_IDS: | |
| current_time = time.monotonic() | |
| # ① 0x02 查询 (100ms) | |
| if current_time - last_status_query_time[motor_id] >= STATUS_QUERY_PERIOD: | |
| status = motor_comm.read_device_status(motor_id) | |
| #if status: | |
| # print( | |
| # f"Loop {loop_count}: Motor {hex(motor_id)} Status: Mode={status['OperatorMode']}, Volt={status['INPUT_VOLTAGE_V']:.2f}V, Temp={status['T_MOS']}C/{status['T_Rotor']}C") | |
| last_status_query_time[motor_id] = current_time | |
| # ② 位置查询 (10ms) | |
| if current_time - last_pos_query_time[motor_id] >= POS_QUERY_PERIOD: | |
| # 假设 PRESENT_POSITION 寄存器返回 4 字节（32位）表示位置 | |
| pos_raw_bytes = motor_comm.read_register(motor_id, PRESENT_POSITION, 4) | |
| if pos_raw_bytes and len(pos_raw_bytes) == 4: | |
| pos_raw = struct.unpack('<i', pos_raw_bytes)[0] # 假设无符号16位小端 | |
| # 将原始整数位置转换为浮点数 (需要根据你的电机文档提供正确的映射范围和位数) | |
| # 这里我假设 PRESENT_POSITION 也是 16 位，范围与 MIT 的 P_DES 类似 | |
| #actual_pos_rad = uint_to_float(pos_raw, POS_MIN_RANGE, POS_MAX_RANGE, 16) | |
| #print( | |
| # f"Loop {loop_count}: Motor {hex(motor_id)} Pos: {pos_raw} ") | |
| last_pos_query_time[motor_id] = current_time | |
| # ③ 更新目标位置 (10ms) | |
| if current_time - last_goal_update_time[motor_id] >= GOAL_UPDATE_PERIOD: | |
| # 简单的目标位置生成：在 MIN_GOAL_POS_RAD 和 MAX_GOAL_POS_RAD 之间往复运动 | |
| if goal_position_direction[motor_id] == 1: | |
| current_goal_position_rad[motor_id] += GOAL_POS_STEP_RAD | |
| if current_goal_position_rad[motor_id] >= MAX_GOAL_POS_RAD: | |
| current_goal_position_rad[motor_id] = MAX_GOAL_POS_RAD | |
| goal_position_direction[motor_id] = -1 | |
| else: | |
| current_goal_position_rad[motor_id] -= GOAL_POS_STEP_RAD | |
| if current_goal_position_rad[motor_id] <= MIN_GOAL_POS_RAD: | |
| current_goal_position_rad[motor_id] = MIN_GOAL_POS_RAD | |
| goal_position_direction[motor_id] = 1 | |
| # 将浮点目标位置转换为电机期望的整数格式（假设是16位，与P_DES_MIN/MAX类似） | |
| #goal_pos_int = float_to_uint(current_goal_position_rad[motor_id], POS_MIN_RANGE, POS_MAX_RANGE, 16) | |
| goal_pos_int = int(RAD_TO_SIGNAL * current_goal_position_rad[motor_id]) | |
| goal_bytes = struct.pack('<i', goal_pos_int) # 假设有符号的4字节小端 | |
| # 写入 GOAL_POSITION 寄存器 | |
| motor_comm.write_register(motor_id, GOAL_POSITION, goal_bytes) | |
| #print(f"Loop {loop_count}: Motor {hex(motor_id)} Set Goal: {current_goal_position_rad[motor_id]:.4f} rad (raw: {goal_pos_int})") | |
| last_goal_update_time[motor_id] = current_time | |
| loop_count += 1 | |
| # 确保主循环以 3000Hz 运行 | |
| loop_end_time = time.monotonic() | |
| time_elapsed = loop_end_time - loop_start_time | |
| time_to_sleep = LOOP_PERIOD - time_elapsed | |
| if time_to_sleep > 0: | |
| time.sleep(time_to_sleep) | |
| else: | |
| # 如果循环执行时间超过了周期，打印警告 | |
| #print(f"Warning: Loop {loop_count} took {time_elapsed*1000:.2f} ms, exceeding {LOOP_PERIOD*1000:.2f} ms.") | |
| pass | |
| except KeyboardInterrupt: | |
| print("\nExiting program, closing torque for all motors.") | |
| finally: | |
| for motor_id in MOTOR_CAN_IDS: | |
| for _ in range(5): | |
| try: | |
| motor_comm.write_register(motor_id, TORQUE_ON_OFF_REGISTER, | |
| bytearray([TORQUE_OFF_VALUE])) | |
| except Exception: | |
| pass | |
| time.sleep(0.05) | |
| else: | |
| print("Failed to connect to SLCan, please check port and device.") | |
| else: | |
| print("Failed to connect to SLCan, please check port and device.") |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment