RobotHardware-UESTC/Hardware/UPbot-Tools/test.py

# 调试小车，ubuntu版本代码
# 可以按格式发送、接受数据

import serial
import time
import struct
import keyboard
import sys
import termios
import tty


class SerialPort:
    def __init__(self, port, baudrate=9600, timeout=1):
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.serial = None

    def open(self):
        try:
            self.serial = serial.Serial(self.port, self.baudrate, timeout=self.timeout)
            print(f"成功打开串口: {self.port}")
        except serial.SerialException as e:
            print(f"打开串口失败: {e}")
            self.serial = None

    def close(self):
        if self.serial and self.serial.is_open:
            self.serial.close()
            print("串口已关闭")

    def write(self, data):
        if self.serial and self.serial.is_open:
            if isinstance(data, str):
                data = data.encode()
            self.serial.write(data)

    def write_hex(self, data):
        if self.serial and self.serial.is_open:
            self.serial.write(data)

    def read(self, size=22):
        if self.serial and self.serial.is_open:
            data = self.serial.read(size)
            hex_data = ', '.join(f'0x{byte:02X}' for byte in data)
            print(f"接收数据: {hex_data}")
            return data
        return None


def parse_serial_feedback(data):
    struct_format = '<HhhhhhhhhHH'
    struct_size = struct.calcsize(struct_format)

    if len(data) < struct_size:
        raise ValueError("接收到的数据长度不足，无法解析。")

    parsed_data = struct.unpack(struct_format, data[:struct_size])
    feedback = {
        'start': parsed_data[0],
        'cmd1': parsed_data[1],
        'cmd2': parsed_data[2],
        'speedR_meas': parsed_data[3],
        'speedL_meas': parsed_data[4],
        'wheelR_cnt': parsed_data[5],
        'wheelL_cnt': parsed_data[6],
        'batVoltage': parsed_data[7] / 100.0,
        'boardTemp': parsed_data[8] / 10.0,
        'cmdLed': parsed_data[9],
        'checksum': parsed_data[10]
    }
    print(feedback)
    return feedback


def calculate_checksum(data):
    checksum = sum(data) & 0xFFFF
    return checksum


def send_speed_command(serial_port, steer, speed):
    start_frame = 0xABCD
    # command_format = '<HhhH'
    command_format = '<HhhH'
    command_data = struct.pack(command_format, start_frame, steer, speed, 0)
    # checksum = start_frame^steer^speed
    checksum = (start_frame ^ steer ^ speed) & 0xFFFF
    command_data = struct.pack(command_format, start_frame, steer, speed, checksum)

    hex_data = ', '.join(f'0x{byte:02X}' for byte in command_data)
    print(f"发送: {hex_data}")
    print(f"发送字节流: {command_data}")
    serial_port.write_hex(command_data)


def set_speed(serial_port, ls, rs):
    set_speed_l = ls / 0.10472
    set_speed_r = rs / 0.10472

    print(ls, rs)

    speed = (int)((set_speed_l + set_speed_r) / 2.0)
    steer = (int)((set_speed_l - speed) * 2.0)
    # steer = max((int)((set_speed_l - speed) * 2.0),(int)((set_speed_r - speed) * 2.0))

    send_speed_command(serial_port, steer, speed)


def get_key():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch


if __name__ == "__main__":
    port_name = '/dev/serial/by-path/platform-fc880000.usb-usb-0:1.2:1.0'
    baudrate = 115200

    serial_port = SerialPort(port=port_name, baudrate=baudrate)
    serial_port.open()

    ls = 1

    rs = 1

    try:
        while True:
            time.sleep(0.1)

            # if keyboard.is_pressed('up'):
            #     ls += 1
            #     rs += 1
            # elif keyboard.is_pressed('down'):
            #     ls -= 1
            #     rs -= 1
            # elif keyboard.is_pressed('left'):
            #     ls += 1
            #     rs -= 1
            # elif keyboard.is_pressed('right'):
            #     ls -= 1
            #     rs += 1

            key = get_key()

            # if key == '\x1b':  # 捕捉箭头键
            #     arrow_key = sys.stdin.read(2)
            if key == 'w':  # Up arrow
                ls += 3
                rs += 3
            elif key == 's':  # Down arrow
                ls = 1
                rs = 1

            elif key == 'a':  # Left arrow
                ls = 0.5
                rs += 3
            elif key == 'd':  # Right arrow
                ls += 3
                rs = 0.5

            elif key == 'q':  # 按 'q' 键退出
                break

            if ls < 0:
                ls = 0

            if rs < 0:
                rs = 0

            set_speed(serial_port, ls, rs)
            received_data = serial_port.read(44)
            parse_serial_feedback(received_data)

            # 重置速度
            if not any(keyboard.is_pressed(key) for key in ['up', 'down', 'left', 'right']):
                ls = 0
                rs = 0

    except KeyboardInterrupt:
        print("停止机器人")
    finally:
        serial_port.close()