"""
多圈电机转动模式1
"""
def decimal_to_hex_bytes(decimal_number, byte_size):
# 检查字节大小参数的有效性
valid_sizes = {"int8": 1, "int16": 2, "int32": 4, "int64": 8}
if byte_size not in valid_sizes:
raise ValueError(f"Invalid byte size: {byte_size}. Expected one of {list(valid_sizes.keys())}")
byte_length = valid_sizes[byte_size]
# 处理负数的补码表示
if decimal_number < 0:
# 计算补码
max_val = (1 << (byte_length * 8)) # 2^(byte_length * 8)
decimal_number = max_val + decimal_number
# 转换十进制数为指定字节数的十六进制表示
hex_string = hex(decimal_number)[2:].zfill(byte_length * 2).lower()
# 将十六进制字符串分割为字节，并反转以得到小端序表示
hex_bytes = [hex_string[i:i + 2] for i in range(0, len(hex_string), 2)][::-1]
# 如果需要的字节数比实际十六进制字符串表示的字节数多，则在前面补0
padding_size = valid_sizes[byte_size] - len(hex_bytes)
if padding_size > 0:
hex_bytes = ['00'] * padding_size + hex_bytes
return hex_bytes
def generate_frame_cmd(id="01"):
"""
生成帧命令
:param id: 01 - 03
"""
cmd_sum = hex(sum([int(i, 16) for i in ["3E", "A3", id, "08"]])) # 最末位求和
return f"3E A3 {id} 08 {cmd_sum.replace("0x", "")}".upper()
def generate_frame_data(angle):
"""
生成帧数据
:param angle: 旋转角度 0-360°
"""
angle_list = decimal_to_hex_bytes(decimal_number=angle * 100, byte_size="int64")
data_sum = hex(sum([int(i, 16) for i in angle_list]) & 0xFF).replace("0x", "") # 最末尾求和
return f"{" ".join(angle_list)} {"{:02x}".format(int(data_sum, 16))}".upper()
def gen_stop_cmd(id="01"):
"""
电机停止指令
:param id: 电机编号
"""
cmd_sum = hex(sum([int(i, 16) for i in ["3E", "81", id, "00"]]))
return f"3E 81 {id} 00 {"{:02x}".format(int(cmd_sum, 16))}".upper()
def gen_clear_cmd(id="01"):
"""
电机圈数置零
:param id: 电机编号
"""
clear_cmd = hex(sum([int(i, 16) for i in ["3E", "93", id, "00"]]))
return f"3E 93 {id} 00 {"{:02x}".format(int(clear_cmd, 16))}".upper()
def gen_status_cmd(id="01"):
"""
获取电机状态，该命令只在电机建立连接调用一次
:param id： 电机编号
"""
cmd_sum = hex(sum([int(i, 16) for i in ["3E", "9C", id, "00"]]))
return f"3E 9C {id} 00 {"{:02x}".format(int(cmd_sum, 16))}".upper()
def gen_angle_cmd(id="01"):
"""
获取电机角度
:param id： 电机编号
"""
cmd_sum = hex(sum([int(i, 16) for i in ["3E", "94", id, "00"]]))
return f"3E 92 {id} 00 {"{:02x}".format(int(cmd_sum, 16))}".upper()
def parse_angle(hex_str):
"""
转圈角度返回值解析
:param hex_str: 角度返回值
"""
formatted_hex_str = " ".join(hex_str[i:i + 2] for i in range(0, len(hex_str), 2))
# 提取第6到第9个位置的字节，并反转顺序
sub_hex_str = formatted_hex_str.split()[5:9][::-1]
# 将提取的四个字符转换为十六进制整数，然后转换为十进制
angle = int("".join(sub_hex_str), 16)
return round(angle / 100, 2)
def generate_cmd(frame_cmd, frame_data):
"""
生成指令
:param frame_cmd: 帧指令
:param frame_data: 帧数据
"""
return " ".join([frame_cmd, frame_data]).upper()
def calculate_rotation(current_angle, target_angle):
"""
计算最小旋转角度以及顺逆时针: 电机：角度增大逆时针 角度减小顺时针
:param current_angle:
:param target_angle:
"""
if target_angle - current_angle >= 180: # 10 210
target_angle = target_angle - 360
if current_angle - target_angle >= 180: # 230 30
current_angle = current_angle - 360
diff = target_angle - current_angle
if diff > 180:
diff = diff - 360
if diff > 0:
min_rotation = min(diff, 360 - diff)
else:
min_rotation = min(-diff, 360 + diff)
# print("当前角度: ", current_angle)
# print("目标角度: ", target_angle) # 目标角度作为下次的当前角度
# print("最小旋转角度: ", min_rotation)
return current_angle, target_angle, min_rotation # 返回目标角度以及最小旋转角度
if __name__ == "__main__":
"""
当前角度被修改，置0
"""
_current_angle = 50
_target_angle = 350
n_current_angle, n_target_angle, _ = calculate_rotation(current_angle=_current_angle, target_angle=_target_angle)
if n_current_angle != _current_angle:
print(gen_clear_cmd(id="03"))
_frame_cmd = generate_frame_cmd(id="03")
_frame_data = generate_frame_data(angle=n_target_angle)
gen_cmd = generate_cmd(frame_cmd=_frame_cmd, frame_data=_frame_data)
print(f"当前角度: {n_current_angle} 目标角度: {n_target_angle} 指令: {gen_cmd}")
# print(calculate_rotation(-128, 300)) # 置零
# print(calculate_rotation(28, 280))
# print(calculate_rotation(228, 20))
# print(calculate_rotation(0, 181))
# print(calculate_rotation(181, 0))
# print(calculate_rotation(10, 210))
# print(calculate_rotation(280, 20))
# print(decimal_to_hex_bytes(decimal_number=2200, byte_size="int64"))
# print(decimal_to_hex_bytes(decimal_number=-2200, byte_size="int64"))
标签：angle,伺服电机,cmd,hex,current,frame,串口,多圈,id From： https://www.cnblogs.com/52-qq/p/18212233