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
测试环境
win11
python 3.10
需要安装包库
pip install python-can
pip install cantact
硬件连接使用官方配备的usb转can线
在windows设备管理器中发现usb-can adapter后，使用以下代码检测是否连接到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'}就是默认的usb-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 = None
self.channel = channel
self.connected = False
self._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 = True
print(f"成功连接到CAN接口: 通道 {self.channel}")
# 启动接收线程
self._stop_event.clear()
self._receive_thread = threading.Thread(target=self._receive_loop)
self._receive_thread.daemon = True
self._receive_thread.start()
# 先获取当前状态
self.request_vehicle_state()
time.sleep(0.5)
return True
except Exception as e:
print(f"CAN连接失败: {e}")
return False
def _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 True
except can.CanError as e:
print(f"发送控制模式命令失败: {e}")
return False
def 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/s
angular_speed_unit = int(angular_vel / 0.001) # rad/s -> 0.001rad/s
# 构建运动控制命令
data = bytearray(8)
# 线速度 (小端格式)
data[0] = linear_speed_mm & 0xFF
data[1] = (linear_speed_mm >> 8) & 0xFF
# 角速度 (小端格式)
data[2] = angular_speed_unit & 0xFF
data[3] = (angular_speed_unit >> 8) & 0xFF
# 保留字节 (根据协议必须为0)
data[4] = 0x00
data[5] = 0x00
data[6] = 0x00
data[7] = 0x00
msg = 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 True
except can.CanError as e:
print(f"发送运动命令失败: {e}")
return False
def 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 True
except can.CanError as e:
print(f"发送状态请求失败: {e}")
return False
def 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("程序结束")
浙公网安备 33010602011771号