惊心动魄的小车调试呀
Maixcam
题目一和二
这些题目都要改动一下lab
from maix import image, camera, display, app, audio, time, uart, touchscreen
import math
import struct
camWidth = 320
ts = touchscreen.TouchScreen()
disp = display.Display()
idStart = 0
idEnd = 4
serial_dev = uart.UART("/dev/ttyS0", 115200)
time.sleep_ms(100)
Threshold = [[0, 40, -5, 5, -5, 5]] # 更宽松的 L/A/B 范围
class LineFollower:
def __init__(self, img_width=320, img_height=240,
lost_frame_threshold=5, recovery_frame_threshold=3,
min_area=256, max_width_ratio=0.4, edge_margin=0.0,
proximity_limit=60,
# 高灵敏度参数（丢线时上 ROI 专用）
sensitive_min_area=64,
sensitive_max_width_ratio=0.2,
sensitive_edge_margin=0.00):
self.img_w = img_width
self.img_h = img_height
# ROI 百分比定义（上/中/下）
rois_pct = [
(0, 0.40, 1.0, 0.2, 0.2), # 上（最远）
(0, 0.60, 1.0, 0.2, 0.4), # 中
(0, 0.80, 1.0, 0.2, 0.9) # 下（最近）
]
self.rois = []
for (xp, yp, wp, hp, wgt) in rois_pct:
x = int(xp * img_width)
y = int(yp * img_height)
w = int(wp * img_width)
h = int(hp * img_height)
self.rois.append((x, y, w, h, wgt))
self.weight_sum = sum(r[4] for r in self.rois)
# 各 ROI 中心 Y 坐标（用于外推）
self.roi_centers_y = [y + h//2 for (_, y, _, h, _) in self.rois]
# 内部状态
self.line_rho = 0
self.last_rho = img_width // 2
self.lost_count = 0
self.recovery_count = 0
self.lost_frame_cnt = 0
self.recovery_frame_cnt = 0
self.confirmed_lost = False
# 常规参数
self.lost_frame_threshold = lost_frame_threshold
self.recovery_frame_threshold = recovery_frame_threshold
self.min_area = min_area
self.max_width_ratio = max_width_ratio
self.edge_margin = edge_margin
self.proximity_limit = proximity_limit
# 高灵敏度参数（丢线时上 ROI 专用）
self.sensitive_min_area = sensitive_min_area
self.sensitive_max_width_ratio = sensitive_max_width_ratio
self.sensitive_edge_margin = sensitive_edge_margin
self.draw_debug = True
# ---------- 辅助函数 ----------
def _filter_blobs(self, blobs, roi_x, roi_w, min_area, max_w_ratio, edge_margin):
"""通用过滤：面积、宽度、边缘"""
valid = []
for b in blobs:
if b.area() < min_area:
continue
if b.w() > roi_w * max_w_ratio:
continue
if (b.x() < roi_x + roi_w * edge_margin or
(b.x() + b.w()) > roi_x + roi_w * (1 - edge_margin)):
continue
valid.append(b)
return valid
def _nearest_blob(self, blobs, target_x, limit=None):
if not blobs:
return None
if limit is None:
limit = self.proximity_limit
best = None
best_dist = float('inf')
for b in blobs:
dist = abs(b.cx() - target_x)
if dist < best_dist and dist <= limit:
best = b
best_dist = dist
return best
def _extrapolate_x(self, x_mid, y_mid, x_down, y_down, y_up):
dy = y_down - y_mid
if abs(dy) < 1e-6:
return x_down
return int(x_mid + (x_down - x_mid) * (y_up - y_mid) / dy)
# ---------- 主处理 ----------
def update(self, img):
# img.gaussian(3)
# try:
# img.clahe(clip_limit=2.0, tile_grid=(8,8))
# except:
# img.histeq()
line_blobs = [] # 用于绘制的实际色块
roi_blobs = [None, None, None] # 上中下最终选中的色块
# 获取各 ROI 的候选色块（常规灵敏度）
candidates = []
for idx, (x, y, w, h, wgt) in enumerate(self.rois):
blobs = img.find_blobs(Threshold, roi=(x, y, w, h),
merge=True, pixels_threshold=self.min_area)
valid = self._filter_blobs(blobs, x, w,
self.min_area,
self.max_width_ratio,
self.edge_margin)
candidates.append(valid)
# ---- 下 ROI：参考 last_rho ----
ref_down = self.last_rho
blob_d = self._nearest_blob(candidates[2], ref_down)
roi_blobs[2] = blob_d
# ---- 中 ROI：如果有下，则参考下中心，否则参考 last_rho ----
ref_mid = blob_d.cx() if blob_d else self.last_rho
blob_m = self._nearest_blob(candidates[1], ref_mid)
roi_blobs[1] = blob_m
# ---- 判断是否丢线（中、下同时无效）----
lose_now = (roi_blobs[2] is None and roi_blobs[1] is None)
# ---- 上 ROI：平时用外推/邻近，丢线时高灵敏度尽力查找 ----
if not lose_now:
# 正常情况：中或下至少有一个有效
if blob_m is not None and blob_d is not None:
# 两点一线外推
x_ext = self._extrapolate_x(blob_m.cx(), self.roi_centers_y[1],
blob_d.cx(), self.roi_centers_y[2],
self.roi_centers_y[0])
blob_u = self._nearest_blob(candidates[0], x_ext,
limit=self.proximity_limit * 1.5)
else:
# 只有一个有效（中或下）
ref_up = blob_m.cx() if blob_m else (blob_d.cx() if blob_d else self.last_rho)
blob_u = self._nearest_blob(candidates[0], ref_up)
roi_blobs[0] = blob_u
else:
# ---- 丢线状态：上 ROI 高灵敏度尽力查找线头 ----
blob_u = None
# 先尝试从常规候选里找（也许已经过滤出小块）
if candidates[0]:
# 取离 last_rho 最近的
blob_u = self._nearest_blob(candidates[0], self.last_rho,
limit=self.proximity_limit * 2.0)
# 如果常规候选没找到，用高灵敏度参数重新查找
if blob_u is None:
x, y, w, h, _ = self.rois[0]
blobs_sensitive = img.find_blobs(Threshold, roi=(x, y, w, h),
merge=True,
pixels_threshold=self.sensitive_min_area)
valid_s = self._filter_blobs(blobs_sensitive, x, w,
min_area=self.sensitive_min_area,
max_w_ratio=self.sensitive_max_width_ratio,
edge_margin=self.sensitive_edge_margin)
if valid_s:
# 取面积最大或离 last_rho 最近的，这里用最近
blob_u = self._nearest_blob(valid_s, self.last_rho,
limit=self.proximity_limit * 2.5)
roi_blobs[0] = blob_u
# ---- 收集用于显示和加权 ----
centroid_sum = 0
weight_used = 0
for idx, b in enumerate(roi_blobs):
if b is not None:
line_blobs.append(b)
centroid_sum += b.cx() * self.rois[idx][4]
weight_used += self.rois[idx][4]
# ---- 连续帧计数 ----
if lose_now:
self.lost_frame_cnt += 1
self.recovery_frame_cnt = 0
else:
self.recovery_frame_cnt += 1
self.lost_frame_cnt = 0
is_lost_confirmed = (self.lost_frame_cnt >= self.lost_frame_threshold)
is_recovered = (self.recovery_frame_cnt >= self.recovery_frame_threshold)
if is_lost_confirmed and not self.confirmed_lost:
self.lost_count += 1
self.confirmed_lost = True
elif self.confirmed_lost and is_recovered:
self.recovery_count += 1
self.confirmed_lost = False
# ---- 计算最终线位置 ----
if not lose_now:
if weight_used > 0:
self.line_rho = int(centroid_sum / weight_used)
else:
self.line_rho = self.last_rho
self.last_rho = self.line_rho
else:
# 丢线时，有上 ROI 就用上，没有则保持
if roi_blobs[0] is not None:
self.line_rho = roi_blobs[0].cx()
self.last_rho = self.line_rho
line_blobs.append(roi_blobs[0]) # 确保绘制出来
else:
self.line_rho = self.last_rho
offset = self.line_rho - self.img_w // 2
# ---- 调试绘制 ----
if self.draw_debug:
for b in line_blobs:
img.draw_rect(b.x(), b.y(), b.w(), b.h(), color=image.COLOR_WHITE)
img.draw_cross(b.cx(), b.cy(),
image.Color.from_rgb(255, 255, 255),
size=5, thickness=1)
img.draw_string(0, 30,
'rho=%d off=%d lost=%d' % (self.line_rho, offset, self.lost_count),
image.COLOR_WHITE)
img.draw_string(0, 50,
'lost_cnt=%d rec=%d' % (self.lost_frame_cnt, self.recovery_count),
image.COLOR_WHITE)
return offset, self.lost_count, self.recovery_count
# ========== 外部主循环示例 ==========
follower = LineFollower(img_width=320, img_height=240)
# 速度控制参数
BASE_SPEED = 33
TURN_GAIN = BASE_SPEED * 0.0045 * 320 / camWidth
MAX_SPEED = BASE_SPEED
MIN_SPEED = 0
# ========== 巡线模式函数 ==========
def calc_speed(error):
"""计算左右轮速度"""
left_speed = BASE_SPEED + TURN_GAIN * error
right_speed = BASE_SPEED - TURN_GAIN * error
left_speed = max(MIN_SPEED, min(MAX_SPEED, left_speed))
right_speed = max(MIN_SPEED, min(MAX_SPEED, right_speed))
return int(left_speed), int(right_speed)
def send_speed_to_stm32(left_speed, right_speed):
"""打包发送6字节协议到STM32"""
left_u16 = left_speed & 0xFFFF
right_u16 = right_speed & 0xFFFF
left_low = left_u16 & 0xFF
left_high = (left_u16 >> 8) & 0xFF
right_low = right_u16 & 0xFF
right_high = (right_u16 >> 8) & 0xFF
checksum = (0xAA + left_low + left_high + right_low + right_high) & 0xFF
packet = bytes([0xAA, left_low, left_high, right_low, right_high, checksum])
serial_dev.write(packet)
return packet
# 提前创建好播放器，避免每次调用都重复初始化，提升响应速度
_player = audio.Player(sample_rate=48000, format=audio.Format.FMT_S16_LE, channel=1)
_player.volume(100) # 音量 0-100，可按需调整
def beep(freq=2000, duration=0.3):
"""
使用板载喇叭播放指定频率和时长的提示音（阻塞式）
:param freq: 频率，单位 Hz，默认 2000
:param duration: 持续时间，单位 秒，默认 0.8
"""
# 1. 根据频率和时长生成 PCM 音频数据
sample_rate = 48000
num_samples = int(sample_rate * duration)
amplitude = 32767 # 最大音量
pcm = bytearray()
for i in range(num_samples):
t = i / sample_rate
sample = amplitude * math.sin(2 * math.pi * freq * t)
pcm.extend(struct.pack('<h', int(sample)))
# 2. 播放生成的 PCM 数据（非阻塞，硬件立即开始输出）
_player.play(bytes(pcm))
# 3. 阻塞等待播放结束
time.sleep_ms(int(duration * 1000))
def detectApriltag():
cam = camera.Camera(320, 240)
families = image.ApriltagFamilies.TAG36H11
print("开始识别 AprilTag，按 Ctrl+C 或开发板上的 Home 键退出...")
while not app.need_exit():
try:
img = cam.read()
if img is None:
continue
apriltags = img.find_apriltags(families=families)
if apriltags:
ids = [tag.id() for tag in apriltags]
print(ids)
img.draw_string(10, 10, "IDs: " + ",".join(map(str, ids)),
scale=3, color=image.COLOR_RED)
for tag in apriltags:
corners = tag.corners()
for i in range(4):
img.draw_line(
corners[i][0], corners[i][1],
corners[(i + 1) % 4][0], corners[(i + 1) % 4][1],
color=image.COLOR_GREEN, thickness=2
)
disp.show(img)
beep() # 确保 beep 函数已定义，否则会报错
print("识别完成，程序退出。")
del cam # 先释放摄像头再返回
return ids # 返回所有 ID 的列表
else:
disp.show(img)
except Exception as e:
print(f"识别出错: {e}")
del cam
return [] # 如果用户主动退出，返回空列表
def is_in_button(x, y, btn_pos):
return x > btn_pos[0] * 2 and x < btn_pos[0] * 2 + btn_pos[2] * 2 and y > btn_pos[1] * 2 and y < btn_pos[1] * 2 + btn_pos[3] * 2
if __name__ == "__main__":
found_id = 0
delay = 60
while True:
ids = detectApriltag()
if not ids:
print("未检测到任何标签或用户主动退出，程序结束。")
break
for tag_id in ids:
if idStart <= tag_id <= idEnd:
found_id = tag_id
break
if found_id is not None:
print(f"找到范围内 ID: {found_id} (范围 {idStart}~{idEnd})")
break
else:
print(f"检测到的 ID {ids} 不在范围 [{idStart}, {idEnd}] 内，重新识别...")
cam = camera.Camera(320, 240)
loop_label = f"LOOP: {found_id}"
exit_label = " R U N "
size = image.string_size(exit_label, scale = 3.0, thickness = 2)
exit_btn_pos = [0, 0, 8*2 + size.width(), 30 + size.height()]
while True:
img = cam.read()
img.draw_string(8, 30, exit_label, image.COLOR_WHITE, scale = 3.0, thickness = 2)
img.draw_rect(exit_btn_pos[0], exit_btn_pos[1], exit_btn_pos[2], exit_btn_pos[3], image.COLOR_WHITE, 2)
img.draw_string(8, 75, loop_label, image.COLOR_WHITE, scale = 3.0, thickness = 2)
x, y, pressed = ts.read()
if is_in_button(x, y, exit_btn_pos):
break
disp.show(img)
while True:
t = time.time_ms()
img = cam.read()
if img is None:
continue
offset, lost_cnt, recovery_count = follower.update(img)
l, r = calc_speed(offset)
if recovery_count > found_id * 3 - 1:
delay -= 1
if delay > 0:
delay -= 1
send_speed_to_stm32(l, -r)
else:
send_speed_to_stm32(0, 0)
else:
send_speed_to_stm32(l, -r)
disp.show(img)
print("FPS =", int(1000 / (time.time_ms() - t)), "Offset:", offset, "Lost:", lost_cnt)
题目三
from maix import image, camera, display, app, audio, time, uart, touchscreen, ahrs
import math
import struct
from maix.ext_dev import imu
camWidth = 320
ts = touchscreen.TouchScreen()
disp = display.Display()
idStart = 0
idEnd = 4
serial_dev = uart.UART("/dev/ttyS0", 115200)
time.sleep_ms(100)
Threshold = [[0, 40, -6, 6, -10, 6]] # 更宽松的 L/A/B 范围
kp = 2
ki = 0.01
calibrate_first = False
sensor = imu.IMU("", mode=imu.Mode.DUAL,
acc_scale=imu.AccScale.ACC_SCALE_2G,
acc_odr=imu.AccOdr.ACC_ODR_1000,
gyro_scale=imu.GyroScale.GYRO_SCALE_256DPS,
gyro_odr=imu.GyroOdr.GYRO_ODR_8000)
ahrs_filter = ahrs.MahonyAHRS(kp, ki)
if calibrate_first or not sensor.calib_gyro_exists():
print("\n\nNeed calibrate fisrt")
print("now calibrate, please !! don't move !! device, wait for 10 seconds")
sensor.calib_gyro(10000)
else:
sensor.load_calib_gyro()
class LineFollower:
def __init__(self, img_width=320, img_height=240,
lost_frame_threshold=5, recovery_frame_threshold=3,
min_area=256, max_width_ratio=0.70, edge_margin=0.0,
proximity_limit=60,
# 高灵敏度参数（丢线时上 ROI 专用）
sensitive_min_area=30,
sensitive_max_width_ratio=0.8,
sensitive_edge_margin=0.0):
self.img_w = img_width
self.img_h = img_height
# ROI 百分比定义（上/中/下）
rois_pct = [
(0.4, 0.40, 0.6, 0.2, 0.2), # 上（最远）
(0, 0.60, 1.0, 0.2, 0.5), # 中
(0, 0.80, 1.0, 0.2, 0.8) # 下（最近）
]
self.rois = []
for (xp, yp, wp, hp, wgt) in rois_pct:
x = int(xp * img_width)
y = int(yp * img_height)
w = int(wp * img_width)
h = int(hp * img_height)
self.rois.append((x, y, w, h, wgt))
self.weight_sum = sum(r[4] for r in self.rois)
# 各 ROI 中心 Y 坐标（用于外推）
self.roi_centers_y = [y + h//2 for (_, y, _, h, _) in self.rois]
# 内部状态
self.line_rho = 0
self.last_rho = img_width // 2
self.lost_count = 0
self.recovery_count = 0
self.lost_frame_cnt = 0
self.recovery_frame_cnt = 0
self.confirmed_lost = False
# 常规参数
self.lost_frame_threshold = lost_frame_threshold
self.recovery_frame_threshold = recovery_frame_threshold
self.min_area = min_area
self.max_width_ratio = max_width_ratio
self.edge_margin = edge_margin
self.proximity_limit = proximity_limit
# 高灵敏度参数（丢线时上 ROI 专用）
self.sensitive_min_area = sensitive_min_area
self.sensitive_max_width_ratio = sensitive_max_width_ratio
self.sensitive_edge_margin = sensitive_edge_margin
self.draw_debug = True
# ---------- 辅助函数 ----------
def _filter_blobs(self, blobs, roi_x, roi_w, min_area, max_w_ratio, edge_margin):
"""通用过滤：面积、宽度、边缘"""
valid = []
for b in blobs:
if b.area() < min_area:
continue
if b.w() > roi_w * max_w_ratio:
continue
if (b.x() < roi_x + roi_w * edge_margin or
(b.x() + b.w()) > roi_x + roi_w * (1 - edge_margin)):
continue
valid.append(b)
return valid
def _nearest_blob(self, blobs, target_x, limit=None):
if not blobs:
return None
if limit is None:
limit = self.proximity_limit
best = None
best_dist = float('inf')
for b in blobs:
dist = abs(b.cx() - target_x)
if dist < best_dist and dist <= limit:
best = b
best_dist = dist
return best
def _extrapolate_x(self, x_mid, y_mid, x_down, y_down, y_up):
dy = y_down - y_mid
if abs(dy) < 1e-6:
return x_down
return int(x_mid + (x_down - x_mid) * (y_up - y_mid) / dy)
# ---------- 主处理 ----------
def update(self, img):
# img.gaussian(3)
# try:
# img.clahe(clip_limit=2.0, tile_grid=(8,8))
# except:
# img.histeq()
line_blobs = [] # 用于绘制的实际色块
roi_blobs = [None, None, None] # 上中下最终选中的色块
# 获取各 ROI 的候选色块（常规灵敏度）
candidates = []
for idx, (x, y, w, h, wgt) in enumerate(self.rois):
blobs = img.find_blobs(Threshold, roi=(x, y, w, h),
merge=True, pixels_threshold=self.min_area)
valid = self._filter_blobs(blobs, x, w,
self.min_area,
self.max_width_ratio,
self.edge_margin)
candidates.append(valid)
# ---- 下 ROI：参考 last_rho ----
ref_down = self.last_rho
blob_d = self._nearest_blob(candidates[2], ref_down)
roi_blobs[2] = blob_d
# ---- 中 ROI：如果有下，则参考下中心，否则参考 last_rho ----
ref_mid = blob_d.cx() if blob_d else self.last_rho
blob_m = self._nearest_blob(candidates[1], ref_mid)
roi_blobs[1] = blob_m
# ---- 判断是否丢线（中、下同时无效）----
lose_now = (roi_blobs[2] is None and roi_blobs[1] is None)
lose_down = (roi_blobs[2] is None)
# ---- 上 ROI：平时用外推/邻近，丢线时高灵敏度尽力查找 ----
if not lose_now:
# 正常情况：中或下至少有一个有效
if blob_m is not None and blob_d is not None:
# 两点一线外推
x_ext = self._extrapolate_x(blob_m.cx(), self.roi_centers_y[1],
blob_d.cx(), self.roi_centers_y[2],
self.roi_centers_y[0])
blob_u = self._nearest_blob(candidates[0], x_ext,
limit=self.proximity_limit * 1.5)
else:
# 只有一个有效（中或下）
ref_up = blob_m.cx() if blob_m else (blob_d.cx() if blob_d else self.last_rho)
blob_u = self._nearest_blob(candidates[0], ref_up)
roi_blobs[0] = blob_u
else:
# ---- 丢线状态：上 ROI 高灵敏度尽力查找线头 ----
blob_u = None
# 先尝试从常规候选里找（也许已经过滤出小块）
if candidates[0]:
# 取离 last_rho 最近的
blob_u = self._nearest_blob(candidates[0], self.last_rho,
limit=self.proximity_limit * 2.0)
# 如果常规候选没找到，用高灵敏度参数重新查找
if blob_u is None:
x, y, w, h, _ = self.rois[0]
blobs_sensitive = img.find_blobs(Threshold, roi=(x, y, w, h),
merge=True,
pixels_threshold=self.sensitive_min_area)
valid_s = self._filter_blobs(blobs_sensitive, x, w,
min_area=self.sensitive_min_area,
max_w_ratio=self.sensitive_max_width_ratio,
edge_margin=self.sensitive_edge_margin)
if valid_s:
# 取面积最大或离 last_rho 最近的，这里用最近
blob_u = self._nearest_blob(valid_s, self.last_rho,
limit=self.proximity_limit * 2.5)
roi_blobs[0] = blob_u
# ---- 收集用于显示和加权 ----
centroid_sum = 0
weight_used = 0
for idx, b in enumerate(roi_blobs):
if b is not None:
line_blobs.append(b)
centroid_sum += b.cx() * self.rois[idx][4]
weight_used += self.rois[idx][4]
# ---- 连续帧计数 ----
if lose_now:
self.lost_frame_cnt += 1
self.recovery_frame_cnt = 0
else:
self.recovery_frame_cnt += 1
self.lost_frame_cnt = 0
is_lost_confirmed = (self.lost_frame_cnt >= self.lost_frame_threshold)
is_recovered = (self.recovery_frame_cnt >= self.recovery_frame_threshold)
if is_lost_confirmed and not self.confirmed_lost:
self.lost_count += 1
self.confirmed_lost = True
elif self.confirmed_lost and is_recovered:
self.recovery_count += 1
self.confirmed_lost = False
# ---- 计算最终线位置 ----
if not lose_now:
if weight_used > 0:
self.line_rho = int(centroid_sum / weight_used)
else:
self.line_rho = self.last_rho
self.last_rho = self.line_rho
else:
# 丢线时，有上 ROI 就用上，没有则保持
if roi_blobs[0] is not None:
self.line_rho = roi_blobs[0].cx()
self.last_rho = self.line_rho
line_blobs.append(roi_blobs[0]) # 确保绘制出来
else:
self.line_rho = self.last_rho
offset = self.line_rho - self.img_w // 2
# ---- 调试绘制 ----
if self.draw_debug:
for b in line_blobs:
img.draw_rect(b.x(), b.y(), b.w(), b.h(), color=image.COLOR_WHITE)
img.draw_cross(b.cx(), b.cy(),
image.Color.from_rgb(255, 255, 255),
size=5, thickness=1)
img.draw_string(0, 30,
'rho=%d off=%d lost=%d' % (self.line_rho, offset, self.lost_count),
image.COLOR_WHITE)
img.draw_string(0, 50,
'lost_cnt=%d rec=%d' % (self.lost_frame_cnt, self.recovery_count),
image.COLOR_WHITE)
return offset, self.lost_count, self.recovery_count, lose_down
# ========== 外部主循环示例 ==========
follower = LineFollower(img_width=320, img_height=240)
# 速度控制参数
BASE_SPEED = 20
TURN_GAIN = BASE_SPEED * 0.009 * 320 / camWidth
MAX_SPEED = BASE_SPEED
MIN_SPEED = 0
# ========== 巡线模式函数 ==========
def calc_speed(error):
"""计算左右轮速度"""
left_speed = BASE_SPEED + TURN_GAIN * error
right_speed = BASE_SPEED - TURN_GAIN * error
left_speed = max(MIN_SPEED, min(MAX_SPEED, left_speed))
right_speed = max(MIN_SPEED, min(MAX_SPEED, right_speed))
return int(left_speed), int(right_speed)
def send_speed_to_stm32(left_speed, right_speed):
"""打包发送6字节协议到STM32"""
left_u16 = left_speed & 0xFFFF
right_u16 = right_speed & 0xFFFF
left_low = left_u16 & 0xFF
left_high = (left_u16 >> 8) & 0xFF
right_low = right_u16 & 0xFF
right_high = (right_u16 >> 8) & 0xFF
checksum = (0xAA + left_low + left_high + right_low + right_high) & 0xFF
packet = bytes([0xAA, left_low, left_high, right_low, right_high, checksum])
serial_dev.write(packet)
return packet
# 提前创建好播放器，避免每次调用都重复初始化，提升响应速度
_player = audio.Player(sample_rate=48000, format=audio.Format.FMT_S16_LE, channel=1)
_player.volume(100) # 音量 0-100，可按需调整
def beep(freq=2000, duration=0.3):
"""
使用板载喇叭播放指定频率和时长的提示音（阻塞式）
:param freq: 频率，单位 Hz，默认 2000
:param duration: 持续时间，单位 秒，默认 0.8
"""
# 1. 根据频率和时长生成 PCM 音频数据
sample_rate = 48000
num_samples = int(sample_rate * duration)
amplitude = 32767 # 最大音量
pcm = bytearray()
for i in range(num_samples):
t = i / sample_rate
sample = amplitude * math.sin(2 * math.pi * freq * t)
pcm.extend(struct.pack('<h', int(sample)))
# 2. 播放生成的 PCM 数据（非阻塞，硬件立即开始输出）
_player.play(bytes(pcm))
# 3. 阻塞等待播放结束
time.sleep_ms(int(duration * 1000))
def scan_apriltag(resolution=(2560, 1440), win_w=400, win_h=300, step=200, scan_y=0):
"""
拍摄一张照片，从左到右滑动窗口检测 AprilTag。
完全内部管理摄像头，不依赖外部 camera 对象。
参数:
resolution: (width, height) 摄像头采集分辨率，默认 (2560,1440)
win_w, win_h: 滑动窗口尺寸，默认 400x300
step: 水平滑动步长（像素），默认 200
scan_y: 垂直起始位置（顶部偏移），默认 0
返回:
若检测到: dict {
'id': int,
'family': str,
'corners': [(x1,y1), (x2,y2), (x3,y3), (x4,y4)], # 在原图上的坐标
'center': (cx, cy)
}
若未检测到: None
"""
# ---------- 1. 内部初始化摄像头（指定分辨率）----------
cam = camera.Camera(resolution[0], resolution[1])
time.sleep(0.5) # 等待传感器稳定
full_img = cam.read() # 拍摄一帧
# 拍摄完成后立即释放摄像头资源（避免占用内存和冲突）
del cam
# 注意：某些固件可能没有显式的 close 方法，del 会触发 __del__ 释放
# ---------- 2. 滑动窗口扫描 ----------
width, height = resolution
max_x = width - win_w
x_start = 0
detected = False
result = None
while x_start <= max_x and not detected:
# 裁剪当前窗口
window_img = full_img.crop(x_start, scan_y, win_w, win_h)
# 实时显示当前扫描窗口（便于观察进度）
disp.show(window_img)
tags = window_img.find_apriltags(families=image.ApriltagFamilies.TAG36H11)
if tags:
detected = True
a = tags[0] # 只取第一个
# 窗口内坐标转换到原图坐标
corners_win = a.corners()
orig_corners = [(int(cx + x_start), int(cy + scan_y)) for (cx, cy) in corners_win]
cx_win = int(a.x() + a.w() / 2)
cy_win = int(a.y() + a.h() / 2)
orig_center = (cx_win + x_start, cy_win + scan_y)
# 在原图上绘制标记
for i in range(4):
x1, y1 = orig_corners[i]
x2, y2 = orig_corners[(i+1)%4]
full_img.draw_line(x1, y1, x2, y2, image.COLOR_RED, thickness=2)
full_img.draw_circle(orig_center[0], orig_center[1], 5, image.COLOR_GREEN, thickness=2)
ox_text = int(a.x() + a.w() + x_start)
oy_text = int(a.y() + scan_y)
full_img.draw_rect(ox_text, oy_text, 100, 30, image.COLOR_BLACK, thickness=-1)
full_img.draw_string(ox_text, oy_text, f"id: {a.id()}", image.COLOR_WHITE)
full_img.draw_string(ox_text, oy_text+15, f"family: {a.family()}", image.COLOR_WHITE)
result = a.id()
# result = {
# 'id': a.id(),
# 'family': a.family(),
# 'corners': orig_corners,
# 'center': orig_center
# }
break
else:
x_start += step
# ---------- 3. 显示最终结果（缩放至屏幕）----------
if not detected:
full_img.draw_string(50, 50, "No AprilTag found", image.COLOR_RED)
# 缩放图像以适应显示屏
if full_img.width() != disp.width() or full_img.height() != disp.height():
full_img = full_img.resize(disp.width(), disp.height())
disp.show(full_img)
return result
def clamp_abs(value: float) -> float:
if value > 0:
return max(value, 1.0)
elif value < 0:
return min(value, -1.0)
else:
# value == 0 的情况，按规则归为 1.0
return 1.0
def execuseTurn(angle: float = 0.0, speed: int = 10, offset: float = 0.5):
"""
相对旋转指定角度（单位：度）
angle > 0 顺时针（或取决于你的底盘定义）
speed 最大速度（int，绝对值 ≤ speed）
offset 允许的停止误差范围（度）
"""
# ---- 初始化，获得当前朝向 ----
data = sensor.read_all(calib_gryo=True, radian=True)
t0 = time.ticks_s()
attitude = ahrs_filter.get_angle(data.acc, data.gyro, data.mag, 0.01, radian=False)
initial_yaw = attitude.z # 记录起始偏航角
last_time = t0
# 计算目标绝对角度，限制在 [-180, 180)
target_absolute = initial_yaw + angle
while target_absolute >= 180:
target_absolute -= 360
while target_absolute < -180:
target_absolute += 360
# 比例系数，保持 30° 对应满速的设定（可调）
Kp = speed / 30.0
while True:
data = sensor.read_all(calib_gryo=True, radian=True)
t = time.ticks_s()
dt = t - last_time
last_time = t
attitude = ahrs_filter.get_angle(data.acc, data.gyro, data.mag, dt, radian=False)
current_yaw = attitude.z
print(f"pitch: {attitude.x:8.2f}, roll: {attitude.y:8.2f}, yaw: {current_yaw:8.2f}, dt: {int(dt*1000):3d}ms, temp: {data.temp:.1f}")
# 误差 = 目标绝对角度 - 当前偏航，并规范到 [-180, 180)
error = target_absolute - current_yaw
while error > 180:
error -= 360
while error < -180:
error += 360
# 进入允许范围则停车退出
if abs(error) <= offset:
send_speed_to_stm32(0, 0)
print(f"旋转完成，剩余误差: {error:.2f}°")
break
# 比例速度，误差大速度快，误差小速度慢
motor_speed = Kp * error
if motor_speed > speed:
motor_speed = speed
elif motor_speed < -speed:
motor_speed = -speed
motor_speed = clamp_abs(motor_speed)
left_speed = int(motor_speed)
right_speed = int(motor_speed) # 右轮取反，实现原地旋转
send_speed_to_stm32(-left_speed, -right_speed)
def blindMove(duration = 1000, speed = 20):
send_speed_to_stm32(speed, -speed)
send_speed_to_stm32(speed, -speed)
send_speed_to_stm32(speed, -speed)
send_speed_to_stm32(speed, -speed)
send_speed_to_stm32(speed, -speed)
send_speed_to_stm32(speed, -speed)
time.sleep_ms(duration / 2)
def is_in_button(x, y, btn_pos):
return x > btn_pos[0] * 2 and x < btn_pos[0] * 2 + btn_pos[2] * 2 and y > btn_pos[1] * 2 and y < btn_pos[1] * 2 + btn_pos[3] * 2
if __name__ == "__main__":
# while True:
# execuseTurn(90)
# time.sleep_ms(1000)
# execuseTurn(-90)
# time.sleep_ms(1000)
found_id = 0
isLabelRequired = 0
tag01 = 0
tag02 = 0
isMode = 0
cam = camera.Camera(320, 240)
change_label = "CHANGE"
size0 = image.string_size(change_label, scale=1.0, thickness=1)
change_btn_pos = [150, 0, 8*2 + size0.width(), size0.height()]
odd_label = " O D D "
even_label = " E V E N "
size1 = image.string_size(odd_label, scale=3.0, thickness=2)
odd_btn_pos = [8, 30, 8*2 + size1.width(), 30 + size1.height()]
size2 = image.string_size(even_label, scale=3.0, thickness=2)
even_btn_pos = [8, odd_btn_pos[3] + 50, 8*2 + size2.width(), 30 + size2.height()] # 与 odd 底部留 20 间隙
left_label = " L E F T "
right_label = " R I G H T "
size3 = image.string_size(left_label, scale=3.0, thickness=2)
left_btn_pos = [8, 30, 8*2 + size3.width(), 30 + size3.height()]
size4 = image.string_size(right_label, scale=3.0, thickness=2)
right_btn_pos = [8, left_btn_pos[3] + 50, 8*2 + size4.width(), 30 + size4.height()] # 与 odd 底部留 20 间隙
isChangePressed = False
while True:
img = cam.read()
img.draw_string(150, 0, change_label, image.COLOR_WHITE, scale = 1.0, thickness = 1)
img.draw_rect(change_btn_pos[0], change_btn_pos[1], change_btn_pos[2], change_btn_pos[3], image.COLOR_RED, 1)
if isMode == 0 :
# 画 Odd 按钮文字和矩形
img.draw_string(odd_btn_pos[0], odd_btn_pos[1] + 15, odd_label, image.COLOR_WHITE, scale=3.0, thickness=2)
img.draw_rect(odd_btn_pos[0], odd_btn_pos[1], odd_btn_pos[2], odd_btn_pos[3], image.COLOR_WHITE, 2)
# 画 Even 按钮文字和矩形（两者位置对齐）
img.draw_string(even_btn_pos[0], even_btn_pos[1] + 15, even_label, image.COLOR_WHITE, scale=3.0, thickness=2)
img.draw_rect(even_btn_pos[0], even_btn_pos[1], even_btn_pos[2], even_btn_pos[3], image.COLOR_WHITE, 2)
else:
# 画 Odd 按钮文字和矩形
img.draw_string(left_btn_pos[0], left_btn_pos[1] + 15, left_label, image.COLOR_WHITE, scale=3.0, thickness=2)
img.draw_rect(left_btn_pos[0], left_btn_pos[1], left_btn_pos[2], left_btn_pos[3], image.COLOR_WHITE, 2)
# 画 Even 按钮文字和矩形（两者位置对齐）
img.draw_string(right_btn_pos[0], right_btn_pos[1] + 15, right_label, image.COLOR_WHITE, scale=3.0, thickness=2)
img.draw_rect(right_btn_pos[0], right_btn_pos[1], right_btn_pos[2], right_btn_pos[3], image.COLOR_WHITE, 2)
x, y, pressed = ts.read()
if pressed and is_in_button(x, y, change_btn_pos) and isChangePressed is False:
isMode = 1 - isMode
isChangePressed = True
if pressed and is_in_button(x, y, odd_btn_pos):
isLabelRequired = 0
break
if pressed and is_in_button(x, y, even_btn_pos):
isLabelRequired = 1
break
if pressed and is_in_button(x, y, left_btn_pos):
isLabelRequired = 0
break
if pressed and is_in_button(x, y, right_btn_pos):
isLabelRequired = 1
break
if not pressed:
isChangePressed = False
disp.show(img)
del cam
execuseTurn(-90)
while True:
ret = scan_apriltag()
if ret is not None:
img02 = image.Image(disp.width(), disp.height())
found_id = ret
label = f":{ret}"
img02.draw_string(8, 30, label, image.COLOR_WHITE, scale = 20.0, thickness = 14)
disp.show(img02)
tag01 = int(ret)
beep()
break
execuseTurn(90)
cam = camera.Camera(320, 240)
while True:
t = time.time_ms()
img = cam.read()
if img is None:
continue
offset, lost_cnt, recovery_count, lose_down = follower.update(img)
l, r = calc_speed(offset)
if lost_cnt > 0:
send_speed_to_stm32(l, -r)
time.sleep_ms(600)
send_speed_to_stm32(0, 0)
break
else:
send_speed_to_stm32(l, -r)
disp.show(img)
print("FPS =", int(1000 / (time.time_ms() - t)), "Offset:", offset, "Lost:", lost_cnt)
del cam
while True:
ret = scan_apriltag()
if ret is not None:
img02 = image.Image(disp.width(), disp.height())
found_id = ret
label = f":{ret}"
img02.draw_string(8, 30, label, image.COLOR_WHITE, scale = 20.0, thickness = 14)
disp.show(img02)
tag02 = int(ret)
beep()
break
# blindMove(duration = 2000)
if isMode == 1:
if isLabelRequired == 1:
if tag02 % 2 == 1:
isLabelRequired = 0
elif tag01 % 2 == 0:
isLabelRequired = 1
if isLabelRequired == 1:
execuseTurn(-55)
blindMove(duration = 2300)
cam = camera.Camera(320, 240)
finishTimerMs = 500
while True:
t = time.time_ms()
img = cam.read()
if img is None:
continue
offset, lost_cnt, recovery_count , lose_down = follower.update(img)
l, r = calc_speed(offset)
if recovery_count > 0 and lost_cnt > recovery_count:
send_speed_to_stm32(l, -r)
send_speed_to_stm32(0, 0)
execuseTurn(83)
break
else:
send_speed_to_stm32(l, -r)
send_speed_to_stm32(l, -r)
disp.show(img)
print("FPS =", int(1000 / (time.time_ms() - t)), "Offset:", offset, "Lost:", lost_cnt)
blindMove(duration = 2600)
while True:
t = time.time_ms()
img = cam.read()
if img is None:
continue
offset, lost_cnt, recovery_count, lose_down = follower.update(img)
if lose_down is False:
finishTimerMs -= int((time.time_ms() - t))
lastTime = t
l, r = calc_speed(offset)
print(f"{finishTimerMs}")
if finishTimerMs < 0:
send_speed_to_stm32(0, 0)
break
send_speed_to_stm32(l, -r)
disp.show(img)
print("FPS =", int(1000 / (time.time_ms() - t)), "Offset:", offset, "Lost:", lost_cnt)
del cam
else:
finishTimerMs = 300
execuseTurn(-10)
blindMove(duration = 3500)
execuseTurn(85)
cam = camera.Camera(320, 240)
blindMove(duration = 200)
while True:
t = time.time_ms()
img = cam.read()
if img is None:
continue
offset, lost_cnt, recovery_count , lose_down = follower.update(img)
l, r = calc_speed(offset)
if recovery_count > 0 and lost_cnt > recovery_count:
send_speed_to_stm32(0, 0)
break
send_speed_to_stm32(l, -r)
disp.show(img)
print("FPS =", int(1000 / (time.time_ms() - t)), "Offset:", offset, "Lost:", lost_cnt)
execuseTurn(45)
blindMove(duration = 5500)
execuseTurn(-100)
while True:
t = time.time_ms()
img = cam.read()
if img is None:
continue
offset, lost_cnt, recovery_count, lose_down = follower.update(img)
if lose_down is False:
finishTimerMs -= int((time.time_ms() - t))
lastTime = t
l, r = calc_speed(offset)
print(f"{finishTimerMs}")
if finishTimerMs < 0:
send_speed_to_stm32(0, 0)
break
send_speed_to_stm32(l, -r)
disp.show(img)
print("FPS =", int(1000 / (time.time_ms() - t)), "Offset:", offset, "Lost:", lost_cnt)
del cam
STM32底座
main.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "motor.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
// UART3 ر
uint8_t rx_buffer; // ֽڽ ջ
static uint8_t uart3_rx_step = 0;
static uint8_t uart3_rx_checksum = 0;
static uint8_t uart3_rx_checksum_rcv = 0;
static uint8_t uart3_rx_int16_1_low = 0;
static uint8_t uart3_rx_int16_1_high = 0;
static uint8_t uart3_rx_int16_2_low = 0;
static uint8_t uart3_rx_int16_2_high = 0;
int16_t left_speed = 0;
int16_t right_speed = 0;
//float base_speed = 0.679325;
//float kp_turn = 0.35f;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
#include <string.h>
#include <stdio.h>
#define RX_BUFFER_SIZE 64
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_USART3_UART_Init();
/* USER CODE BEGIN 2 */
HAL_UART_Receive_IT(&huart3, &rx_buffer, 1);
motorInit();
// HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
// __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 500);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
// 设置电机目标速度（PID速度闭环会自动调节PWM）
setSpeedL(left_speed);
setSpeedR(right_speed);
// HAL_Delay(5); // 10ms控制周期，与PID周期匹配
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV8;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV8;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart->Instance == USART3)
{
switch(uart3_rx_step)
{
case 0:
if(rx_buffer == 0xAA)
{
uart3_rx_step = 1;
uart3_rx_checksum = 0xAA;
HAL_UART_Transmit(&huart3, &rx_buffer, 1, 100);
}
break;
case 1:
uart3_rx_int16_1_low = rx_buffer;
uart3_rx_checksum += rx_buffer;
uart3_rx_step = 2;
break;
case 2:
uart3_rx_int16_1_high = rx_buffer;
uart3_rx_checksum += rx_buffer;
uart3_rx_step = 3;
break;
case 3:
uart3_rx_int16_2_low = rx_buffer;
uart3_rx_checksum += rx_buffer;
uart3_rx_step = 4;
break;
case 4:
uart3_rx_int16_2_high = rx_buffer;
uart3_rx_checksum += rx_buffer;
uart3_rx_step = 5;
break;
case 5:
uart3_rx_checksum_rcv = rx_buffer;
if((uart3_rx_checksum & 0xFF) == uart3_rx_checksum_rcv)
{
int16_t value1 = (int16_t)(uart3_rx_int16_1_low | (uart3_rx_int16_1_high << 8));
int16_t value2 = (int16_t)(uart3_rx_int16_2_low | (uart3_rx_int16_2_high << 8));
left_speed = value1;
right_speed = value2;
uint8_t response[6] = {
0xAA,
uart3_rx_int16_1_low,
uart3_rx_int16_1_high,
uart3_rx_int16_2_low,
uart3_rx_int16_2_high,
uart3_rx_checksum_rcv
};
HAL_UART_Transmit(&huart3, response, 6, 100);
}
uart3_rx_step = 0;
break;
}
HAL_UART_Receive_IT(&huart3, &rx_buffer, 1);
}
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
motor.c
#include "motor.h"
#include "tim.h"
#define ABS(x) ((x) > 0) ? (x) : -(x)
#define PULSE2SPEED 0.16983125
#define PWM_MAX 999
#define PWM_MIN -999
#define INTEGRAL_LIMIT 99.0f
#define PWM_TIM htim3
#define PID_TIM htim4
#define COUNTER_A_TIM htim1
#define COUNTER_B_TIM htim2
// class GuoguoILoveU
// 线数 13 四倍频率 52
// 10ms 52 * 0.01 = 0.52
// 28.26cm
// 27.173 -> 5 0.27173
//
volatile float debug_actualL = 0.0f;
volatile float debug_actualR = 0.0f;
volatile int16_t testA = 0;
volatile int16_t testB = 0;
typedef struct {
float p;
float i;
float d;
float targetSpeed;
float lastErr;
float totalErr;
int16_t pwm;
} pid_element_t;
pid_element_t motorL = {
.p = -40.0f,
.i = -0.05f,
.d = -60.0f,
.targetSpeed = 10.0f,
.lastErr = 0.0f,
.totalErr = 0.0f,
.pwm = 0
};
pid_element_t motorR = {
.p = -40.0f,
.i = -0.05f,
.d = -60.0f,
.targetSpeed = 10.0f,
.lastErr = 0.0f,
.totalErr = 0.0f,
.pwm = 0
};
void motorInit()
{
// 启动两个 PWM 通道（使用 TIM4 的 CH1 和 CH2）
HAL_TIM_PWM_Start(&PWM_TIM, TIM_CHANNEL_1); // 左电机 PWM (PB6)
HAL_TIM_PWM_Start(&PWM_TIM, TIM_CHANNEL_2); // 右电机 PWM (PB7)
// 设置左电机方向（正转）
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET); // AIN1 高
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET); // AIN2 低
// 设置右电机方向（正转）
HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET); // BIN1 高
HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET); // BIN2 低
// 设置两个电机的初始占空比（30%），对应比较值 300（若 Period=999）
__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_1, 0); // 左电机
__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_2, 0); // 右电机
HAL_TIM_Encoder_Start(&COUNTER_A_TIM, TIM_CHANNEL_ALL);//编码器启动,定时器开始计算
HAL_TIM_Encoder_Start(&COUNTER_B_TIM, TIM_CHANNEL_ALL);
HAL_TIM_Base_Start_IT(&PID_TIM);
}
/**
* @Breif 设置左轮PWM
* @Input pwm数值 范围 -1000 ~ 1000
*/
void setL(int16_t pwm)
{
if(pwm > 1000 || pwm < -1000) return;
if(pwm < 0) {
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
} else {
HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
}
__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_1, ABS(pwm));
testA = ABS(pwm);
}
/**
* @Breif 设置右轮PWM
* @Input pwm数值 范围 -1000 ~ 1000
*/
void setR(int16_t pwm)
{
if(pwm > 1000 || pwm < -1000) return;
if(pwm < 0) {
HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
} else {
HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
}
__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_2, ABS(pwm));
testB = ABS(pwm);
}
/**
* @Breif 读出左轮脉冲
* @output 输出左轮脉冲
*/
int16_t getL()
{
int16_t current = __HAL_TIM_GET_COUNTER(&COUNTER_A_TIM);
__HAL_TIM_SET_COUNTER(&COUNTER_A_TIM,0);
return current;
}
/**
* @Breif 读出右轮脉冲
* @output 输出右轮脉冲
*/
int16_t getR()
{
int16_t current = __HAL_TIM_GET_COUNTER(&COUNTER_B_TIM);
__HAL_TIM_SET_COUNTER(&COUNTER_B_TIM,0);
return current;
}
void setSpeedL(float speed) {
motorL.targetSpeed = speed;
}
void setSpeedR(float speed) {
motorR.targetSpeed = -speed;
}
int16_t pidCalcUnit(pid_element_t* i, float err)
{
i->pwm += (i->p * err + i->i * i->totalErr + i->d * (err - i->lastErr));
if (i->pwm > PWM_MAX) i->pwm = PWM_MAX;
if (i->pwm < PWM_MIN) i->pwm = PWM_MIN;
i->totalErr += err;
if (i->totalErr > INTEGRAL_LIMIT) i->totalErr = INTEGRAL_LIMIT;
if (i->totalErr < -INTEGRAL_LIMIT) i->totalErr = -INTEGRAL_LIMIT;
i->lastErr = err;
return i->pwm;
}
void pidCalc()
{
// 先读取脉冲数并计算实际速度
int16_t pulseL = getL();
int16_t pulseR = getR();
float actualL = PULSE2SPEED * pulseL;
float actualR = PULSE2SPEED * pulseR;
// 赋值给调试变量
debug_actualL = actualL;
debug_actualR = actualR;
// 计算误差
float errL = actualL - motorL.targetSpeed ;
float errR = actualR - motorR.targetSpeed;
// 执行 PID 并设置电机
setL(pidCalcUnit(&motorL, errL));
setR(pidCalcUnit(&motorR, errR));
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
pidCalc();
}
