162 lines
4.4 KiB
Python
162 lines
4.4 KiB
Python
# 调试小车，windows版本代码
|
||
# 可以按格式发送、接受数据
|
||
|
||
import serial
|
||
import time
|
||
import struct
|
||
import keyboard
|
||
import sys
|
||
import msvcrt # Windows 环境下用于捕捉键盘输入
|
||
|
||
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_data = struct.pack(command_format, start_frame, steer, speed, 0)
|
||
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}, {hex(checksum)}")
|
||
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)
|
||
|
||
send_speed_command(serial_port, steer, speed)
|
||
|
||
|
||
def get_key():
|
||
"""捕捉 Windows 上的键盘输入"""
|
||
if msvcrt.kbhit():
|
||
return msvcrt.getch().decode('utf-8')
|
||
return None
|
||
|
||
|
||
if __name__ == "__main__":
|
||
# 修改为 Windows 上的串口名称，例如 'COM3'
|
||
port_name = 'COM9'
|
||
baudrate = 115200
|
||
|
||
serial_port = SerialPort(port=port_name, baudrate=baudrate)
|
||
serial_port.open()
|
||
|
||
ls = 3
|
||
rs = 3
|
||
|
||
try:
|
||
while True:
|
||
time.sleep(0.1)
|
||
|
||
key = get_key() # 获取按键输入
|
||
|
||
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 key is None: # 如果没有按下键，重置速度
|
||
ls = 0
|
||
rs = 0
|
||
|
||
except KeyboardInterrupt:
|
||
print("停止机器人")
|
||
finally:
|
||
serial_port.close() |