Python串口控制MS伺服电机(多圈角度)

"""
多圈电机转动模式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"))