189 lines
5.1 KiB
Python
189 lines
5.1 KiB
Python
# 调试小车，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()
|