170 lines
5.7 KiB
Python
170 lines
5.7 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# @Time : 2025/7/18
|
||
# @Author : hx
|
||
# @File : motor_control_can_all.py
|
||
'''
|
||
|
||
import can
|
||
import time
|
||
import argparse
|
||
|
||
|
||
# 细分值映射表
|
||
MICROSTEP_MAP = {
|
||
2: 0x01,
|
||
4: 0x02,
|
||
8: 0x04,
|
||
16: 0x0,
|
||
32: 0x20
|
||
}
|
||
|
||
# ========================================
|
||
# CAN 发送函数（使用 socketcan）
|
||
# ========================================
|
||
def send_can_frame(data, can_id=0x01, channel='can0', bustype='socketcan'):
|
||
"""
|
||
发送CAN帧
|
||
:param data: 7字节的列表
|
||
:param can_id: CAN ID
|
||
:param channel: CAN 通道
|
||
:param bustype: 总线类型
|
||
"""
|
||
try:
|
||
bus = can.interface.Bus(channel=channel, bustype=bustype)
|
||
msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False)
|
||
bus.send(msg)
|
||
print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]")
|
||
bus.shutdown()
|
||
except Exception as e:
|
||
print(f"[发送失败] {e}")
|
||
|
||
|
||
# ========================================
|
||
# 模式1：速度控制模式 (0x01)
|
||
# ========================================
|
||
def generate_speed_command(direction, microstep, speed):
|
||
"""
|
||
生成速度控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param speed: 速度值 (单位: rad/s)
|
||
"""
|
||
direction_byte = 0x01 if direction == 1 else 0x00
|
||
microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)
|
||
|
||
raw_speed = int(speed * 10)
|
||
speed_high = (raw_speed >> 8) & 0xFF
|
||
speed_low = raw_speed & 0xFF
|
||
|
||
return [0x01, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low]
|
||
|
||
|
||
# ========================================
|
||
# 模式2：位置控制模式 (0x02)
|
||
# ========================================
|
||
def generate_position_command(direction, microstep, angle):
|
||
"""
|
||
生成位置控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param angle: 角度值 (单位: 度)
|
||
"""
|
||
direction_byte = 0x01 if direction == 1 else 0x00
|
||
microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)
|
||
|
||
raw_angle = int(angle * 10)
|
||
pos_high = (raw_angle >> 8) & 0xFF
|
||
pos_low = raw_angle & 0xFF
|
||
|
||
return [0x02, direction_byte, microstep_byte, pos_high, pos_low, 0x00, 0x64]
|
||
|
||
|
||
# ========================================
|
||
# 模式3：力矩控制模式 (0x03)
|
||
# ========================================
|
||
def generate_torque_command(direction, microstep, current):
|
||
"""
|
||
生成力矩控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param current: 电流值 (单位: mA)
|
||
"""
|
||
direction_byte = 0x01 if direction == 1 else 0x00
|
||
microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)
|
||
|
||
raw_current = int(current)
|
||
current_high = (raw_current >> 8) & 0xFF
|
||
current_low = raw_current & 0xFF
|
||
|
||
return [0x03, direction_byte, microstep_byte, current_high, current_low, 0x00, 0x64]
|
||
|
||
|
||
# ========================================
|
||
# 模式4：单圈绝对角度控制模式 (0x04)
|
||
# ========================================
|
||
def generate_absolute_angle_command(direction, microstep, angle, speed):
|
||
"""
|
||
生成单圈绝对角度控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param angle: 目标角度 (单位: 度)
|
||
:param speed: 速度值 (单位: rad/s)
|
||
"""
|
||
direction_byte = 0x01 if direction == 1 else 0x00
|
||
microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)
|
||
|
||
raw_angle = int(angle * 10)
|
||
pos_high = (raw_angle >> 8) & 0xFF
|
||
pos_low = raw_angle & 0xFF
|
||
|
||
raw_speed = int(speed * 10)
|
||
speed_high = (raw_speed >> 8) & 0xFF
|
||
speed_low = raw_speed & 0xFF
|
||
|
||
|
||
return [0x04, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low]
|
||
|
||
|
||
# ========================================
|
||
# 主函数（命令行调用）
|
||
# ========================================
|
||
def main():
|
||
parser = argparse.ArgumentParser(description="CAN电机控制程序，支持4种模式")
|
||
parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4],
|
||
help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度")
|
||
parser.add_argument("--direction", type=int, choices=[0, 1], default=1,
|
||
help="方向: 0=逆时针, 1=顺时针")
|
||
parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32,
|
||
help="细分值: 2, 4, 8, 16, 32")
|
||
parser.add_argument("--value", type=float, required=True,
|
||
help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）")
|
||
parser.add_argument("--speed_rad_per_sec", type=float,
|
||
help="速度值（仅用于绝对角度模式）")
|
||
args = parser.parse_args()
|
||
|
||
try:
|
||
if args.mode == 1:
|
||
cmd = generate_speed_command(args.direction, args.microstep, args.value)
|
||
elif args.mode == 2:
|
||
cmd = generate_position_command(args.direction, args.microstep, args.value)
|
||
elif args.mode == 3:
|
||
cmd = generate_torque_command(args.direction, args.microstep, args.value)
|
||
elif args.mode == 4:
|
||
if args.speed_rad_per_sec is None:
|
||
raise ValueError("绝对角度模式需要提供速度参数 (--speed_rad_per_sec)")
|
||
cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value, args.speed_rad_per_sec)
|
||
|
||
print(f"生成CAN指令: [{', '.join(f'0x{x:02X}' for x in cmd)}]")
|
||
send_can_frame(cmd)
|
||
|
||
except Exception as e:
|
||
print(f"错误: {e}")
|
||
|
||
|
||
# ========================================
|
||
# 程序入口
|
||
# ========================================
|
||
if __name__ == "__main__":
|
||
main() |