{"id":610,"date":"2026-05-22T12:22:39","date_gmt":"2026-05-22T04:22:39","guid":{"rendered":"https:\/\/yin.nnneri.me\/?p=610"},"modified":"2026-05-22T12:28:09","modified_gmt":"2026-05-22T04:28:09","slug":"%e6%a0%a1%e7%94%b5%e8%b5%9b%e5%86%b3%e8%b5%9b%e4%b8%80%e7%ad%89%e5%a5%96%e6%ba%90%e7%a0%81%e5%ad%98%e6%a1%a3","status":"publish","type":"post","link":"https:\/\/yin.nnneri.me\/?p=610","title":{"rendered":"\u6821\u7535\u8d5b\u51b3\u8d5b\u4e00\u7b49\u5956H\u9898\u6e90\u7801\u5b58\u6863"},"content":{"rendered":"\n

\u60ca\u5fc3\u52a8\u9b44\u7684\u5c0f\u8f66\u8c03\u8bd5\u5440<\/strong><\/mark><\/p>\n\n\n\n

Maixcam<\/h2>\n\n\n\n

\u9898\u76ee\u4e00\u548c\u4e8c<\/h3>\n\n\n\n

\u8fd9\u4e9b\u9898\u76ee\u90fd\u8981\u6539\u52a8\u4e00\u4e0blab<\/p>\n\n\n\n

from maix import image, camera, display, app, audio, time, uart, touchscreen\nimport math\nimport struct\n\ncamWidth = 320\n\nts = touchscreen.TouchScreen()\ndisp = display.Display()\n\nidStart = 0\nidEnd = 4\n\nserial_dev = uart.UART(\"\/dev\/ttyS0\", 115200)\ntime.sleep_ms(100)\n\nThreshold = [[0, 40, -5, 5, -5, 5]]  # \u66f4\u5bbd\u677e\u7684 L\/A\/B \u8303\u56f4\n\nclass LineFollower:\n    def __init__(self, img_width=320, img_height=240,\n                 lost_frame_threshold=5, recovery_frame_threshold=3,\n                 min_area=256, max_width_ratio=0.4, edge_margin=0.0,\n                 proximity_limit=60,\n                 # \u9ad8\u7075\u654f\u5ea6\u53c2\u6570\uff08\u4e22\u7ebf\u65f6\u4e0a ROI \u4e13\u7528\uff09\n                 sensitive_min_area=64,\n                 sensitive_max_width_ratio=0.2,\n                 sensitive_edge_margin=0.00):\n        self.img_w = img_width\n        self.img_h = img_height\n\n        # ROI \u767e\u5206\u6bd4\u5b9a\u4e49\uff08\u4e0a\/\u4e2d\/\u4e0b\uff09\n        rois_pct = [\n            (0,   0.40, 1.0, 0.2, 0.2),  # \u4e0a\uff08\u6700\u8fdc\uff09\n            (0,   0.60, 1.0, 0.2, 0.4),  # \u4e2d\n            (0,   0.80, 1.0, 0.2, 0.9)   # \u4e0b\uff08\u6700\u8fd1\uff09\n        ]\n        self.rois = []\n        for (xp, yp, wp, hp, wgt) in rois_pct:\n            x = int(xp * img_width)\n            y = int(yp * img_height)\n            w = int(wp * img_width)\n            h = int(hp * img_height)\n            self.rois.append((x, y, w, h, wgt))\n        self.weight_sum = sum(r[4] for r in self.rois)\n\n        # \u5404 ROI \u4e2d\u5fc3 Y \u5750\u6807\uff08\u7528\u4e8e\u5916\u63a8\uff09\n        self.roi_centers_y = [y + h\/\/2 for (_, y, _, h, _) in self.rois]\n\n        # \u5185\u90e8\u72b6\u6001\n        self.line_rho = 0\n        self.last_rho = img_width \/\/ 2\n        self.lost_count = 0\n        self.recovery_count = 0\n        self.lost_frame_cnt = 0\n        self.recovery_frame_cnt = 0\n        self.confirmed_lost = False\n\n        # \u5e38\u89c4\u53c2\u6570\n        self.lost_frame_threshold = lost_frame_threshold\n        self.recovery_frame_threshold = recovery_frame_threshold\n        self.min_area = min_area\n        self.max_width_ratio = max_width_ratio\n        self.edge_margin = edge_margin\n        self.proximity_limit = proximity_limit\n\n        # \u9ad8\u7075\u654f\u5ea6\u53c2\u6570\uff08\u4e22\u7ebf\u65f6\u4e0a ROI \u4e13\u7528\uff09\n        self.sensitive_min_area = sensitive_min_area\n        self.sensitive_max_width_ratio = sensitive_max_width_ratio\n        self.sensitive_edge_margin = sensitive_edge_margin\n\n        self.draw_debug = True\n\n    # ---------- \u8f85\u52a9\u51fd\u6570 ----------\n    def _filter_blobs(self, blobs, roi_x, roi_w, min_area, max_w_ratio, edge_margin):\n        \"\"\"\u901a\u7528\u8fc7\u6ee4\uff1a\u9762\u79ef\u3001\u5bbd\u5ea6\u3001\u8fb9\u7f18\"\"\"\n        valid = []\n        for b in blobs:\n            if b.area() < min_area:\n                continue\n            if b.w() > roi_w * max_w_ratio:\n                continue\n            if (b.x() < roi_x + roi_w * edge_margin or\n                (b.x() + b.w()) > roi_x + roi_w * (1 - edge_margin)):\n                continue\n            valid.append(b)\n        return valid\n\n    def _nearest_blob(self, blobs, target_x, limit=None):\n        if not blobs:\n            return None\n        if limit is None:\n            limit = self.proximity_limit\n        best = None\n        best_dist = float('inf')\n        for b in blobs:\n            dist = abs(b.cx() - target_x)\n            if dist < best_dist and dist <= limit:\n                best = b\n                best_dist = dist\n        return best\n\n    def _extrapolate_x(self, x_mid, y_mid, x_down, y_down, y_up):\n        dy = y_down - y_mid\n        if abs(dy) < 1e-6:\n            return x_down\n        return int(x_mid + (x_down - x_mid) * (y_up - y_mid) \/ dy)\n\n    # ---------- \u4e3b\u5904\u7406 ----------\n    def update(self, img):\n        # img.gaussian(3)\n        # try:\n        #     img.clahe(clip_limit=2.0, tile_grid=(8,8))\n        # except:\n        #     img.histeq() \n        line_blobs = []           # \u7528\u4e8e\u7ed8\u5236\u7684\u5b9e\u9645\u8272\u5757\n        roi_blobs = [None, None, None]  # \u4e0a\u4e2d\u4e0b\u6700\u7ec8\u9009\u4e2d\u7684\u8272\u5757\n\n        # \u83b7\u53d6\u5404 ROI \u7684\u5019\u9009\u8272\u5757\uff08\u5e38\u89c4\u7075\u654f\u5ea6\uff09\n        candidates = []\n        for idx, (x, y, w, h, wgt) in enumerate(self.rois):\n            blobs = img.find_blobs(Threshold, roi=(x, y, w, h),\n                                   merge=True, pixels_threshold=self.min_area)\n            valid = self._filter_blobs(blobs, x, w,\n                                       self.min_area,\n                                       self.max_width_ratio,\n                                       self.edge_margin)\n            candidates.append(valid)\n\n        # ---- \u4e0b ROI\uff1a\u53c2\u8003 last_rho ----\n        ref_down = self.last_rho\n        blob_d = self._nearest_blob(candidates[2], ref_down)\n        roi_blobs[2] = blob_d\n\n        # ---- \u4e2d ROI\uff1a\u5982\u679c\u6709\u4e0b\uff0c\u5219\u53c2\u8003\u4e0b\u4e2d\u5fc3\uff0c\u5426\u5219\u53c2\u8003 last_rho ----\n        ref_mid = blob_d.cx() if blob_d else self.last_rho\n        blob_m = self._nearest_blob(candidates[1], ref_mid)\n        roi_blobs[1] = blob_m\n\n        # ---- \u5224\u65ad\u662f\u5426\u4e22\u7ebf\uff08\u4e2d\u3001\u4e0b\u540c\u65f6\u65e0\u6548\uff09----\n        lose_now = (roi_blobs[2] is None and roi_blobs[1] is None)\n\n        # ---- \u4e0a ROI\uff1a\u5e73\u65f6\u7528\u5916\u63a8\/\u90bb\u8fd1\uff0c\u4e22\u7ebf\u65f6\u9ad8\u7075\u654f\u5ea6\u5c3d\u529b\u67e5\u627e ----\n        if not lose_now:\n            # \u6b63\u5e38\u60c5\u51b5\uff1a\u4e2d\u6216\u4e0b\u81f3\u5c11\u6709\u4e00\u4e2a\u6709\u6548\n            if blob_m is not None and blob_d is not None:\n                # \u4e24\u70b9\u4e00\u7ebf\u5916\u63a8\n                x_ext = self._extrapolate_x(blob_m.cx(), self.roi_centers_y[1],\n                                            blob_d.cx(), self.roi_centers_y[2],\n                                            self.roi_centers_y[0])\n                blob_u = self._nearest_blob(candidates[0], x_ext,\n                                            limit=self.proximity_limit * 1.5)\n            else:\n                # \u53ea\u6709\u4e00\u4e2a\u6709\u6548\uff08\u4e2d\u6216\u4e0b\uff09\n                ref_up = blob_m.cx() if blob_m else (blob_d.cx() if blob_d else self.last_rho)\n                blob_u = self._nearest_blob(candidates[0], ref_up)\n            roi_blobs[0] = blob_u\n        else:\n            # ---- \u4e22\u7ebf\u72b6\u6001\uff1a\u4e0a ROI \u9ad8\u7075\u654f\u5ea6\u5c3d\u529b\u67e5\u627e\u7ebf\u5934 ----\n            blob_u = None\n\n            # \u5148\u5c1d\u8bd5\u4ece\u5e38\u89c4\u5019\u9009\u91cc\u627e\uff08\u4e5f\u8bb8\u5df2\u7ecf\u8fc7\u6ee4\u51fa\u5c0f\u5757\uff09\n            if candidates[0]:\n                # \u53d6\u79bb last_rho \u6700\u8fd1\u7684\n                blob_u = self._nearest_blob(candidates[0], self.last_rho,\n                                            limit=self.proximity_limit * 2.0)\n\n            # \u5982\u679c\u5e38\u89c4\u5019\u9009\u6ca1\u627e\u5230\uff0c\u7528\u9ad8\u7075\u654f\u5ea6\u53c2\u6570\u91cd\u65b0\u67e5\u627e\n            if blob_u is None:\n                x, y, w, h, _ = self.rois[0]\n                blobs_sensitive = img.find_blobs(Threshold, roi=(x, y, w, h),\n                                                 merge=True,\n                                                 pixels_threshold=self.sensitive_min_area)\n                valid_s = self._filter_blobs(blobs_sensitive, x, w,\n                                             min_area=self.sensitive_min_area,\n                                             max_w_ratio=self.sensitive_max_width_ratio,\n                                             edge_margin=self.sensitive_edge_margin)\n                if valid_s:\n                    # \u53d6\u9762\u79ef\u6700\u5927\u6216\u79bb last_rho \u6700\u8fd1\u7684\uff0c\u8fd9\u91cc\u7528\u6700\u8fd1\n                    blob_u = self._nearest_blob(valid_s, self.last_rho,\n                                                limit=self.proximity_limit * 2.5)\n\n            roi_blobs[0] = blob_u\n\n        # ---- \u6536\u96c6\u7528\u4e8e\u663e\u793a\u548c\u52a0\u6743 ----\n        centroid_sum = 0\n        weight_used = 0\n        for idx, b in enumerate(roi_blobs):\n            if b is not None:\n                line_blobs.append(b)\n                centroid_sum += b.cx() * self.rois[idx][4]\n                weight_used += self.rois[idx][4]\n\n        # ---- \u8fde\u7eed\u5e27\u8ba1\u6570 ----\n        if lose_now:\n            self.lost_frame_cnt += 1\n            self.recovery_frame_cnt = 0\n        else:\n            self.recovery_frame_cnt += 1\n            self.lost_frame_cnt = 0\n\n        is_lost_confirmed = (self.lost_frame_cnt >= self.lost_frame_threshold)\n        is_recovered = (self.recovery_frame_cnt >= self.recovery_frame_threshold)\n\n        if is_lost_confirmed and not self.confirmed_lost:\n            self.lost_count += 1\n            self.confirmed_lost = True\n        elif self.confirmed_lost and is_recovered:\n            self.recovery_count += 1\n            self.confirmed_lost = False\n\n        # ---- \u8ba1\u7b97\u6700\u7ec8\u7ebf\u4f4d\u7f6e ----\n        if not lose_now:\n            if weight_used > 0:\n                self.line_rho = int(centroid_sum \/ weight_used)\n            else:\n                self.line_rho = self.last_rho\n            self.last_rho = self.line_rho\n        else:\n            # \u4e22\u7ebf\u65f6\uff0c\u6709\u4e0a ROI \u5c31\u7528\u4e0a\uff0c\u6ca1\u6709\u5219\u4fdd\u6301\n            if roi_blobs[0] is not None:\n                self.line_rho = roi_blobs[0].cx()\n                self.last_rho = self.line_rho\n                line_blobs.append(roi_blobs[0])  # \u786e\u4fdd\u7ed8\u5236\u51fa\u6765\n            else:\n                self.line_rho = self.last_rho\n\n        offset = self.line_rho - self.img_w \/\/ 2\n\n        # ---- \u8c03\u8bd5\u7ed8\u5236 ----\n        if self.draw_debug:\n            for b in line_blobs:\n                img.draw_rect(b.x(), b.y(), b.w(), b.h(), color=image.COLOR_WHITE)\n                img.draw_cross(b.cx(), b.cy(),\n                               image.Color.from_rgb(255, 255, 255),\n                               size=5, thickness=1)\n            img.draw_string(0, 30,\n                            'rho=%d off=%d lost=%d' % (self.line_rho, offset, self.lost_count),\n                            image.COLOR_WHITE)\n            img.draw_string(0, 50,\n                            'lost_cnt=%d rec=%d' % (self.lost_frame_cnt, self.recovery_count),\n                            image.COLOR_WHITE)\n\n        return offset, self.lost_count, self.recovery_count\n\n# ========== \u5916\u90e8\u4e3b\u5faa\u73af\u793a\u4f8b ==========\nfollower = LineFollower(img_width=320, img_height=240)\n\n# \u901f\u5ea6\u63a7\u5236\u53c2\u6570\nBASE_SPEED = 33\nTURN_GAIN = BASE_SPEED * 0.0045 * 320 \/ camWidth\nMAX_SPEED = BASE_SPEED\nMIN_SPEED = 0\n\n# ========== \u5de1\u7ebf\u6a21\u5f0f\u51fd\u6570 ==========\ndef calc_speed(error):\n    \"\"\"\u8ba1\u7b97\u5de6\u53f3\u8f6e\u901f\u5ea6\"\"\"\n    left_speed = BASE_SPEED + TURN_GAIN * error\n    right_speed = BASE_SPEED - TURN_GAIN * error\n    \n    left_speed = max(MIN_SPEED, min(MAX_SPEED, left_speed))\n    right_speed = max(MIN_SPEED, min(MAX_SPEED, right_speed))\n    \n    return int(left_speed), int(right_speed)\n\ndef send_speed_to_stm32(left_speed, right_speed):\n    \"\"\"\u6253\u5305\u53d1\u90016\u5b57\u8282\u534f\u8bae\u5230STM32\"\"\"\n    left_u16 = left_speed & 0xFFFF\n    right_u16 = right_speed & 0xFFFF\n    \n    left_low = left_u16 & 0xFF\n    left_high = (left_u16 >> 8) & 0xFF\n    right_low = right_u16 & 0xFF\n    right_high = (right_u16 >> 8) & 0xFF\n    \n    checksum = (0xAA + left_low + left_high + right_low + right_high) & 0xFF\n    packet = bytes([0xAA, left_low, left_high, right_low, right_high, checksum])\n    serial_dev.write(packet)\n    return packet\n\n\n# \u63d0\u524d\u521b\u5efa\u597d\u64ad\u653e\u5668\uff0c\u907f\u514d\u6bcf\u6b21\u8c03\u7528\u90fd\u91cd\u590d\u521d\u59cb\u5316\uff0c\u63d0\u5347\u54cd\u5e94\u901f\u5ea6\n_player = audio.Player(sample_rate=48000, format=audio.Format.FMT_S16_LE, channel=1)\n_player.volume(100)   # \u97f3\u91cf 0-100\uff0c\u53ef\u6309\u9700\u8c03\u6574\n\ndef beep(freq=2000, duration=0.3):\n    \"\"\"\n    \u4f7f\u7528\u677f\u8f7d\u5587\u53ed\u64ad\u653e\u6307\u5b9a\u9891\u7387\u548c\u65f6\u957f\u7684\u63d0\u793a\u97f3\uff08\u963b\u585e\u5f0f\uff09\n    :param freq: \u9891\u7387\uff0c\u5355\u4f4d Hz\uff0c\u9ed8\u8ba4 2000\n    :param duration: \u6301\u7eed\u65f6\u95f4\uff0c\u5355\u4f4d \u79d2\uff0c\u9ed8\u8ba4 0.8\n    \"\"\"\n    # 1. \u6839\u636e\u9891\u7387\u548c\u65f6\u957f\u751f\u6210 PCM \u97f3\u9891\u6570\u636e\n    sample_rate = 48000\n    num_samples = int(sample_rate * duration)\n    amplitude = 32767   # \u6700\u5927\u97f3\u91cf\n    pcm = bytearray()\n\n    for i in range(num_samples):\n        t = i \/ sample_rate\n        sample = amplitude * math.sin(2 * math.pi * freq * t)\n        pcm.extend(struct.pack('<h', int(sample)))\n\n    # 2. \u64ad\u653e\u751f\u6210\u7684 PCM \u6570\u636e\uff08\u975e\u963b\u585e\uff0c\u786c\u4ef6\u7acb\u5373\u5f00\u59cb\u8f93\u51fa\uff09\n    _player.play(bytes(pcm))\n\n    # 3. \u963b\u585e\u7b49\u5f85\u64ad\u653e\u7ed3\u675f\n    time.sleep_ms(int(duration * 1000))\n\ndef detectApriltag():\n    cam = camera.Camera(320, 240)\n    families = image.ApriltagFamilies.TAG36H11\n\n    print(\"\u5f00\u59cb\u8bc6\u522b AprilTag\uff0c\u6309 Ctrl+C \u6216\u5f00\u53d1\u677f\u4e0a\u7684 Home \u952e\u9000\u51fa...\")\n\n    while not app.need_exit():\n        try:\n            img = cam.read()\n            if img is None:\n                continue\n\n            apriltags = img.find_apriltags(families=families)\n\n            if apriltags:\n                ids = [tag.id() for tag in apriltags]\n                print(ids)\n\n                img.draw_string(10, 10, \"IDs: \" + \",\".join(map(str, ids)),\n                                scale=3, color=image.COLOR_RED)\n\n                for tag in apriltags:\n                    corners = tag.corners()\n                    for i in range(4):\n                        img.draw_line(\n                            corners[i][0], corners[i][1],\n                            corners[(i + 1) % 4][0], corners[(i + 1) % 4][1],\n                            color=image.COLOR_GREEN, thickness=2\n                        )\n\n                disp.show(img)\n                beep()   # \u786e\u4fdd beep \u51fd\u6570\u5df2\u5b9a\u4e49\uff0c\u5426\u5219\u4f1a\u62a5\u9519\n                print(\"\u8bc6\u522b\u5b8c\u6210\uff0c\u7a0b\u5e8f\u9000\u51fa\u3002\")\n                del cam          # \u5148\u91ca\u653e\u6444\u50cf\u5934\u518d\u8fd4\u56de\n                return ids       # \u8fd4\u56de\u6240\u6709 ID \u7684\u5217\u8868\n            else:\n                disp.show(img)\n\n        except Exception as e:\n            print(f\"\u8bc6\u522b\u51fa\u9519: {e}\")\n\n    del cam\n    return []      # \u5982\u679c\u7528\u6237\u4e3b\u52a8\u9000\u51fa\uff0c\u8fd4\u56de\u7a7a\u5217\u8868\n\ndef is_in_button(x, y, btn_pos):\n    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\n\n\nif __name__ == \"__main__\":\n    found_id = 0\n    delay = 60\n    while True:\n        ids = detectApriltag()  \n        if not ids:\n            print(\"\u672a\u68c0\u6d4b\u5230\u4efb\u4f55\u6807\u7b7e\u6216\u7528\u6237\u4e3b\u52a8\u9000\u51fa\uff0c\u7a0b\u5e8f\u7ed3\u675f\u3002\")\n            break\n        for tag_id in ids:\n            if idStart <= tag_id <= idEnd:\n                found_id = tag_id\n                break\n\n        if found_id is not None:\n            print(f\"\u627e\u5230\u8303\u56f4\u5185 ID: {found_id} (\u8303\u56f4 {idStart}~{idEnd})\")\n            break  \n        else:\n            print(f\"\u68c0\u6d4b\u5230\u7684 ID {ids} \u4e0d\u5728\u8303\u56f4 [{idStart}, {idEnd}] \u5185\uff0c\u91cd\u65b0\u8bc6\u522b...\")\n\n    cam = camera.Camera(320, 240)\n    loop_label = f\"LOOP: {found_id}\"\n    exit_label = \" R U N \"\n    size = image.string_size(exit_label, scale = 3.0, thickness = 2)\n    exit_btn_pos = [0, 0, 8*2 + size.width(), 30 + size.height()]\n    while True:\n        img = cam.read()\n        img.draw_string(8, 30, exit_label, image.COLOR_WHITE, scale = 3.0, thickness = 2)\n        img.draw_rect(exit_btn_pos[0], exit_btn_pos[1], exit_btn_pos[2], exit_btn_pos[3],  image.COLOR_WHITE, 2)\n        img.draw_string(8, 75, loop_label, image.COLOR_WHITE, scale = 3.0, thickness = 2)\n        x, y, pressed = ts.read()\n        if is_in_button(x, y, exit_btn_pos):\n            break\n        disp.show(img)\n\n    \n    while True:\n        t = time.time_ms()\n        img = cam.read()\n        if img is None: \n            continue\n        offset, lost_cnt, recovery_count = follower.update(img)\n\n        l, r = calc_speed(offset)\n        if recovery_count > found_id * 3 - 1:\n            delay -= 1\n            if delay > 0:\n                delay -= 1\n                send_speed_to_stm32(l, -r)\n            else:\n                send_speed_to_stm32(0, 0)\n        else:\n            send_speed_to_stm32(l, -r)\n\n        disp.show(img)\n        print(\"FPS =\", int(1000 \/ (time.time_ms() - t)), \"Offset:\", offset, \"Lost:\", lost_cnt)\n        <\/code><\/pre>\n\n\n\n
\u9898\u76ee\u4e09<\/h3>\n\n\n\n
from maix import image, camera, display, app, audio, time, uart, touchscreen, ahrs\nimport math\nimport struct\n\nfrom maix.ext_dev import imu\n\ncamWidth = 320\n\nts = touchscreen.TouchScreen()\ndisp = display.Display()\n\nidStart = 0\nidEnd = 4\n\nserial_dev = uart.UART(\"\/dev\/ttyS0\", 115200)\ntime.sleep_ms(100)\n\nThreshold = [[0, 40, -6, 6, -10, 6]]  # \u66f4\u5bbd\u677e\u7684 L\/A\/B \u8303\u56f4\n\n\nkp = 2\nki = 0.01\ncalibrate_first = False\nsensor = imu.IMU(\"\", mode=imu.Mode.DUAL,\n                                    acc_scale=imu.AccScale.ACC_SCALE_2G,\n                                    acc_odr=imu.AccOdr.ACC_ODR_1000,\n                                    gyro_scale=imu.GyroScale.GYRO_SCALE_256DPS,\n                                    gyro_odr=imu.GyroOdr.GYRO_ODR_8000)\nahrs_filter = ahrs.MahonyAHRS(kp, ki)\n\nif calibrate_first or not sensor.calib_gyro_exists():\n    print(\"\\n\\nNeed calibrate fisrt\")\n    print(\"now calibrate, please !! don't move !! device, wait for 10 seconds\")\n    sensor.calib_gyro(10000)\nelse:\n    sensor.load_calib_gyro()\n\nclass LineFollower:\n    def __init__(self, img_width=320, img_height=240,\n                 lost_frame_threshold=5, recovery_frame_threshold=3,\n                 min_area=256, max_width_ratio=0.70, edge_margin=0.0,\n                 proximity_limit=60,\n                 # \u9ad8\u7075\u654f\u5ea6\u53c2\u6570\uff08\u4e22\u7ebf\u65f6\u4e0a ROI \u4e13\u7528\uff09\n                 sensitive_min_area=30,\n                 sensitive_max_width_ratio=0.8,\n                 sensitive_edge_margin=0.0):\n        self.img_w = img_width\n        self.img_h = img_height\n\n        # ROI \u767e\u5206\u6bd4\u5b9a\u4e49\uff08\u4e0a\/\u4e2d\/\u4e0b\uff09\n        rois_pct = [\n            (0.4,   0.40, 0.6, 0.2, 0.2),  # \u4e0a\uff08\u6700\u8fdc\uff09\n            (0,   0.60, 1.0, 0.2, 0.5),  # \u4e2d\n            (0,   0.80, 1.0, 0.2, 0.8)   # \u4e0b\uff08\u6700\u8fd1\uff09\n        ]\n        self.rois = []\n        for (xp, yp, wp, hp, wgt) in rois_pct:\n            x = int(xp * img_width)\n            y = int(yp * img_height)\n            w = int(wp * img_width)\n            h = int(hp * img_height)\n            self.rois.append((x, y, w, h, wgt))\n        self.weight_sum = sum(r[4] for r in self.rois)\n\n        # \u5404 ROI \u4e2d\u5fc3 Y \u5750\u6807\uff08\u7528\u4e8e\u5916\u63a8\uff09\n        self.roi_centers_y = [y + h\/\/2 for (_, y, _, h, _) in self.rois]\n\n        # \u5185\u90e8\u72b6\u6001\n        self.line_rho = 0\n        self.last_rho = img_width \/\/ 2\n        self.lost_count = 0\n        self.recovery_count = 0\n        self.lost_frame_cnt = 0\n        self.recovery_frame_cnt = 0\n        self.confirmed_lost = False\n\n        # \u5e38\u89c4\u53c2\u6570\n        self.lost_frame_threshold = lost_frame_threshold\n        self.recovery_frame_threshold = recovery_frame_threshold\n        self.min_area = min_area\n        self.max_width_ratio = max_width_ratio\n        self.edge_margin = edge_margin\n        self.proximity_limit = proximity_limit\n\n        # \u9ad8\u7075\u654f\u5ea6\u53c2\u6570\uff08\u4e22\u7ebf\u65f6\u4e0a ROI \u4e13\u7528\uff09\n        self.sensitive_min_area = sensitive_min_area\n        self.sensitive_max_width_ratio = sensitive_max_width_ratio\n        self.sensitive_edge_margin = sensitive_edge_margin\n\n        self.draw_debug = True\n\n    # ---------- \u8f85\u52a9\u51fd\u6570 ----------\n    def _filter_blobs(self, blobs, roi_x, roi_w, min_area, max_w_ratio, edge_margin):\n        \"\"\"\u901a\u7528\u8fc7\u6ee4\uff1a\u9762\u79ef\u3001\u5bbd\u5ea6\u3001\u8fb9\u7f18\"\"\"\n        valid = []\n        for b in blobs:\n            if b.area() < min_area:\n                continue\n            if b.w() > roi_w * max_w_ratio:\n                continue\n            if (b.x() < roi_x + roi_w * edge_margin or\n                (b.x() + b.w()) > roi_x + roi_w * (1 - edge_margin)):\n                continue\n            valid.append(b)\n        return valid\n\n    def _nearest_blob(self, blobs, target_x, limit=None):\n        if not blobs:\n            return None\n        if limit is None:\n            limit = self.proximity_limit\n        best = None\n        best_dist = float('inf')\n        for b in blobs:\n            dist = abs(b.cx() - target_x)\n            if dist < best_dist and dist <= limit:\n                best = b\n                best_dist = dist\n        return best\n\n    def _extrapolate_x(self, x_mid, y_mid, x_down, y_down, y_up):\n        dy = y_down - y_mid\n        if abs(dy) < 1e-6:\n            return x_down\n        return int(x_mid + (x_down - x_mid) * (y_up - y_mid) \/ dy)       \n\n    # ---------- \u4e3b\u5904\u7406 ----------\n    def update(self, img):\n        # img.gaussian(3)\n        # try:\n        #     img.clahe(clip_limit=2.0, tile_grid=(8,8))\n        # except:\n        #     img.histeq() \n        line_blobs = []           # \u7528\u4e8e\u7ed8\u5236\u7684\u5b9e\u9645\u8272\u5757\n        roi_blobs = [None, None, None]  # \u4e0a\u4e2d\u4e0b\u6700\u7ec8\u9009\u4e2d\u7684\u8272\u5757\n\n        # \u83b7\u53d6\u5404 ROI \u7684\u5019\u9009\u8272\u5757\uff08\u5e38\u89c4\u7075\u654f\u5ea6\uff09\n        candidates = []\n        for idx, (x, y, w, h, wgt) in enumerate(self.rois):\n            blobs = img.find_blobs(Threshold, roi=(x, y, w, h),\n                                   merge=True, pixels_threshold=self.min_area)\n            valid = self._filter_blobs(blobs, x, w,\n                                       self.min_area,\n                                       self.max_width_ratio,\n                                       self.edge_margin)\n            candidates.append(valid)\n\n        # ---- \u4e0b ROI\uff1a\u53c2\u8003 last_rho ----\n        ref_down = self.last_rho\n        blob_d = self._nearest_blob(candidates[2], ref_down)\n        roi_blobs[2] = blob_d\n\n        # ---- \u4e2d ROI\uff1a\u5982\u679c\u6709\u4e0b\uff0c\u5219\u53c2\u8003\u4e0b\u4e2d\u5fc3\uff0c\u5426\u5219\u53c2\u8003 last_rho ----\n        ref_mid = blob_d.cx() if blob_d else self.last_rho\n        blob_m = self._nearest_blob(candidates[1], ref_mid)\n        roi_blobs[1] = blob_m\n\n        # ---- \u5224\u65ad\u662f\u5426\u4e22\u7ebf\uff08\u4e2d\u3001\u4e0b\u540c\u65f6\u65e0\u6548\uff09----\n        lose_now = (roi_blobs[2] is None and roi_blobs[1] is None)\n        lose_down = (roi_blobs[2] is None)\n\n        # ---- \u4e0a ROI\uff1a\u5e73\u65f6\u7528\u5916\u63a8\/\u90bb\u8fd1\uff0c\u4e22\u7ebf\u65f6\u9ad8\u7075\u654f\u5ea6\u5c3d\u529b\u67e5\u627e ----\n        if not lose_now:\n            # \u6b63\u5e38\u60c5\u51b5\uff1a\u4e2d\u6216\u4e0b\u81f3\u5c11\u6709\u4e00\u4e2a\u6709\u6548\n            if blob_m is not None and blob_d is not None:\n                # \u4e24\u70b9\u4e00\u7ebf\u5916\u63a8\n                x_ext = self._extrapolate_x(blob_m.cx(), self.roi_centers_y[1],\n                                            blob_d.cx(), self.roi_centers_y[2],\n                                            self.roi_centers_y[0])\n                blob_u = self._nearest_blob(candidates[0], x_ext,\n                                            limit=self.proximity_limit * 1.5)\n            else:\n                # \u53ea\u6709\u4e00\u4e2a\u6709\u6548\uff08\u4e2d\u6216\u4e0b\uff09\n                ref_up = blob_m.cx() if blob_m else (blob_d.cx() if blob_d else self.last_rho)\n                blob_u = self._nearest_blob(candidates[0], ref_up)\n            roi_blobs[0] = blob_u\n        else:\n            # ---- \u4e22\u7ebf\u72b6\u6001\uff1a\u4e0a ROI \u9ad8\u7075\u654f\u5ea6\u5c3d\u529b\u67e5\u627e\u7ebf\u5934 ----\n            blob_u = None\n\n            # \u5148\u5c1d\u8bd5\u4ece\u5e38\u89c4\u5019\u9009\u91cc\u627e\uff08\u4e5f\u8bb8\u5df2\u7ecf\u8fc7\u6ee4\u51fa\u5c0f\u5757\uff09\n            if candidates[0]:\n                # \u53d6\u79bb last_rho \u6700\u8fd1\u7684\n                blob_u = self._nearest_blob(candidates[0], self.last_rho,\n                                            limit=self.proximity_limit * 2.0)\n\n            # \u5982\u679c\u5e38\u89c4\u5019\u9009\u6ca1\u627e\u5230\uff0c\u7528\u9ad8\u7075\u654f\u5ea6\u53c2\u6570\u91cd\u65b0\u67e5\u627e\n            if blob_u is None:\n                x, y, w, h, _ = self.rois[0]\n                blobs_sensitive = img.find_blobs(Threshold, roi=(x, y, w, h),\n                                                 merge=True,\n                                                 pixels_threshold=self.sensitive_min_area)\n                valid_s = self._filter_blobs(blobs_sensitive, x, w,\n                                             min_area=self.sensitive_min_area,\n                                             max_w_ratio=self.sensitive_max_width_ratio,\n                                             edge_margin=self.sensitive_edge_margin)\n                if valid_s:\n                    # \u53d6\u9762\u79ef\u6700\u5927\u6216\u79bb last_rho \u6700\u8fd1\u7684\uff0c\u8fd9\u91cc\u7528\u6700\u8fd1\n                    blob_u = self._nearest_blob(valid_s, self.last_rho,\n                                                limit=self.proximity_limit * 2.5)\n\n            roi_blobs[0] = blob_u\n\n        # ---- \u6536\u96c6\u7528\u4e8e\u663e\u793a\u548c\u52a0\u6743 ----\n        centroid_sum = 0\n        weight_used = 0\n        for idx, b in enumerate(roi_blobs):\n            if b is not None:\n                line_blobs.append(b)\n                centroid_sum += b.cx() * self.rois[idx][4]\n                weight_used += self.rois[idx][4]\n\n        # ---- \u8fde\u7eed\u5e27\u8ba1\u6570 ----\n        if lose_now:\n            self.lost_frame_cnt += 1\n            self.recovery_frame_cnt = 0\n        else:\n            self.recovery_frame_cnt += 1\n            self.lost_frame_cnt = 0\n\n        is_lost_confirmed = (self.lost_frame_cnt >= self.lost_frame_threshold)\n        is_recovered = (self.recovery_frame_cnt >= self.recovery_frame_threshold)\n\n        if is_lost_confirmed and not self.confirmed_lost:\n            self.lost_count += 1\n            self.confirmed_lost = True\n        elif self.confirmed_lost and is_recovered:\n            self.recovery_count += 1\n            self.confirmed_lost = False\n\n        # ---- \u8ba1\u7b97\u6700\u7ec8\u7ebf\u4f4d\u7f6e ----\n        if not lose_now:\n            if weight_used > 0:\n                self.line_rho = int(centroid_sum \/ weight_used)\n            else:\n                self.line_rho = self.last_rho\n            self.last_rho = self.line_rho\n        else:\n            # \u4e22\u7ebf\u65f6\uff0c\u6709\u4e0a ROI \u5c31\u7528\u4e0a\uff0c\u6ca1\u6709\u5219\u4fdd\u6301\n            if roi_blobs[0] is not None:\n                self.line_rho = roi_blobs[0].cx()\n                self.last_rho = self.line_rho\n                line_blobs.append(roi_blobs[0])  # \u786e\u4fdd\u7ed8\u5236\u51fa\u6765\n            else:\n                self.line_rho = self.last_rho\n\n        offset = self.line_rho - self.img_w \/\/ 2\n\n        # ---- \u8c03\u8bd5\u7ed8\u5236 ----\n        if self.draw_debug:\n            for b in line_blobs:\n                img.draw_rect(b.x(), b.y(), b.w(), b.h(), color=image.COLOR_WHITE)\n                img.draw_cross(b.cx(), b.cy(),\n                               image.Color.from_rgb(255, 255, 255),\n                               size=5, thickness=1)\n            img.draw_string(0, 30,\n                            'rho=%d off=%d lost=%d' % (self.line_rho, offset, self.lost_count),\n                            image.COLOR_WHITE)\n            img.draw_string(0, 50,\n                            'lost_cnt=%d rec=%d' % (self.lost_frame_cnt, self.recovery_count),\n                            image.COLOR_WHITE)\n\n        return offset, self.lost_count, self.recovery_count, lose_down\n\n\n\n# ========== \u5916\u90e8\u4e3b\u5faa\u73af\u793a\u4f8b ==========\nfollower = LineFollower(img_width=320, img_height=240)\n\n# \u901f\u5ea6\u63a7\u5236\u53c2\u6570\nBASE_SPEED = 20\nTURN_GAIN = BASE_SPEED * 0.009 * 320 \/ camWidth\nMAX_SPEED = BASE_SPEED\nMIN_SPEED = 0\n\n# ========== \u5de1\u7ebf\u6a21\u5f0f\u51fd\u6570 ==========\ndef calc_speed(error):\n    \"\"\"\u8ba1\u7b97\u5de6\u53f3\u8f6e\u901f\u5ea6\"\"\"\n    left_speed = BASE_SPEED + TURN_GAIN * error\n    right_speed = BASE_SPEED - TURN_GAIN * error\n    \n    left_speed = max(MIN_SPEED, min(MAX_SPEED, left_speed))\n    right_speed = max(MIN_SPEED, min(MAX_SPEED, right_speed))\n    \n    return int(left_speed), int(right_speed)\n\ndef send_speed_to_stm32(left_speed, right_speed):\n    \"\"\"\u6253\u5305\u53d1\u90016\u5b57\u8282\u534f\u8bae\u5230STM32\"\"\"\n    left_u16 = left_speed & 0xFFFF\n    right_u16 = right_speed & 0xFFFF\n    \n    left_low = left_u16 & 0xFF\n    left_high = (left_u16 >> 8) & 0xFF\n    right_low = right_u16 & 0xFF\n    right_high = (right_u16 >> 8) & 0xFF\n    \n    checksum = (0xAA + left_low + left_high + right_low + right_high) & 0xFF\n    packet = bytes([0xAA, left_low, left_high, right_low, right_high, checksum])\n    serial_dev.write(packet)\n    return packet\n\n\n# \u63d0\u524d\u521b\u5efa\u597d\u64ad\u653e\u5668\uff0c\u907f\u514d\u6bcf\u6b21\u8c03\u7528\u90fd\u91cd\u590d\u521d\u59cb\u5316\uff0c\u63d0\u5347\u54cd\u5e94\u901f\u5ea6\n_player = audio.Player(sample_rate=48000, format=audio.Format.FMT_S16_LE, channel=1)\n_player.volume(100)   # \u97f3\u91cf 0-100\uff0c\u53ef\u6309\u9700\u8c03\u6574\n\ndef beep(freq=2000, duration=0.3):\n    \"\"\"\n    \u4f7f\u7528\u677f\u8f7d\u5587\u53ed\u64ad\u653e\u6307\u5b9a\u9891\u7387\u548c\u65f6\u957f\u7684\u63d0\u793a\u97f3\uff08\u963b\u585e\u5f0f\uff09\n    :param freq: \u9891\u7387\uff0c\u5355\u4f4d Hz\uff0c\u9ed8\u8ba4 2000\n    :param duration: \u6301\u7eed\u65f6\u95f4\uff0c\u5355\u4f4d \u79d2\uff0c\u9ed8\u8ba4 0.8\n    \"\"\"\n    # 1. \u6839\u636e\u9891\u7387\u548c\u65f6\u957f\u751f\u6210 PCM \u97f3\u9891\u6570\u636e\n    sample_rate = 48000\n    num_samples = int(sample_rate * duration)\n    amplitude = 32767   # \u6700\u5927\u97f3\u91cf\n    pcm = bytearray()\n\n    for i in range(num_samples):\n        t = i \/ sample_rate\n        sample = amplitude * math.sin(2 * math.pi * freq * t)\n        pcm.extend(struct.pack('<h', int(sample)))\n\n    # 2. \u64ad\u653e\u751f\u6210\u7684 PCM \u6570\u636e\uff08\u975e\u963b\u585e\uff0c\u786c\u4ef6\u7acb\u5373\u5f00\u59cb\u8f93\u51fa\uff09\n    _player.play(bytes(pcm))\n\n    # 3. \u963b\u585e\u7b49\u5f85\u64ad\u653e\u7ed3\u675f\n    time.sleep_ms(int(duration * 1000))\n\ndef scan_apriltag(resolution=(2560, 1440), win_w=400, win_h=300, step=200, scan_y=0):\n    \"\"\"\n    \u62cd\u6444\u4e00\u5f20\u7167\u7247\uff0c\u4ece\u5de6\u5230\u53f3\u6ed1\u52a8\u7a97\u53e3\u68c0\u6d4b AprilTag\u3002\n    \u5b8c\u5168\u5185\u90e8\u7ba1\u7406\u6444\u50cf\u5934\uff0c\u4e0d\u4f9d\u8d56\u5916\u90e8 camera \u5bf9\u8c61\u3002\n\n    \u53c2\u6570:\n        resolution: (width, height) \u6444\u50cf\u5934\u91c7\u96c6\u5206\u8fa8\u7387\uff0c\u9ed8\u8ba4 (2560,1440)\n        win_w, win_h: \u6ed1\u52a8\u7a97\u53e3\u5c3a\u5bf8\uff0c\u9ed8\u8ba4 400x300\n        step: \u6c34\u5e73\u6ed1\u52a8\u6b65\u957f\uff08\u50cf\u7d20\uff09\uff0c\u9ed8\u8ba4 200\n        scan_y: \u5782\u76f4\u8d77\u59cb\u4f4d\u7f6e\uff08\u9876\u90e8\u504f\u79fb\uff09\uff0c\u9ed8\u8ba4 0\n\n    \u8fd4\u56de:\n        \u82e5\u68c0\u6d4b\u5230: dict {\n            'id': int,\n            'family': str,\n            'corners': [(x1,y1), (x2,y2), (x3,y3), (x4,y4)],  # \u5728\u539f\u56fe\u4e0a\u7684\u5750\u6807\n            'center': (cx, cy)\n        }\n        \u82e5\u672a\u68c0\u6d4b\u5230: None\n    \"\"\"\n    # ---------- 1. \u5185\u90e8\u521d\u59cb\u5316\u6444\u50cf\u5934\uff08\u6307\u5b9a\u5206\u8fa8\u7387\uff09----------\n    cam = camera.Camera(resolution[0], resolution[1])\n    time.sleep(0.5)  # \u7b49\u5f85\u4f20\u611f\u5668\u7a33\u5b9a\n    full_img = cam.read()  # \u62cd\u6444\u4e00\u5e27\n    # \u62cd\u6444\u5b8c\u6210\u540e\u7acb\u5373\u91ca\u653e\u6444\u50cf\u5934\u8d44\u6e90\uff08\u907f\u514d\u5360\u7528\u5185\u5b58\u548c\u51b2\u7a81\uff09\n    del cam\n    # \u6ce8\u610f\uff1a\u67d0\u4e9b\u56fa\u4ef6\u53ef\u80fd\u6ca1\u6709\u663e\u5f0f\u7684 close \u65b9\u6cd5\uff0cdel \u4f1a\u89e6\u53d1 __del__ \u91ca\u653e\n\n    # ---------- 2. \u6ed1\u52a8\u7a97\u53e3\u626b\u63cf ----------\n    width, height = resolution\n    max_x = width - win_w\n    x_start = 0\n    detected = False\n    result = None\n\n    while x_start <= max_x and not detected:\n        # \u88c1\u526a\u5f53\u524d\u7a97\u53e3\n        window_img = full_img.crop(x_start, scan_y, win_w, win_h)\n        # \u5b9e\u65f6\u663e\u793a\u5f53\u524d\u626b\u63cf\u7a97\u53e3\uff08\u4fbf\u4e8e\u89c2\u5bdf\u8fdb\u5ea6\uff09\n        disp.show(window_img)\n\n        tags = window_img.find_apriltags(families=image.ApriltagFamilies.TAG36H11)\n        if tags:\n            detected = True\n            a = tags[0]  # \u53ea\u53d6\u7b2c\u4e00\u4e2a\n            # \u7a97\u53e3\u5185\u5750\u6807\u8f6c\u6362\u5230\u539f\u56fe\u5750\u6807\n            corners_win = a.corners()\n            orig_corners = [(int(cx + x_start), int(cy + scan_y)) for (cx, cy) in corners_win]\n            cx_win = int(a.x() + a.w() \/ 2)\n            cy_win = int(a.y() + a.h() \/ 2)\n            orig_center = (cx_win + x_start, cy_win + scan_y)\n\n            # \u5728\u539f\u56fe\u4e0a\u7ed8\u5236\u6807\u8bb0\n            for i in range(4):\n                x1, y1 = orig_corners[i]\n                x2, y2 = orig_corners[(i+1)%4]\n                full_img.draw_line(x1, y1, x2, y2, image.COLOR_RED, thickness=2)\n            full_img.draw_circle(orig_center[0], orig_center[1], 5, image.COLOR_GREEN, thickness=2)\n\n            ox_text = int(a.x() + a.w() + x_start)\n            oy_text = int(a.y() + scan_y)\n            full_img.draw_rect(ox_text, oy_text, 100, 30, image.COLOR_BLACK, thickness=-1)\n            full_img.draw_string(ox_text, oy_text, f\"id: {a.id()}\", image.COLOR_WHITE)\n            full_img.draw_string(ox_text, oy_text+15, f\"family: {a.family()}\", image.COLOR_WHITE)\n\n            result = a.id()\n\n            # result = {\n            #     'id': a.id(),\n            #     'family': a.family(),\n            #     'corners': orig_corners,\n            #     'center': orig_center\n            # }\n            break\n        else:\n            x_start += step\n\n    # ---------- 3. \u663e\u793a\u6700\u7ec8\u7ed3\u679c\uff08\u7f29\u653e\u81f3\u5c4f\u5e55\uff09----------\n    if not detected:\n        full_img.draw_string(50, 50, \"No AprilTag found\", image.COLOR_RED)\n\n    # \u7f29\u653e\u56fe\u50cf\u4ee5\u9002\u5e94\u663e\u793a\u5c4f\n    if full_img.width() != disp.width() or full_img.height() != disp.height():\n        full_img = full_img.resize(disp.width(), disp.height())\n    disp.show(full_img)\n\n    return result\n\ndef clamp_abs(value: float) -> float:\n    if value > 0:\n        return max(value, 1.0)\n    elif value < 0:\n        return min(value, -1.0)\n    else:\n        # value == 0 \u7684\u60c5\u51b5\uff0c\u6309\u89c4\u5219\u5f52\u4e3a 1.0\n        return 1.0\n\ndef execuseTurn(angle: float = 0.0, speed: int = 10, offset: float = 0.5):\n    \"\"\"\n    \u76f8\u5bf9\u65cb\u8f6c\u6307\u5b9a\u89d2\u5ea6\uff08\u5355\u4f4d\uff1a\u5ea6\uff09\n    angle  > 0  \u987a\u65f6\u9488\uff08\u6216\u53d6\u51b3\u4e8e\u4f60\u7684\u5e95\u76d8\u5b9a\u4e49\uff09\n    speed      \u6700\u5927\u901f\u5ea6\uff08int\uff0c\u7edd\u5bf9\u503c \u2264 speed\uff09\n    offset     \u5141\u8bb8\u7684\u505c\u6b62\u8bef\u5dee\u8303\u56f4\uff08\u5ea6\uff09\n    \"\"\"\n    # ---- \u521d\u59cb\u5316\uff0c\u83b7\u5f97\u5f53\u524d\u671d\u5411 ----\n    data = sensor.read_all(calib_gryo=True, radian=True)\n    t0 = time.ticks_s()\n    attitude = ahrs_filter.get_angle(data.acc, data.gyro, data.mag, 0.01, radian=False)\n    initial_yaw = attitude.z          # \u8bb0\u5f55\u8d77\u59cb\u504f\u822a\u89d2\n    last_time = t0\n\n    # \u8ba1\u7b97\u76ee\u6807\u7edd\u5bf9\u89d2\u5ea6\uff0c\u9650\u5236\u5728 [-180, 180)\n    target_absolute = initial_yaw + angle\n    while target_absolute >= 180:\n        target_absolute -= 360\n    while target_absolute < -180:\n        target_absolute += 360\n\n    # \u6bd4\u4f8b\u7cfb\u6570\uff0c\u4fdd\u6301 30\u00b0 \u5bf9\u5e94\u6ee1\u901f\u7684\u8bbe\u5b9a\uff08\u53ef\u8c03\uff09\n    Kp = speed \/ 30.0\n\n    while True:\n        data = sensor.read_all(calib_gryo=True, radian=True)\n        t = time.ticks_s()\n        dt = t - last_time\n        last_time = t\n\n        attitude = ahrs_filter.get_angle(data.acc, data.gyro, data.mag, dt, radian=False)\n        current_yaw = attitude.z\n\n        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}\")\n\n        # \u8bef\u5dee = \u76ee\u6807\u7edd\u5bf9\u89d2\u5ea6 - \u5f53\u524d\u504f\u822a\uff0c\u5e76\u89c4\u8303\u5230 [-180, 180)\n        error = target_absolute - current_yaw\n        while error > 180:\n            error -= 360\n        while error < -180:\n            error += 360\n\n        # \u8fdb\u5165\u5141\u8bb8\u8303\u56f4\u5219\u505c\u8f66\u9000\u51fa\n        if abs(error) <= offset:\n            send_speed_to_stm32(0, 0)\n            print(f\"\u65cb\u8f6c\u5b8c\u6210\uff0c\u5269\u4f59\u8bef\u5dee: {error:.2f}\u00b0\")\n            break\n\n        # \u6bd4\u4f8b\u901f\u5ea6\uff0c\u8bef\u5dee\u5927\u901f\u5ea6\u5feb\uff0c\u8bef\u5dee\u5c0f\u901f\u5ea6\u6162\n        motor_speed = Kp * error\n        if motor_speed > speed:\n            motor_speed = speed\n        elif motor_speed < -speed:\n            motor_speed = -speed\n\n        motor_speed  = clamp_abs(motor_speed)\n\n        left_speed = int(motor_speed)\n        right_speed = int(motor_speed)   # \u53f3\u8f6e\u53d6\u53cd\uff0c\u5b9e\u73b0\u539f\u5730\u65cb\u8f6c\n        send_speed_to_stm32(-left_speed, -right_speed)\n\ndef blindMove(duration = 1000, speed = 20):\n    send_speed_to_stm32(speed, -speed)\n    send_speed_to_stm32(speed, -speed)\n    send_speed_to_stm32(speed, -speed)\n    send_speed_to_stm32(speed, -speed)\n    send_speed_to_stm32(speed, -speed)\n    send_speed_to_stm32(speed, -speed)\n    time.sleep_ms(duration \/ 2)\n\n\ndef is_in_button(x, y, btn_pos):\n    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\n\n\nif __name__ == \"__main__\":\n\n    # while True:\n    #     execuseTurn(90)\n    #     time.sleep_ms(1000)\n    #     execuseTurn(-90)\n    #     time.sleep_ms(1000)\n\n    found_id = 0\n    isLabelRequired = 0\n    tag01 = 0\n    tag02 = 0\n    isMode = 0\n    cam = camera.Camera(320, 240)\n\n    change_label = \"CHANGE\"\n    size0 = image.string_size(change_label, scale=1.0, thickness=1)\n    change_btn_pos = [150, 0, 8*2 + size0.width(),  size0.height()]\n\n    odd_label = \" O D D \"\n    even_label = \" E V E N \"\n\n    size1 = image.string_size(odd_label, scale=3.0, thickness=2)\n    odd_btn_pos = [8, 30, 8*2 + size1.width(), 30 + size1.height()]\n\n    size2 = image.string_size(even_label, scale=3.0, thickness=2)\n    even_btn_pos = [8, odd_btn_pos[3] + 50, 8*2 + size2.width(), 30 + size2.height()]   # \u4e0e odd \u5e95\u90e8\u7559 20 \u95f4\u9699\n\n    left_label = \" L E F T \"\n    right_label = \" R I G H T \"\n\n    size3 = image.string_size(left_label, scale=3.0, thickness=2)\n    left_btn_pos = [8, 30, 8*2 + size3.width(), 30 + size3.height()]\n\n    size4 = image.string_size(right_label, scale=3.0, thickness=2)\n    right_btn_pos = [8, left_btn_pos[3] + 50, 8*2 + size4.width(), 30 + size4.height()]   # \u4e0e odd \u5e95\u90e8\u7559 20 \u95f4\u9699\n\n    isChangePressed = False\n    while True:\n        \n        img = cam.read()\n        \n        img.draw_string(150, 0, change_label, image.COLOR_WHITE, scale = 1.0, thickness = 1)\n        img.draw_rect(change_btn_pos[0], change_btn_pos[1], change_btn_pos[2], change_btn_pos[3],  image.COLOR_RED, 1)\n\n        if isMode == 0 :\n            # \u753b Odd \u6309\u94ae\u6587\u5b57\u548c\u77e9\u5f62\n            img.draw_string(odd_btn_pos[0], odd_btn_pos[1] + 15, odd_label, image.COLOR_WHITE, scale=3.0, thickness=2)\n            img.draw_rect(odd_btn_pos[0], odd_btn_pos[1], odd_btn_pos[2], odd_btn_pos[3], image.COLOR_WHITE, 2)\n            \n            # \u753b Even \u6309\u94ae\u6587\u5b57\u548c\u77e9\u5f62\uff08\u4e24\u8005\u4f4d\u7f6e\u5bf9\u9f50\uff09\n            img.draw_string(even_btn_pos[0], even_btn_pos[1] + 15, even_label, image.COLOR_WHITE, scale=3.0, thickness=2)\n            img.draw_rect(even_btn_pos[0], even_btn_pos[1], even_btn_pos[2], even_btn_pos[3], image.COLOR_WHITE, 2)\n        else:\n            # \u753b Odd \u6309\u94ae\u6587\u5b57\u548c\u77e9\u5f62\n            img.draw_string(left_btn_pos[0], left_btn_pos[1] + 15, left_label, image.COLOR_WHITE, scale=3.0, thickness=2)\n            img.draw_rect(left_btn_pos[0], left_btn_pos[1], left_btn_pos[2], left_btn_pos[3], image.COLOR_WHITE, 2)\n            \n            # \u753b Even \u6309\u94ae\u6587\u5b57\u548c\u77e9\u5f62\uff08\u4e24\u8005\u4f4d\u7f6e\u5bf9\u9f50\uff09\n            img.draw_string(right_btn_pos[0], right_btn_pos[1] + 15, right_label, image.COLOR_WHITE, scale=3.0, thickness=2)\n            img.draw_rect(right_btn_pos[0], right_btn_pos[1], right_btn_pos[2], right_btn_pos[3], image.COLOR_WHITE, 2)\n        \n        x, y, pressed = ts.read()\n\n        if pressed and is_in_button(x, y, change_btn_pos) and isChangePressed is False:\n            isMode = 1 - isMode\n            isChangePressed = True\n\n        if pressed and is_in_button(x, y, odd_btn_pos):\n            isLabelRequired = 0\n            break\n        if pressed and is_in_button(x, y, even_btn_pos):\n            isLabelRequired = 1\n            break\n        \n        if pressed and is_in_button(x, y, left_btn_pos):\n            isLabelRequired = 0\n            break\n        if pressed and is_in_button(x, y, right_btn_pos):\n            isLabelRequired = 1\n            break\n        if not pressed:\n            isChangePressed = False\n\n        disp.show(img)\n    del cam\n\n    execuseTurn(-90)\n\n    while True:\n        ret = scan_apriltag()\n        if ret is not None:\n            img02 = image.Image(disp.width(), disp.height())\n            found_id = ret\n            label = f\":{ret}\"\n            img02.draw_string(8, 30, label, image.COLOR_WHITE, scale = 20.0, thickness = 14)\n            disp.show(img02)\n            tag01 = int(ret)\n            beep()\n            break\n\n    execuseTurn(90)\n\n    cam = camera.Camera(320, 240)\n    while True:\n        t = time.time_ms()\n        img = cam.read()\n        if img is None: \n            continue\n        offset, lost_cnt, recovery_count, lose_down = follower.update(img)\n\n        l, r = calc_speed(offset)\n        if lost_cnt > 0:\n            send_speed_to_stm32(l, -r)\n            time.sleep_ms(600)\n            send_speed_to_stm32(0, 0)\n            break\n        else:\n            send_speed_to_stm32(l, -r)\n\n        disp.show(img)\n        print(\"FPS =\", int(1000 \/ (time.time_ms() - t)), \"Offset:\", offset, \"Lost:\", lost_cnt)\n    del cam\n\n    while True:\n        ret = scan_apriltag()\n        if ret is not None:\n            img02 = image.Image(disp.width(), disp.height())\n            found_id = ret\n            label = f\":{ret}\"\n            img02.draw_string(8, 30, label, image.COLOR_WHITE, scale = 20.0, thickness = 14)\n            disp.show(img02)\n            tag02 = int(ret)\n            beep()\n            break\n    \n    # blindMove(duration = 2000)\n\n    if isMode == 1:\n        if isLabelRequired == 1:\n            if tag02 % 2 == 1:\n                isLabelRequired = 0\n        elif tag01 % 2 == 0:\n            isLabelRequired = 1\n\n\n\n    if isLabelRequired == 1:\n        execuseTurn(-55)\n        blindMove(duration = 2300)\n        cam = camera.Camera(320, 240)\n        finishTimerMs = 500\n        while True:\n            t = time.time_ms()\n            img = cam.read()\n            if img is None: \n                continue\n            offset, lost_cnt, recovery_count , lose_down = follower.update(img)\n\n            l, r = calc_speed(offset)\n            if recovery_count > 0 and lost_cnt > recovery_count:\n                send_speed_to_stm32(l, -r)\n                send_speed_to_stm32(0, 0)\n                execuseTurn(83)\n                break\n            else:\n                send_speed_to_stm32(l, -r)\n\n            send_speed_to_stm32(l, -r)\n\n            disp.show(img)\n            print(\"FPS =\", int(1000 \/ (time.time_ms() - t)), \"Offset:\", offset, \"Lost:\", lost_cnt)\n\n        blindMove(duration = 2600)\n        \n        while True:\n            t = time.time_ms()\n            img = cam.read()\n            if img is None: \n                continue\n            offset, lost_cnt, recovery_count, lose_down = follower.update(img)\n\n            if lose_down is False:\n                finishTimerMs -= int((time.time_ms() - t))\n                lastTime = t\n\n            l, r = calc_speed(offset)\n\n            print(f\"{finishTimerMs}\")\n\n            if finishTimerMs < 0:\n                send_speed_to_stm32(0, 0)\n                break\n            send_speed_to_stm32(l, -r)\n\n            disp.show(img)\n            print(\"FPS =\", int(1000 \/ (time.time_ms() - t)), \"Offset:\", offset, \"Lost:\", lost_cnt)\n        \n        del cam\n    else:\n        finishTimerMs = 300\n        execuseTurn(-10)\n        blindMove(duration = 3500)\n        execuseTurn(85)\n        cam = camera.Camera(320, 240)\n        blindMove(duration = 200)\n\n        while True:\n            t = time.time_ms()\n            img = cam.read()\n            if img is None: \n                continue\n            offset, lost_cnt, recovery_count , lose_down = follower.update(img)\n\n            l, r = calc_speed(offset)\n            if recovery_count > 0 and lost_cnt > recovery_count:\n                send_speed_to_stm32(0, 0)\n                break\n            send_speed_to_stm32(l, -r)\n\n            disp.show(img)\n            print(\"FPS =\", int(1000 \/ (time.time_ms() - t)), \"Offset:\", offset, \"Lost:\", lost_cnt)\n\n        execuseTurn(45)\n        blindMove(duration = 5500)\n        execuseTurn(-100)\n\n        while True:\n            t = time.time_ms()\n            img = cam.read()\n            if img is None: \n                continue\n            offset, lost_cnt, recovery_count, lose_down = follower.update(img)\n\n            if lose_down is False:\n                finishTimerMs -= int((time.time_ms() - t))\n                lastTime = t\n\n            l, r = calc_speed(offset)\n        \n            print(f\"{finishTimerMs}\")\n\n            if finishTimerMs < 0:\n                send_speed_to_stm32(0, 0)\n                break\n            send_speed_to_stm32(l, -r)\n\n            disp.show(img)\n            print(\"FPS =\", int(1000 \/ (time.time_ms() - t)), \"Offset:\", offset, \"Lost:\", lost_cnt)\n        \n        del cam\n<\/code><\/pre>\n\n\n\n
STM32\u5e95\u5ea7<\/h2>\n\n\n\n
main.c<\/h3>\n\n\n\n
\/* USER CODE BEGIN Header *\/\n\/**\n  ******************************************************************************\n  * @file           : main.c\n  * @brief          : Main program body\n  ******************************************************************************\n  * @attention\n  *\n  * Copyright (c) 2026 STMicroelectronics.\n  * All rights reserved.\n  *\n  * This software is licensed under terms that can be found in the LICENSE file\n  * in the root directory of this software component.\n  * If no LICENSE file comes with this software, it is provided AS-IS.\n  *\n  ******************************************************************************\n  *\/\n\/* USER CODE END Header *\/\n\/* Includes ------------------------------------------------------------------*\/\n#include \"main.h\"\n#include \"tim.h\"\n#include \"usart.h\"\n#include \"gpio.h\"\n\n\/* Private includes ----------------------------------------------------------*\/\n\/* USER CODE BEGIN Includes *\/\n\n#include \"motor.h\"\n\n\/* USER CODE END Includes *\/\n\n\/* Private typedef -----------------------------------------------------------*\/\n\/* USER CODE BEGIN PTD *\/\n\n\/* USER CODE END PTD *\/\n\n\/* Private define ------------------------------------------------------------*\/\n\/* USER CODE BEGIN PD *\/\n\n\/* USER CODE END PD *\/\n\n\/* Private macro -------------------------------------------------------------*\/\n\/* USER CODE BEGIN PM *\/\n\n\/* USER CODE END PM *\/\n\n\/* Private variables ---------------------------------------------------------*\/\n\n\/* USER CODE BEGIN PV *\/\n\n\/\/ UART3       \u0631   \nuint8_t rx_buffer;              \/\/    \u05bd\u06bd  \u057b   \nstatic uint8_t uart3_rx_step = 0;\nstatic uint8_t uart3_rx_checksum = 0;\nstatic uint8_t uart3_rx_checksum_rcv = 0;\nstatic uint8_t uart3_rx_int16_1_low = 0;\nstatic uint8_t uart3_rx_int16_1_high = 0;\nstatic uint8_t uart3_rx_int16_2_low = 0;\nstatic uint8_t uart3_rx_int16_2_high = 0;\n\nint16_t left_speed = 0;\nint16_t right_speed = 0;\n\n  \n\/\/float base_speed = 0.679325;     \n\/\/float kp_turn = 0.35f;        \n\n\/* USER CODE END PV *\/\n\n\/* Private function prototypes -----------------------------------------------*\/\nvoid SystemClock_Config(void);\n\/* USER CODE BEGIN PFP *\/\n\n\/* USER CODE END PFP *\/\n\n\/* Private user code ---------------------------------------------------------*\/\n\/* USER CODE BEGIN 0 *\/\n\n#include <string.h>\n#include <stdio.h>\n\n#define RX_BUFFER_SIZE 64\n\n\/* USER CODE END 0 *\/\n\n\/**\n  * @brief  The application entry point.\n  * @retval int\n  *\/\nint main(void)\n{\n\n  \/* USER CODE BEGIN 1 *\/\n\n  \/* USER CODE END 1 *\/\n\n  \/* MCU Configuration--------------------------------------------------------*\/\n\n  \/* Reset of all peripherals, Initializes the Flash interface and the Systick. *\/\n  HAL_Init();\n\n  \/* USER CODE BEGIN Init *\/\n\n  \/* USER CODE END Init *\/\n\n  \/* Configure the system clock *\/\n  SystemClock_Config();\n\n  \/* USER CODE BEGIN SysInit *\/\n\n  \/* USER CODE END SysInit *\/\n\n  \/* Initialize all configured peripherals *\/\n  MX_GPIO_Init();\n  MX_TIM1_Init();\n  MX_TIM2_Init();\n  MX_TIM3_Init();\n  MX_TIM4_Init();\n  MX_USART3_UART_Init();\n  \/* USER CODE BEGIN 2 *\/\n\tHAL_UART_Receive_IT(&huart3, &rx_buffer, 1);\n\tmotorInit();\n\n\/\/\tHAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);\n\/\/\t__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 500);\n\n\n  \/* USER CODE END 2 *\/\n\n  \/* Infinite loop *\/\n  \/* USER CODE BEGIN WHILE *\/\n\nwhile (1)\n  {\n    \/* USER CODE END WHILE *\/\n\n    \/* USER CODE BEGIN 3 *\/\n    \n    \/\/ \u8bbe\u7f6e\u7535\u673a\u76ee\u6807\u901f\u5ea6\uff08PID\u901f\u5ea6\u95ed\u73af\u4f1a\u81ea\u52a8\u8c03\u8282PWM\uff09\n    setSpeedL(left_speed);\n    setSpeedR(right_speed);\n\t\n\/\/\tHAL_Delay(5);  \/\/ 10ms\u63a7\u5236\u5468\u671f\uff0c\u4e0ePID\u5468\u671f\u5339\u914d \n\t  \n  }\n  \/* USER CODE END 3 *\/\n}\n\n\/**\n  * @brief System Clock Configuration\n  * @retval None\n  *\/\nvoid SystemClock_Config(void)\n{\n  RCC_OscInitTypeDef RCC_OscInitStruct = {0};\n  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};\n\n  \/** Initializes the RCC Oscillators according to the specified parameters\n  * in the RCC_OscInitTypeDef structure.\n  *\/\n  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;\n  RCC_OscInitStruct.HSIState = RCC_HSI_ON;\n  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;\n  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;\n  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;\n  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;\n  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)\n  {\n    Error_Handler();\n  }\n\n  \/** Initializes the CPU, AHB and APB buses clocks\n  *\/\n  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK\n                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;\n  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;\n  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;\n  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV8;\n  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV8;\n\n  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)\n  {\n    Error_Handler();\n  }\n}\n\n\/* USER CODE BEGIN 4 *\/\nvoid HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)\n{\n    if(huart->Instance == USART3)\n    {\n       \n        switch(uart3_rx_step)\n        {\n            case 0: \n                if(rx_buffer == 0xAA)\n                {\n                    uart3_rx_step = 1;\n                    uart3_rx_checksum = 0xAA;\n                    \n                 \n                    HAL_UART_Transmit(&huart3, &rx_buffer, 1, 100);\n                }\n                break;\n                \n            case 1: \n                uart3_rx_int16_1_low = rx_buffer;\n                uart3_rx_checksum += rx_buffer;\n                uart3_rx_step = 2;\n                break;\n                \n            case 2: \n                uart3_rx_int16_1_high = rx_buffer;\n                uart3_rx_checksum += rx_buffer;\n                uart3_rx_step = 3;\n                break;\n                \n            case 3: \n                uart3_rx_int16_2_low = rx_buffer;\n                uart3_rx_checksum += rx_buffer;\n                uart3_rx_step = 4;\n                break;\n                \n            case 4:  \n                uart3_rx_int16_2_high = rx_buffer;\n                uart3_rx_checksum += rx_buffer;\n                uart3_rx_step = 5;\n                break;\n                \n            case 5: \n                uart3_rx_checksum_rcv = rx_buffer;\n                if((uart3_rx_checksum & 0xFF) == uart3_rx_checksum_rcv)\n                {\n                    int16_t value1 = (int16_t)(uart3_rx_int16_1_low | (uart3_rx_int16_1_high << 8));\n                    int16_t value2 = (int16_t)(uart3_rx_int16_2_low | (uart3_rx_int16_2_high << 8));\n                    \n                    left_speed = value1;\n                    right_speed = value2;\n                    \n                   \n                    uint8_t response[6] = {\n                        0xAA,\n                        uart3_rx_int16_1_low,\n                        uart3_rx_int16_1_high,\n                        uart3_rx_int16_2_low,\n                        uart3_rx_int16_2_high,\n                        uart3_rx_checksum_rcv\n                    };\n                    HAL_UART_Transmit(&huart3, response, 6, 100);\n                }\n                uart3_rx_step = 0;\n                break;\n        }\n        \n        HAL_UART_Receive_IT(&huart3, &rx_buffer, 1);\n    }\n}\n\/* USER CODE END 4 *\/\n\n\/**\n  * @brief  This function is executed in case of error occurrence.\n  * @retval None\n  *\/\nvoid Error_Handler(void)\n{\n  \/* USER CODE BEGIN Error_Handler_Debug *\/\n  \/* User can add his own implementation to report the HAL error return state *\/\n  __disable_irq();\n  while (1)\n  {\n  }\n  \/* USER CODE END Error_Handler_Debug *\/\n}\n#ifdef USE_FULL_ASSERT\n\/**\n  * @brief  Reports the name of the source file and the source line number\n  *         where the assert_param error has occurred.\n  * @param  file: pointer to the source file name\n  * @param  line: assert_param error line source number\n  * @retval None\n  *\/\nvoid assert_failed(uint8_t *file, uint32_t line)\n{\n  \/* USER CODE BEGIN 6 *\/\n  \/* User can add his own implementation to report the file name and line number,\n     ex: printf(\"Wrong parameters value: file %s on line %d\\r\\n\", file, line) *\/\n  \/* USER CODE END 6 *\/\n}\n#endif \/* USE_FULL_ASSERT *\/\n<\/code><\/pre>\n\n\n\n
motor.c<\/h3>\n\n\n\n
#include \"motor.h\"\n#include \"tim.h\"\n\n#define ABS(x) ((x) > 0) ? (x) : -(x)\n\n#define PULSE2SPEED 0.16983125\n#define PWM_MAX 999\n#define PWM_MIN -999\n#define INTEGRAL_LIMIT 99.0f\n\n#define PWM_TIM htim3\n#define PID_TIM htim4\n#define COUNTER_A_TIM htim1\n#define COUNTER_B_TIM htim2\n\n\/\/ class GuoguoILoveU\n\/\/ \u7ebf\u6570 13 \u56db\u500d\u9891\u7387 52\n\/\/ 10ms 52 * 0.01 = 0.52\n\/\/ 28.26cm \n\/\/ 27.173 -> 5 0.27173\n\/\/ \nvolatile float debug_actualL = 0.0f;\nvolatile float debug_actualR = 0.0f;\nvolatile  int16_t testA = 0;\nvolatile int16_t testB = 0;\n\ntypedef struct {\n\tfloat p;\n\tfloat i;\n\tfloat d;\n\tfloat targetSpeed;\n\tfloat lastErr;\n\tfloat totalErr;\n\tint16_t pwm;\n} pid_element_t;\n\npid_element_t motorL = {\n\t.p = -40.0f,\n\t.i = -0.05f,\n\t.d = -60.0f,\n\t.targetSpeed = 10.0f,\n\t.lastErr = 0.0f,\n\t.totalErr = 0.0f,\n\t.pwm = 0\n};\n\npid_element_t motorR = {\n\t.p = -40.0f,\n\t.i = -0.05f,\n\t.d = -60.0f,\n\t.targetSpeed = 10.0f,\n\t\n\t.lastErr = 0.0f,\n\t.totalErr = 0.0f,\n\t.pwm = 0\n};\n\nvoid motorInit()\n{\n\t\/\/ \u542f\u52a8\u4e24\u4e2a PWM \u901a\u9053\uff08\u4f7f\u7528 TIM4 \u7684 CH1 \u548c CH2\uff09\n\tHAL_TIM_PWM_Start(&PWM_TIM, TIM_CHANNEL_1);  \/\/ \u5de6\u7535\u673a PWM (PB6)\n\tHAL_TIM_PWM_Start(&PWM_TIM, TIM_CHANNEL_2);  \/\/ \u53f3\u7535\u673a PWM (PB7)\n\n\t\/\/ \u8bbe\u7f6e\u5de6\u7535\u673a\u65b9\u5411\uff08\u6b63\u8f6c\uff09\n\tHAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);   \/\/ AIN1 \u9ad8\n\tHAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET); \/\/ AIN2 \u4f4e\n\n\t\/\/ \u8bbe\u7f6e\u53f3\u7535\u673a\u65b9\u5411\uff08\u6b63\u8f6c\uff09\n\tHAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);   \/\/ BIN1 \u9ad8\n\tHAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET); \/\/ BIN2 \u4f4e\n\n\t\/\/ \u8bbe\u7f6e\u4e24\u4e2a\u7535\u673a\u7684\u521d\u59cb\u5360\u7a7a\u6bd4\uff0830%\uff09\uff0c\u5bf9\u5e94\u6bd4\u8f83\u503c 300\uff08\u82e5 Period=999\uff09\n\t__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_1, 0); \/\/ \u5de6\u7535\u673a\n\t__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_2, 0); \/\/ \u53f3\u7535\u673a\n\t\n\tHAL_TIM_Encoder_Start(&COUNTER_A_TIM, TIM_CHANNEL_ALL);\/\/\u7f16\u7801\u5668\u542f\u52a8,\u5b9a\u65f6\u5668\u5f00\u59cb\u8ba1\u7b97\n\tHAL_TIM_Encoder_Start(&COUNTER_B_TIM, TIM_CHANNEL_ALL);\n\t\n\tHAL_TIM_Base_Start_IT(&PID_TIM);\n}\n\n\n\/**\n * @Breif \u8bbe\u7f6e\u5de6\u8f6ePWM\n * @Input pwm\u6570\u503c \u8303\u56f4 -1000 ~ 1000\n *\/\nvoid setL(int16_t pwm)\n{\n\tif(pwm > 1000 || pwm < -1000) return;\n\tif(pwm < 0) {\n\t\tHAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);\n\t\tHAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);\n\t} else {\n\t\tHAL_GPIO_WritePin(AIN1_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);\n\t\tHAL_GPIO_WritePin(AIN2_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);\n\t}\n\t__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_1, ABS(pwm));\n\ttestA = ABS(pwm);\n}\n\n\/**\n * @Breif \u8bbe\u7f6e\u53f3\u8f6ePWM\n * @Input pwm\u6570\u503c \u8303\u56f4 -1000 ~ 1000\n *\/\nvoid setR(int16_t pwm)\n{\n\tif(pwm > 1000 || pwm < -1000) return;\n\tif(pwm < 0) {\n\t\tHAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);\n\t\tHAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);\n\t} else {\n\t\tHAL_GPIO_WritePin(BIN1_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);\n\t\tHAL_GPIO_WritePin(BIN2_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);\n\t}\n\t__HAL_TIM_SET_COMPARE(&PWM_TIM, TIM_CHANNEL_2, ABS(pwm));\n\ttestB = ABS(pwm);\n}\n\n\/**\n * @Breif \u8bfb\u51fa\u5de6\u8f6e\u8109\u51b2\n * @output \u8f93\u51fa\u5de6\u8f6e\u8109\u51b2\n *\/\nint16_t getL()\n{\n\tint16_t current = __HAL_TIM_GET_COUNTER(&COUNTER_A_TIM);\n\t__HAL_TIM_SET_COUNTER(&COUNTER_A_TIM,0);\n\treturn current;\n}\n\n\/**\n * @Breif \u8bfb\u51fa\u53f3\u8f6e\u8109\u51b2\n * @output \u8f93\u51fa\u53f3\u8f6e\u8109\u51b2\n *\/\nint16_t getR()\n{\n\tint16_t current = __HAL_TIM_GET_COUNTER(&COUNTER_B_TIM);\n\t__HAL_TIM_SET_COUNTER(&COUNTER_B_TIM,0);\n\treturn current;\n}\n\n\nvoid setSpeedL(float speed) {\n\tmotorL.targetSpeed = speed;\n}\n\nvoid setSpeedR(float speed) {\n\tmotorR.targetSpeed = -speed;\n}\n\nint16_t pidCalcUnit(pid_element_t* i, float err)\n{\n    i->pwm += (i->p * err + i->i * i->totalErr + i->d * (err - i->lastErr));\n    if (i->pwm > PWM_MAX) i->pwm = PWM_MAX;\n    if (i->pwm < PWM_MIN) i->pwm = PWM_MIN;\n    i->totalErr += err;\n    if (i->totalErr > INTEGRAL_LIMIT) i->totalErr = INTEGRAL_LIMIT;\n    if (i->totalErr < -INTEGRAL_LIMIT) i->totalErr = -INTEGRAL_LIMIT;\n    i->lastErr = err;\n    return i->pwm;\n}\n\nvoid pidCalc()\n{\n    \/\/ \u5148\u8bfb\u53d6\u8109\u51b2\u6570\u5e76\u8ba1\u7b97\u5b9e\u9645\u901f\u5ea6\n    int16_t pulseL = getL();\n    int16_t pulseR = getR();\n    \n    float actualL = PULSE2SPEED * pulseL;\n    float actualR = PULSE2SPEED * pulseR;\n    \n    \/\/ \u8d4b\u503c\u7ed9\u8c03\u8bd5\u53d8\u91cf\n    debug_actualL = actualL;\n    debug_actualR = actualR;\n    \n    \/\/ \u8ba1\u7b97\u8bef\u5dee\n    float errL = actualL - motorL.targetSpeed ;\n    float errR = actualR - motorR.targetSpeed;\n    \n    \/\/ \u6267\u884c PID \u5e76\u8bbe\u7f6e\u7535\u673a\n    setL(pidCalcUnit(&motorL, errL));\n    setR(pidCalcUnit(&motorR, errR));\n}\n\n\nvoid HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)\n{\n\tpidCalc();\n}\n\n<\/code><\/pre>\n\n\n\n
<\/p>\n","protected":false},"excerpt":{"rendered":"
\u60ca\u5fc3\u52a8\u9b44\u7684\u5c0f\u8f66\u8c03\u8bd5\u5440 Maixcam \u9898\u76ee\u4e00\u548c\u4e8c \u8fd9\u4e9b\u9898\u76ee\u90fd\u8981\u6539\u52a8\u4e00\u4e0blab \u9898\u76ee\u4e09 STM32\u5e95\u5ea7 main […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1,3],"tags":[],"class_list":["post-610","post","type-post","status-publish","format-standard","hentry","category-uncategorized","category-code"],"_links":{"self":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/610","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=610"}],"version-history":[{"count":3,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/610\/revisions"}],"predecessor-version":[{"id":615,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/610\/revisions\/615"}],"wp:attachment":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=610"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=610"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=610"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}