R360-ControlPanelV2/PitchControl.py

import serial
import time
import struct
import logging


def calculate_bcc(data_bytes):
    checksum = 0
    for byte in data_bytes:
        checksum ^= byte
    return checksum


def convert_2_to_1_byte(high_byte, low_byte):
    return ((high_byte & 0x7F) << 4) | (low_byte & 0x7F)


def convert_and_interpret(data, data_type):
    # 将字节数据转换为32位无符号整数
    udata = data[0]
    for i in range(1, 5):
        udata <<= 7
        udata |= data[i] & 0x7F
    # 根据所需的类型解释数据
    if data_type == 'float':
        return struct.unpack('f', udata.to_bytes(4, byteorder='little'))[0]
    elif data_type == 'int32':
        return struct.unpack('i', udata.to_bytes(4, byteorder='little'))[0]
    elif data_type == 'uint32':
        return udata
    else:
        raise ValueError("Invalid data type specified.")


def is_bit_zero(data, bit):
    return (data & (1 << bit)) == 0


def calculate_spd():
    """
    根据蜗轮蜗杆的输出速度，步进电机的步距角，细分值（micro_step）和减速比反向计算spd值。
    """
    # 使用之前的例子中的参数来计算spd的值

    output_speed = 15 / 360  # 之前计算出的蜗轮蜗杆的输出速度，单位为周/秒
    step_angle = 1.8  # 电机的步距角
    micro_step = 1 / 32  # 电机的细分值
    gear_ratio = 31  # 减速比
    motor_speed = output_speed * gear_ratio
    spd = motor_speed * (360 / step_angle / micro_step)
    return spd


def get_position(angle):
    if angle > 6 or angle < -6:
        raise ValueError('角度超出调整范围')
    else:
        position = 31 * angle * 32 / 1.8
        return int(position)


class MotorControlVince:
    def __init__(self, port='COM4', baudrate=9600):
        self.logger = logging.getLogger(
            'pitch_motor'
        )
        self.ser = serial.Serial(port, baudrate, timeout=1)
        device_number, speed, position, condition = self.send_command('1 sts')
        self.current_position = position
        self.current_angle = self.current_position * 1.8 / (32 * 31)

    def update_angle(self):
        device_number, speed, position, condition = self.send_command('1 sts')
        self.current_position = position
        self.current_angle = self.current_position * 1.8 / (32 * 31)

    def validate_data_stream(self, data_stream):
        if data_stream[0] != 0xFF or data_stream[-1] != 0xFE:
            self.logger.error("数据流格式错误")
            raise ValueError(f"数据流格式错误，数据流为{data_stream}")
        bcc_received_high, bcc_received_low = data_stream[-3], data_stream[-2]
        bcc_received = convert_2_to_1_byte(bcc_received_high, bcc_received_low)
        bcc_calculated = calculate_bcc(data_stream[1:-3])
        if bcc_received != bcc_calculated:
            self.logger.error("校验码不匹配")
            raise ValueError("校验码不匹配")
        device_number = data_stream[1]
        feedback_number = data_stream[2]
        data_content = data_stream[3:-3]
        return device_number, feedback_number, data_content

    def send_command(self, command):
        for _ in range(3):  # 重试3次
            try:
                self.ser.reset_input_buffer()
                self.ser.write((command + "\n").encode('utf-8'))
                self.logger.info(msg=f"发送: '{command}'")
                time.sleep(0.1)
                response = self.process_feedback()  # 处理反馈
                if response:  # 如果收到预期的回复，跳出循环
                    return response
            except (ValueError, serial.SerialException, IOError) as e:
                self.logger.error(f"发送命令时发生错误: {e}")
                time.sleep(0.5)  # 等待一段时间后重试
        self.logger.error("重试次数达到上限，未能成功发送命令")
        raise Exception("Command sending failed after multiple attempts")

    def process_feedback(self):
        if self.ser.in_waiting:
            reply = self.ser.read(self.ser.in_waiting)
            reply_bytes = list(reply)
            device_number, feedback_number, data_content = self.validate_data_stream(reply_bytes)

            if feedback_number == 3:
                string_content = bytes(data_content).decode('utf-8')
                self.logger.info(f"{string_content}")
                return f"字符串数据内容: {string_content}"
            else:
                speed = data_content[0:5]
                speed = convert_and_interpret(speed, 'float')
                position = data_content[5:10]
                position = convert_and_interpret(position, 'int32')
                condition = data_content[10:15]
                condition = convert_and_interpret(condition, 'uint32')
                self.logger.info(f"设备号:{device_number} 速度:{speed} 位置:{position} "
                                 f"角度:{position * 1.8 / (32 * 31):.1f}° 状态码:{condition}")
                return device_number, speed, position, condition
        self.logger.error("没有回复")
        raise ValueError('No Response')

    def find_zero_position(self):
        self.send_command('1 ena')
        time.sleep(2)
        self.logger.info('>>>>开始回零<<<<')
        self.move_to_limit(1, 5000, 2)  # 移动到正极限
        self.logger.info(">>>>到达正极限<<<<")
        # 移动到正极限
        psr_position = self.current_position

        self.move_to_limit(1, -5000, 3)  # 移动到负极限
        msr_position = self.current_position
        self.logger.info(">>>>到达负极限<<<<")

        zero_position = (psr_position + msr_position) / 2
        print(f"零点位置: {zero_position}")

        self.send_command(f'1 pos {zero_position}')
        self.wait_for_target(8)
        self.send_command('1 org')
        print('零点设置成功')
        self.logger.info(">>>>回零结束<<<<")

        return zero_position

    def move_to_limit(self, device_number, speed, condition_bit):
        self.send_command(f'{device_number} mov spd={speed}')
        while True:
            device_number, speed, position, condition = self.send_command('1 sts')
            if not is_bit_zero(condition, condition_bit):
                self.current_position = position
                self.update_angle()
                break
        print(f"到达极限，当前位置: {position}")

    def move_to_angle(self, angle):
        position = get_position(angle)
        self.reach_target(1, position, 8)

    def wait_for_target(self, condition_bit):
        while True:
            device_number, speed, position, condition = self.send_command('1 sts')
            if not is_bit_zero(condition, condition_bit):
                print("到达目标位置")
                break

    def reach_target(self, device_number, position, condition_bit):
        self.send_command(f'{device_number} pos {position}')
        while True:
            device_number, speed, position, condition = self.send_command('1 sts')
            if not is_bit_zero(condition, condition_bit):
                self.current_position = position
                self.update_angle()
                break
        print(f"到达位置: {position}")

    def increase_angle(self):  # 俯仰角增加一度
        self.move_to_angle(self.current_angle + 1)

    def decrease_angle(self):  # 俯仰角减小一度
        self.move_to_angle(self.current_angle - 1)

    def close(self):
        self.ser.close()


if __name__ == "__main__":
    motor_control = MotorControlVince()
    motor_control.move_to_angle(0)