forked from logzhan/RobotHardware-UESTC
modify(UPbot-Tools): 添加了win/linux测试程序，以及过程记录文档
parent
0103fa823d
commit
3c5be485f0
|
|
@ -3,4 +3,6 @@
|
|||
*.obj
|
||||
*.crf
|
||||
/History/
|
||||
/Output/
|
||||
/Output/
|
||||
.idea/
|
||||
__pycache__/
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 2.9 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 62 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 3.7 MiB |
|
|
@ -0,0 +1,188 @@
|
|||
# 调试小车，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()
|
||||
|
|
@ -0,0 +1,162 @@
|
|||
# 调试小车，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()
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
## 依赖问题
|
||||
1. 出现打不开串口或某类没有某个属性的问题，考虑是不是`serial`库装错了，要装的是`pyserial`而不是`serial`
|
||||
2. 串口没有写对，如应该是`COM9`，写的却是`COM6`
|
||||
|
||||
|
||||
|
||||
```
|
||||
pip install pyserial
|
||||
```
|
||||
|
||||
## ST-Link 连接
|
||||
|
||||
对应接就行（3v3、SWO、SWCLK、GND）
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## Keil烧录显示设备不匹配
|
||||
可能是`Keil`版本问题，在`5.4.0`上显示的错误，但是`5.2.1`就可以正常烧录
|
||||
|
||||
|
||||
## 正常启动但没有IMU数据
|
||||
选择界面中IMU型号为`gy87`，再点设置即可。
|
||||
Loading…
Reference in New Issue