import time
import struct
import threading
from dataclasses import dataclass
from typing import Tuple, Optional
import serial
----------------------------
CRC-8 (poly=0x07, init=0x00) 覆盖 [LEN, CMD, DATA...]
----------------------------
def crc8_0x07(data: bytes) -> int:
crc = 0x00
for b in data:
crc ^= b
for _ in range(8):
if crc & 0x80:
crc = ((crc << 1) & 0xFF) ^ 0x07
else:
crc = (crc << 1) & 0xFF
return crc & 0xFF
@dataclass
class EncoderState:
count: int
direction: int
speed_raw: int
class CH32V203UART:
CMD_GET_DUAL_ENCODER = 0x03
CMD_SET_ALL_PWM = 0x08
CMD_RESET_ALL_ENC = 0x0C
def __init__(self, port="/dev/ttyAMA0", baud=115200, timeout=0.2):
self.ser = serial.Serial(
port=port,
baudrate=baud,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=timeout,
)
self.ser.reset_input_buffer()
self.ser.reset_output_buffer()
self._lock = threading.Lock()
def close(self):
try:
self.ser.close()
except Exception:
pass
def _build_frame(self, cmd: int, data: bytes = b"") -> bytes:
length = 1 + len(data) + 1
body = bytes([length, cmd]) + data
crc = crc8_0x07(body)
return b"\xAA" + body + bytes([crc])
def _read_frame(self, deadline: float) -> Tuple[int, bytes]:
ser = self.ser
while time.monotonic() < deadline:
b = ser.read(1)
if not b or b[0] != 0xAA:
continue
lb = ser.read(1)
if not lb:
continue
L = lb[0]
payload = ser.read(L)
if len(payload) != L:
continue
# A: LEN包含CRC
if L >= 2:
calc = crc8_0x07(lb + payload[:-1])
if calc == payload[-1]:
cmd = payload[0]
data = payload[1:-1]
return cmd, data
# B: LEN不包含CRC
extra = ser.read(1)
if extra:
calc = crc8_0x07(lb + payload)
if calc == extra[0]:
cmd = payload[0]
data = payload[1:]
return cmd, data
raise TimeoutError("UART read_frame timeout")
def request(self, cmd: int, data: bytes = b"", resp_timeout: float = 0.25) -> Tuple[int, bytes]:
frame = self._build_frame(cmd, data)
deadline = time.monotonic() + resp_timeout
with self._lock:
self.ser.write(frame)
return self._read_frame(deadline)
def set_all_pwm(self, pwm1: int, pwm2: int, pwm3: int, pwm4: int) -> bool:
def clamp(x): return max(0, min(100, int(x)))
data = bytes([clamp(pwm1), clamp(pwm2), clamp(pwm3), clamp(pwm4)])
cmd, resp = self.request(self.CMD_SET_ALL_PWM, data=data, resp_timeout=0.25)
return (cmd == self.CMD_SET_ALL_PWM and len(resp) >= 1 and resp[0] == 0x01)
def reset_encoders(self) -> bool:
cmd, resp = self.request(self.CMD_RESET_ALL_ENC, data=b"", resp_timeout=0.3)
return (cmd == self.CMD_RESET_ALL_ENC and len(resp) >= 1 and resp[0] == 0x01)
def get_dual_encoder(self) -> Tuple[EncoderState, EncoderState]:
cmd, data = self.request(self.CMD_GET_DUAL_ENCODER, data=b"", resp_timeout=0.25)
if cmd != self.CMD_GET_DUAL_ENCODER or len(data) < 14:
raise RuntimeError("bad dual encoder response")
def parse7(buf: bytes) -> EncoderState:
count = struct.unpack(">i", buf[0:4])[0]
direction = buf[4]
speed_raw = struct.unpack(">h", buf[5:7])[0]
return EncoderState(count=count, direction=direction, speed_raw=speed_raw)
return parse7(data[0:7]), parse7(data[7:14])
class CarOpenLoop:
"""
开环小车控制（无PID）
固化你的电机方向/通道：
左轮：PWM4 正转 / PWM3 反转
右轮：PWM1 正转 / PWM2 反转
"""
def __init__(self, port="/dev/ttyAMA0", baud=115200):
self.hw = CH32V203UART(port=port, baud=baud, timeout=0.2)
def close(self):
self.stop()
self.hw.close()
def __enter__(self):
return self
def __exit__(self, exc_type, exc, tb):
self.close()
# ---- 底层：给左右轮有符号PWM（-100..100） ----
def set_wheel_pwm(self, left: float, right: float) -> bool:
left = max(-100.0, min(100.0, float(left)))
right = max(-100.0, min(100.0, float(right)))
# 左轮：PWM4正 / PWM3反
if left >= 0:
pwm4 = int(abs(left))
pwm3 = 0
else:
pwm4 = 0
pwm3 = int(abs(left))
# 右轮：PWM1正 / PWM2反
if right >= 0:
pwm1 = int(abs(right))
pwm2 = 0
else:
pwm1 = 0
pwm2 = int(abs(right))
return self.hw.set_all_pwm(pwm1, pwm2, pwm3, pwm4)
def stop(self):
self.set_wheel_pwm(0, 0)
# ---- 常用动作 ----
def forward(self, pwm: float = 20):
"""前进：两轮同向同PWM"""
self.set_wheel_pwm(abs(pwm), abs(pwm))
def backward(self, pwm: float = 20):
"""后退：两轮反向同PWM"""
self.set_wheel_pwm(-abs(pwm), -abs(pwm))
def turn_left(self, pwm: float = 20, ratio: float = 0.5):
"""
差速左转（弧线）：左轮慢，右轮快
ratio: 0~1，越小转得越急
"""
ratio = max(0.0, min(1.0, ratio))
self.set_wheel_pwm(abs(pwm) * ratio, abs(pwm))
def turn_right(self, pwm: float = 20, ratio: float = 0.5):
"""差速右转（弧线）：右轮慢，左轮快"""
ratio = max(0.0, min(1.0, ratio))
self.set_wheel_pwm(abs(pwm), abs(pwm) * ratio)
def rotate_left(self, pwm: float = 20):
"""原地左旋：左轮后退，右轮前进"""
p = abs(pwm)
self.set_wheel_pwm(-p, p)
def rotate_right(self, pwm: float = 20):
"""原地右旋：左轮前进，右轮后退"""
p = abs(pwm)
self.set_wheel_pwm(p, -p)
# ---- 可选：读编码器（仅显示/调试，不闭环） ----
def read_encoders(self) -> Tuple[EncoderState, EncoderState]:
return self.hw.get_dual_encoder()
def reset_encoders(self) -> bool:
return self.hw.reset_encoders()
if name == "main":
with CarOpenLoop(port="/dev/ttyAMA0") as car:
car.reset_encoders()
car.forward(20)
time.sleep(2)
car.turn_left(pwm=22, ratio=0.4)
time.sleep(1.5)
car.turn_right(pwm=22, ratio=0.4)
time.sleep(1.5)
car.rotate_left(20)
time.sleep(1.2)
car.stop()
time.sleep(0.5)
e1, e2 = car.read_encoders()
print("enc1:", e1, "enc2:", e2)
网站备案号:ICP备16046599号-1