WIN11系统环境松灵机器人SCOUT2.0底盘CAN通信控制测试

参考资料

用户手册 https://new.agilex.ai/raw/upload/20230718/SCOUT 2.0用户手册20230718_74677.pdf
如何通过开源SDK控制松灵机器人SCOUT底盘？ https://blog.csdn.net/AgileX/article/details/106730024
SCOUT底盘测试 通过CAN总线和RS232控制松灵地盘 https://blog.csdn.net/qsl56789/article/details/135264569

测试环境

python 3.10
需要安装包库
pip install python-can
pip install cantact

使用以下代码检测是否连接到usb转can设备

import can
print("可用CAN接口:", can.detect_available_configs())

可能存在的回复如下：
可用CAN接口: [{'interface': 'cantact', 'channel': 'ch:0'}, {'interface': 'virtual', 'channel': 'channel-2798'}, {'interface': 'serial', 'channel': 'COM3'}, {'interface': 'serial', 'channel': 'COM4'}]
其中{'interface': 'cantact', 'channel': 'ch:0'}就是默认的can设备

测试代码

import can
import time
import threading
import struct# print("可用CAN接口:", can.detect_available_configs())class ScoutCANController:def __init__(self, channel=0):"""初始化CAN控制器"""self.bus = Noneself.channel = channelself.connected = Falseself._stop_event = threading.Event()self._receive_thread = None# CAN ID定义self.CAN_ID_CONTROL_MODE = 0x421  # 控制模式设定self.CAN_ID_MOTION_CMD = 0x111  # 运动控制命令self.CAN_ID_VEHICLE_STATE = 0x162  # 车辆状态反馈# 存储状态数据self.state = {'linear_velocity': 0.0,  # m/s'angular_velocity': 0.0,  # rad/s'battery_voltage': 0.0,  # V'system_status': 0,'control_mode': 0}def connect(self):"""连接到CAN总线"""try:self.bus = can.interface.Bus(interface='cantact',channel=self.channel,bitrate=500000)self.connected = Trueprint(f"成功连接到CAN接口: 通道 {self.channel}")# 启动接收线程self._stop_event.clear()self._receive_thread = threading.Thread(target=self._receive_loop)self._receive_thread.daemon = Trueself._receive_thread.start()# 先获取当前状态self.request_vehicle_state()time.sleep(0.5)return Trueexcept Exception as e:print(f"CAN连接失败: {e}")return Falsedef _receive_loop(self):"""接收CAN消息循环"""while not self._stop_event.is_set() and self.connected:try:msg = self.bus.recv(timeout=0.1)if msg:self._process_message(msg)except can.CanError as e:if not self._stop_event.is_set():print(f"CAN接收错误: {e}")except Exception as e:print(f"接收处理错误: {e}")def _process_message(self, msg):"""处理接收到的CAN消息"""if msg.arbitration_id == self.CAN_ID_VEHICLE_STATE:self._parse_vehicle_state(msg.data)elif msg.arbitration_id == self.CAN_ID_CONTROL_MODE:if len(msg.data) >= 1:self.state['control_mode'] = msg.data[0]mode_str = "CAN控制模式" if msg.data[0] == 0x01 else "待机模式"print(f"接收到控制模式状态: {mode_str}")def _parse_vehicle_state(self, data):"""解析车辆状态数据"""if len(data) < 8:print(f"状态数据长度不足: {len(data)} < 8")return# 线速度 (mm/s -> m/s)linear_speed_mm = struct.unpack('<h', data[0:2])[0]self.state['linear_velocity'] = linear_speed_mm / 1000.0# 角速度 (0.001rad/s -> rad/s)angular_speed_unit = struct.unpack('<h', data[2:4])[0]self.state['angular_velocity'] = angular_speed_unit * 0.001# 电池电压 (0.1V -> V)battery_voltage_tenth = struct.unpack('<B', data[4:5])[0]self.state['battery_voltage'] = battery_voltage_tenth / 10.0# 系统状态if len(data) >= 6:self.state['system_status'] = data[5]# 打印状态更新print(f"【状态反馈】速度: {self.state['linear_velocity']:.2f}m/s, "f"角速度: {self.state['angular_velocity']:.3f}rad/s, "f"电池: {self.state['battery_voltage']:.1f}V, "f"状态: {self.state['system_status']}")def set_control_mode(self, enable=True):"""设置控制模式Args:enable: True=使能CAN控制模式, False=待机模式"""if not self.connected:print("未连接到CAN总线")return False# 构建控制模式命令data = [0x01 if enable else 0x00]  # 0x01: CAN控制使能, 0x00: 待机msg = can.Message(arbitration_id=self.CAN_ID_CONTROL_MODE,data=data,is_extended_id=False)try:self.bus.send(msg)mode_str = "使能CAN控制模式" if enable else "设置待机模式"print(f"【已发送】{mode_str} 命令 (ID: 0x{self.CAN_ID_CONTROL_MODE:X})")return Trueexcept can.CanError as e:print(f"发送控制模式命令失败: {e}")return Falsedef set_motion_command(self, linear_vel=0.0, angular_vel=0.0):"""设置运动命令Args:linear_vel: 线速度 (m/s), 范围[-1.5, 1.5]angular_vel: 角速度 (rad/s), 范围[-0.523, 0.523]"""if not self.connected:print("未连接到CAN总线")return False# 限制速度范围linear_vel = max(min(linear_vel, 1.5), -1.5)angular_vel = max(min(angular_vel, 0.523), -0.523)# 转换为协议要求的单位linear_speed_mm = int(linear_vel * 1000)  # m/s -> mm/sangular_speed_unit = int(angular_vel / 0.001)  # rad/s -> 0.001rad/s# 构建运动控制命令data = bytearray(8)# 线速度 (小端格式)data[0] = linear_speed_mm & 0xFFdata[1] = (linear_speed_mm >> 8) & 0xFF# 角速度 (小端格式)data[2] = angular_speed_unit & 0xFFdata[3] = (angular_speed_unit >> 8) & 0xFF# 保留字节 (根据协议必须为0)data[4] = 0x00data[5] = 0x00data[6] = 0x00data[7] = 0x00msg = can.Message(arbitration_id=self.CAN_ID_MOTION_CMD,data=data,is_extended_id=False)try:self.bus.send(msg)print(f"【已发送】运动命令 (ID: 0x{self.CAN_ID_MOTION_CMD:X}): "f"线速度={linear_vel:.2f}m/s, 角速度={angular_vel:.3f}rad/s")return Trueexcept can.CanError as e:print(f"发送运动命令失败: {e}")return Falsedef request_vehicle_state(self):"""请求车辆状态"""# 对于Scout 2.0，状态通常是主动发送的，但我们可以尝试发送请求try:# 发送一个空请求，有些设备会响应msg = can.Message(arbitration_id=self.CAN_ID_VEHICLE_STATE,data=[0x00],is_extended_id=False)self.bus.send(msg)print(f"【已发送】状态请求 (ID: 0x{self.CAN_ID_VEHICLE_STATE:X})")return Trueexcept can.CanError as e:print(f"发送状态请求失败: {e}")return Falsedef get_state(self):"""获取当前状态"""return self.state.copy()def disconnect(self):"""断开连接"""self._stop_event.set()if self._receive_thread and self._receive_thread.is_alive():self._receive_thread.join(timeout=1.0)if self.bus:# 发送停止命令self.set_motion_command(0.0, 0.0)time.sleep(0.2)# 停止接收self.bus.shutdown()print("CAN连接已关闭")self.connected = False# 主程序
if __name__ == "__main__":# 创建控制器controller = ScoutCANController(channel=0)try:print("=== 开始连接到Scout 2.0 ===")if not controller.connect():print("连接失败，退出程序")exit(1)# 等待初始状态print("\n=== 等待初始状态反馈 ===")time.sleep(1.0)# 步骤1: 使能CAN控制模式print("\n=== 步骤1: 使能CAN控制模式 ===")if not controller.set_control_mode(True):print("警告: 控制模式设置失败，继续尝试运动命令...")time.sleep(0.5)# 步骤2: 向前移动print("\n=== 步骤2: 向前移动 (0.2 m/s) ===")controller.set_motion_command(linear_vel=0.1, angular_vel=0.0)start_time = time.time()while time.time() - start_time < 2.0:current_state = controller.get_state()print(f"  当前速度: {current_state['linear_velocity']:.2f}m/s")time.sleep(0.2)# 步骤3: 原地旋转print("\n=== 步骤3: 原地旋转 (0.2 rad/s) ===")controller.set_motion_command(linear_vel=0.0, angular_vel=0.1)start_time = time.time()while time.time() - start_time < 2.0:current_state = controller.get_state()print(f"  当前角速度: {current_state['angular_velocity']:.3f}rad/s")time.sleep(0.2)# 步骤4: 曲线运动print("\n=== 步骤4: 曲线运动 (前进+旋转) ===")controller.set_motion_command(linear_vel=0.1, angular_vel=0.1)time.sleep(2.0)# 步骤5: 停止print("\n=== 步骤5: 停止 ===")controller.set_motion_command(0.0, 0.0)time.sleep(1.0)# 显示最终状态final_state = controller.get_state()print("\n=== 最终状态 ===")print(f"线速度: {final_state['linear_velocity']:.3f} m/s")print(f"角速度: {final_state['angular_velocity']:.3f} rad/s")print(f"电池电压: {final_state['battery_voltage']:.1f} V")print(f"系统状态: {final_state['system_status']}")print(f"控制模式: {'CAN控制' if final_state['control_mode'] == 1 else '待机'}")except KeyboardInterrupt:print("\n程序被用户中断")except Exception as e:print(f"发生错误: {e}")finally:print("\n=== 安全关闭 ===")if controller.connected:# 确保机器人停止print("发送停止命令...")controller.set_motion_command(0.0, 0.0)time.sleep(0.5)# 恢复待机模式print("恢复待机模式...")controller.set_control_mode(False)time.sleep(0.3)# 断开连接controller.disconnect()print("程序结束")