{"id":111780,"date":"2026-10-01T20:41:05","date_gmt":"2026-10-01T12:41:05","guid":{"rendered":"https:\/\/www.wsisp.com\/helps\/111780.html"},"modified":"2026-10-01T20:41:05","modified_gmt":"2026-10-01T12:41:05","slug":"%e3%80%90%e8%8a%b1%e9%9b%95%e5%ad%a6%e7%bc%96%e7%a8%8b%e3%80%91arduino-bldc-%e4%b9%8b%e6%9c%ba%e5%99%a8%e4%ba%ba%e5%9b%ad%e5%8c%ba%e5%b7%a1%e9%80%bb%e8%b7%9f%e9%9a%8f%e8%b7%af%e5%be%84%e4%bc%98","status":"publish","type":"post","link":"https:\/\/www.wsisp.com\/helps\/111780.html","title":{"rendered":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f"},"content":{"rendered":"

\"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"
\nArduino BLDC \u4e4b\u56ed\u533a\u5de1\u903b\u8ddf\u968f + \u8def\u5f84\u4f18\u5316\u7684\u672c\u8d28，\u662f\u628a\u201c\u56fa\u5b9a\u5de1\u903b\u8def\u5f84\u3001\u52a8\u6001\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u3001\u5b9e\u65f6\u907f\u969c\u4e0e\u8def\u5f84\u91cd\u89c4\u5212\u201d\u6574\u5408\u5230\u4e00\u4e2a\u5206\u5c42\u95ed\u73af\u4e2d：\u4e0a\u4f4d\u673a\u8d1f\u8d23\u5730\u56fe\u3001\u8def\u5f84\u4e0e\u76ee\u6807\u8bc6\u522b，\u4e0b\u4f4d\u673a\u8d1f\u8d23 BLDC \u5dee\u901f\u95ed\u73af\u3001\u8ddf\u968f\u63a7\u5236\u548c\u5c40\u90e8\u907f\u969c\u3002 \u5b83\u65e2\u8981\u80fd\u6309\u9884\u8bbe\u8def\u7ebf\u5de1\u822a，\u4e5f\u8981\u80fd\u5728\u9047\u5230\u884c\u4eba\u3001\u8f66\u8f86\u3001\u4e34\u65f6\u969c\u788d\u6216\u5b89\u4fdd\u4eba\u5458\u79fb\u52a8\u65f6\u5e73\u6ed1\u5207\u6362\u884c\u4e3a，\u800c\u4e0d\u662f\u7b80\u5355\u5730\u5728\u201c\u5de1\u903b\u201d\u548c\u201c\u8ddf\u968f\u201d\u4e4b\u95f4\u786c\u5207\u6362\u3002<\/p>\n

\u4e3b\u8981\u7279\u70b9
\n. \u5de1\u903b\u8ddf\u968f\u53cc\u6a21\u5f0f\u534f\u540c
\n\u7cfb\u7edf\u901a\u5e38\u5177\u5907\u81f3\u5c11\u4e09\u79cd\u884c\u4e3a\u6a21\u5f0f：\u9884\u8bbe\u8def\u7ebf\u5de1\u903b\u3001\u4eba\u5458\u8ddf\u968f\u3001\u907f\u969c\u7ed5\u884c\u3002\u5de1\u903b\u6a21\u5f0f\u4f9d\u8d56\u5168\u5c40\u8def\u5f84\u548c\u5b9a\u4f4d，\u4fdd\u8bc1\u8986\u76d6\u56ed\u533a\u91cd\u70b9\u70b9\u4f4d；\u8ddf\u968f\u6a21\u5f0f\u5219\u6839\u636e\u76ee\u6807\u76f8\u5bf9\u8ddd\u79bb\u548c\u65b9\u4f4d\u89d2，\u8f93\u51fa\u7ebf\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\u6307\u4ee4，\u4f7f\u673a\u5668\u4eba\u4fdd\u6301\u5b89\u5168\u8ddf\u968f\u95f4\u8ddd；\u5f53\u4e24\u79cd\u4efb\u52a1\u51b2\u7a81\u65f6，\u7531\u6709\u9650\u72b6\u6001\u673a\u51b3\u5b9a\u4f18\u5148\u7ea7，\u4f8b\u5982\u907f\u969c\u4f18\u5148\u3001\u8ddf\u968f\u6b21\u4e4b\u3001\u5de1\u903b\u5de1\u822a\u6700\u4f4e\u3002
\n. \u5168\u5c40\u8def\u5f84\u4f18\u5316\u964d\u4f4e\u65e0\u6548\u7ed5\u884c
\n\u56ed\u533a\u73af\u5883\u901a\u5e38\u5b58\u5728\u957f\u76f4\u9053\u3001\u8def\u53e3\u3001\u7ed5\u884c\u533a\u57df\u548c\u4e34\u65f6\u5c01\u95ed\u8def\u6bb5\u3002\u5168\u5c40\u5c42\u53ef\u91c7\u7528 A*\u3001Dijkstra \u6216 Voronoi \u9aa8\u67b6\u56fe\u751f\u6210\u5de1\u903b\u8def\u5f84，\u518d\u901a\u8fc7\u62d0\u70b9\u7b5b\u9009\u3001\u8def\u5f84\u5e73\u6ed1\u548c\u822a\u70b9\u538b\u7f29\u51cf\u5c11\u5197\u4f59\u8f6c\u5411\u3002 \u5bf9\u4e8e Arduino \u8fd9\u7c7b\u8d44\u6e90\u53d7\u9650\u5e73\u53f0，\u66f4\u63a8\u8350\u7531\u4e0a\u4f4d\u673a\u8ba1\u7b97\u5168\u5c40\u8def\u5f84，\u4e0b\u4f4d\u673a\u53ea\u63a5\u6536\u822a\u70b9\u5e8f\u5217\u5e76\u6267\u884c\u8def\u5f84\u8ddf\u8e2a\u3002
\n. \u5c40\u90e8\u907f\u969c\u91c7\u7528 DWA \u6216\u52a8\u6001\u5206\u533a\u7b56\u7565
\n\u5168\u5c40\u8def\u5f84\u89e3\u51b3\u201c\u53bb\u54ea\u91cc\u201d，\u5c40\u90e8\u89c4\u5212\u89e3\u51b3\u201c\u73b0\u5728\u600e\u4e48\u8d70\u201d\u3002DWA \u5728\u901f\u5ea6\u7a7a\u95f4\u5185\u91c7\u6837 (v, \u03c9)，\u9884\u6d4b\u77ed\u65f6\u8f68\u8ff9\u5e76\u8bc4\u4ef7\u5b89\u5168\u6027\u3001\u76ee\u6807\u8d8b\u8fd1\u6027\u548c\u901f\u5ea6\u6548\u7387，\u9002\u5408\u5904\u7406\u7a81\u7136\u51fa\u73b0\u7684\u4eba\u5458\u3001\u7535\u52a8\u8f66\u6216\u4e34\u65f6\u5806\u653e\u7269\u3002 \u82e5\u7b97\u529b\u4e0d\u8db3，\u4e5f\u53ef\u91c7\u7528\u52a8\u6001\u5206\u533a\u907f\u969c：\u8fd1\u8ddd\u5371\u9669\u533a\u89e6\u53d1\u6025\u505c，\u4e2d\u8ddd\u9884\u8b66\u533a\u89e6\u53d1\u5c40\u90e8\u91cd\u89c4\u5212，\u8fdc\u8ddd\u5b89\u5168\u533a\u7ef4\u6301\u539f\u8def\u5f84\u3002
\n. \u8ddf\u968f\u63a7\u5236\u9700\u878d\u5408\u76ee\u6807\u611f\u77e5\u4e0e\u5e95\u76d8\u95ed\u73af
\n\u8ddf\u968f\u76ee\u6807\u53ef\u901a\u8fc7 UWB\/BLE \u6807\u7b7e\u3001\u89c6\u89c9\u8bc6\u522b\u3001\u6fc0\u5149\u96f7\u8fbe\u8f6e\u5ed3\u8ddf\u8e2a\u6216\u8d85\u58f0\u6ce2\u77e9\u9635\u5b9e\u73b0\u3002 \u76ee\u6807\u76f8\u5bf9\u4f4d\u59ff\u901a\u5e38\u5305\u62ec\u8ddd\u79bb\u3001\u65b9\u4f4d\u89d2\u548c\u76f8\u5bf9\u901f\u5ea6；\u63a7\u5236\u5668\u518d\u5c06\u5176\u8f6c\u5316\u4e3a\u671f\u671b\u7ebf\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\u3002BLDC \u5dee\u901f\u5e95\u76d8\u914d\u5408\u7f16\u7801\u5668\u6216\u8f6e\u6bc2\u7535\u673a\u970d\u5c14\u53cd\u9988，\u53ef\u5b9e\u73b0\u5de6\u53f3\u8f6e\u72ec\u7acb\u901f\u5ea6\u95ed\u73af，\u4f7f\u8ddf\u968f\u8f68\u8ff9\u66f4\u5e73\u6ed1\u3002
\n. \u5206\u5c42\u67b6\u6784\u9002\u914d Arduino \u751f\u6001
\n\u5178\u578b\u5b9e\u73b0\u4e3a：\u4e0a\u4f4d\u673a\u8fd0\u884c SLAM\u3001\u5168\u5c40\u89c4\u5212\u3001\u76ee\u6807\u8bc6\u522b\u548c\u4efb\u52a1\u8c03\u5ea6；\u4e0b\u4f4d\u673a\u8fd0\u884c IMU \u878d\u5408\u3001\u91cc\u7a0b\u8ba1\u3001DWA\/\u8ddf\u968f\u63a7\u5236\u3001BLDC FOC \u6216 PID \u901f\u5ea6\u95ed\u73af\u3002 Arduino Uno\/Nano \u9002\u5408\u505a\u8f7b\u91cf\u6267\u884c\u8282\u70b9；\u82e5\u52a0\u5165\u6fc0\u5149\u96f7\u8fbe\u3001\u89c6\u89c9\u6216 SLAM，\u5efa\u8bae\u4f7f\u7528 ESP32\u3001STM32\u3001\u6811\u8393\u6d3e\u6216 Jetson \u7cfb\u5217\u627f\u62c5\u611f\u77e5\u4e0e\u89c4\u5212\u3002<\/p>\n

\u5e94\u7528\u573a\u666f
\n\u56ed\u533a\u5b89\u9632\u5de1\u903b：\u6309\u56fa\u5b9a\u8def\u7ebf\u5de1\u67e5\u56f4\u5899\u3001\u51fa\u5165\u53e3\u3001\u505c\u8f66\u573a\u3001\u8bbe\u5907\u95f4，\u53d1\u73b0\u5f02\u5e38\u65f6\u81ea\u52a8\u8bb0\u5f55\u5e76\u4e0a\u62a5\u3002
\n\u5b89\u4fdd\u4eba\u5458\u4f34\u968f\u5de1\u903b：\u673a\u5668\u4eba\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u79fb\u52a8，\u643a\u5e26\u7167\u660e\u3001\u5bf9\u8bb2\u3001\u6444\u50cf\u6216\u68c0\u6d4b\u8bbe\u5907，\u51cf\u5c11\u4eba\u5458\u8d1f\u91cd\u3002
\n\u7269\u4e1a\/\u5382\u533a\u5de1\u68c0：\u5728\u5de5\u4e1a\u56ed\u533a\u3001\u7269\u6d41\u56ed\u533a\u3001\u6821\u56ed\u3001\u533b\u9662\u5916\u56f4\u6267\u884c\u591c\u95f4\u5de1\u903b\u3001\u73af\u5883\u76d1\u6d4b\u548c\u8bbe\u5907\u70b9\u68c0\u3002
\n\u4e34\u65f6\u6d3b\u52a8\u4fdd\u969c：\u5c55\u4f1a\u3001\u8d5b\u4e8b\u3001\u5927\u578b\u6d3b\u52a8\u73b0\u573a\u4eba\u6d41\u53d8\u5316\u5feb，\u673a\u5668\u4eba\u53ef\u5728\u5de1\u903b\u4e0e\u8ddf\u968f\u4e4b\u95f4\u5207\u6362，\u517c\u987e\u79e9\u5e8f\u7ef4\u62a4\u548c\u670d\u52a1\u652f\u6301\u3002
\n\u6559\u5b66\u4e0e\u79d1\u7814\u9a8c\u8bc1：\u9002\u5408\u9a8c\u8bc1 SLAM\u3001\u8def\u5f84\u4f18\u5316\u3001DWA \u5c40\u90e8\u907f\u969c\u3001\u8ddf\u968f\u63a7\u5236\u3001BLDC \u5dee\u901f\u95ed\u73af\u548c\u591a\u4f20\u611f\u5668\u878d\u5408\u3002<\/p>\n

\u9700\u8981\u6ce8\u610f\u7684\u4e8b\u9879
\n\u4e0d\u8981\u628a\u5168\u90e8\u7b97\u6cd5\u538b\u5728 Arduino \u4e0a
\nSLAM\u30013D \u70b9\u4e91\u5904\u7406\u3001\u89c6\u89c9\u8bc6\u522b\u548c\u5b8c\u6574 DWA \u540c\u65f6\u8fd0\u884c\u65f6，AVR \u7cfb\u5217 MCU \u5f88\u96be\u7a33\u5b9a\u652f\u6491\u3002\u5efa\u8bae\u91c7\u7528\u4e0a\u4f4d\u673a\u89c4\u5212\u3001\u4e0b\u4f4d\u673a\u6267\u884c\u7684\u67b6\u6784，Arduino \u53ea\u8d1f\u8d23\u63a5\u6536\u901f\u5ea6\u6307\u4ee4\u5e76\u6267\u884c BLDC \u95ed\u73af\u3002
\n\u8ddf\u968f\u4e0e\u5de1\u903b\u5207\u6362\u5fc5\u987b\u5e73\u6ed1
\n\u4ece\u5de1\u903b\u5207\u5230\u8ddf\u968f\u3001\u4ece\u8ddf\u968f\u5207\u56de\u5de1\u903b\u65f6，\u4e0d\u80fd\u76f4\u63a5\u66ff\u6362\u76ee\u6807\u70b9\u6216\u901f\u5ea6\u6307\u4ee4，\u5426\u5219\u4f1a\u51fa\u73b0\u6025\u505c\u3001\u6025\u8f6c\u6216\u6765\u56de\u632f\u8361\u3002\u5207\u6362\u524d\u5e94\u8fdb\u884c\u822a\u5411\u5bf9\u9f50\u3001\u901f\u5ea6\u6e10\u53d8\u548c\u72b6\u6001\u786e\u8ba4，\u4f8b\u5982\u76ee\u6807\u7a33\u5b9a\u9501\u5b9a\u540e\u518d\u8fdb\u5165\u8ddf\u968f\u6a21\u5f0f\u3002
\n\u76ee\u6807\u4e22\u5931\u8981\u6709\u964d\u7ea7\u7b56\u7565
\n\u8ddf\u968f\u76ee\u6807\u53ef\u80fd\u56e0\u906e\u6321\u3001\u5f3a\u5149\u3001\u4eba\u6d41\u5e72\u6270\u6216\u4fe1\u53f7\u4e22\u5931\u800c\u6682\u65f6\u6d88\u5931\u3002\u7cfb\u7edf\u5e94\u8bbe\u7f6e\u8d85\u65f6\u673a\u5236：\u77ed\u671f\u4e22\u5931\u4fdd\u6301\u4f4e\u901f\u7b49\u5f85，\u957f\u671f\u4e22\u5931\u8fd4\u56de\u6700\u8fd1\u5de1\u903b\u70b9\u6216\u539f\u5730\u505c\u673a，\u800c\u4e0d\u662f\u76f2\u76ee\u8ffd\u641c\u3002
\n\u5c40\u90e8\u907f\u969c\u9700\u517c\u987e\u5b89\u5168\u4e0e\u6548\u7387
\nDWA \u7684\u969c\u788d\u7269\u8ddd\u79bb\u6743\u91cd\u8fc7\u4f4e\u4f1a\u8d34\u969c\u884c\u9a76，\u8fc7\u9ad8\u5219\u5bb9\u6613\u505c\u6ede\u4e0d\u524d\u3002\u56ed\u533a\u4eba\u6d41\u5bc6\u96c6\u65f6\u5e94\u63d0\u9ad8\u5b89\u5168\u6743\u91cd\u5e76\u7f29\u77ed\u9884\u6d4b\u7a97\u53e3；\u7a7a\u65f7\u8def\u6bb5\u53ef\u9002\u5f53\u63d0\u9ad8\u901f\u5ea6\u6743\u91cd\u3002 \u540c\u65f6\u5e94\u4fdd\u7559\u786c\u4ef6\u7ea7\u6025\u505c，\u9632\u6b62\u8f6f\u4ef6\u5f02\u5e38\u5bfc\u81f4\u5931\u63a7\u3002
\n\u6237\u5916\u73af\u5883\u8981\u91cd\u70b9\u8003\u8651\u5b9a\u4f4d\u4e0e\u9632\u62a4
\n\u56ed\u533a\u573a\u666f\u5b58\u5728 GPS \u906e\u6321\u3001\u73bb\u7483\u5e55\u5899\u53cd\u5c04\u3001\u591c\u95f4\u4f4e\u7167\u5ea6\u3001\u96e8\u96ea\u548c\u7535\u78c1\u5e72\u6270\u7b49\u95ee\u9898\u3002\u5b9a\u4f4d\u5b9c\u91c7\u7528\u8f6e\u5f0f\u91cc\u7a0b\u8ba1 + IMU + \u6fc0\u5149\u96f7\u8fbe\/\u89c6\u89c9\u878d\u5408；\u5916\u58f3\u3001\u63a5\u63d2\u4ef6\u548c\u7535\u6c60\u8231\u5e94\u6ee1\u8db3\u9632\u6c34\u9632\u5c18\u8981\u6c42；\u52a8\u529b\u7535\u6e90\u4e0e\u63a7\u5236\u7535\u6e90\u9700\u9694\u79bb，\u907f\u514d BLDC \u542f\u52a8\u7535\u6d41\u9020\u6210 MCU \u590d\u4f4d\u3002<\/p>\n

\"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"
\n1\u3001\u72b6\u6001\u673a\u9a71\u52a8\u7684\u5de1\u903b-\u8ddf\u968f\u53cc\u6a21\u5f0f\u5207\u6362（\u57fa\u7840\u8ddf\u968f\u903b\u8f91）
\n\u9002\u7528\u573a\u666f：\u56ed\u533a\u5b89\u9632\u5de1\u903b，\u673a\u5668\u4eba\u9ed8\u8ba4\u6cbf\u9884\u8bbe\u822a\u70b9\u5de1\u903b，\u68c0\u6d4b\u5230\u5de1\u903b\u4eba\u5458\u540e\u81ea\u52a8\u5207\u6362\u8ddf\u968f\u6a21\u5f0f，\u4fdd\u6301\u5b89\u5168\u8ddd\u79bb\u8ddf\u968f，\u76ee\u6807\u4e22\u5931\u540e\u6062\u590d\u5de1\u903b\u3002\u6838\u5fc3\u903b\u8f91\u53c2\u8003\u201c\u6709\u9650\u72b6\u6001\u673a\u7ba1\u7406\u53cc\u6a21\u5f0f\u5207\u6362\u201d\u3002<\/p>\n

\/**
\n * \u56ed\u533a\u5de1\u903b\u8ddf\u968f\u673a\u5668\u4eba – \u72b6\u6001\u673a\u9a71\u52a8\u53cc\u6a21\u5f0f\u5207\u6362
\n * \u786c\u4ef6\u5047\u8bbe：ESP32\/Arduino Due, 2\u00d7BLDC(SimpleFOC), UWB\/\u89c6\u89c9\u76ee\u6807\u68c0\u6d4b, \u8d85\u58f0\u6ce2
\n * \u6838\u5fc3：\u5de1\u903b\u6a21\u5f0f <-> \u8ddf\u968f\u6a21\u5f0f，\u5361\u5c14\u66fc\u6ee4\u6ce2\u5e73\u6ed1\u76ee\u6807\u4f4d\u7f6e
\n *\/<\/span><\/p>\n

#<\/span>include<\/span> <SimpleFOC.h><\/span><\/span><\/p>\n

BLDCMotor motorL<\/span>(<\/span>7<\/span>)<\/span>,<\/span> motorR<\/span>(<\/span>7<\/span>)<\/span>;<\/span>
\nEncoder encL<\/span>(<\/span>18<\/span>,<\/span> 19<\/span>,<\/span> 2048<\/span>)<\/span>,<\/span> encR<\/span>(<\/span>20<\/span>,<\/span> 21<\/span>,<\/span> 2048<\/span>)<\/span>;<\/span><\/p>\n

\/\/ ===== \u72b6\u6001\u5b9a\u4e49 =====<\/span>
\nenum<\/span> RobotState<\/span> {<\/span> STATE_PATROL,<\/span> STATE_FOLLOW,<\/span> STATE_GUARD }<\/span>;<\/span>
\nRobotState currentState =<\/span> STATE_PATROL;<\/span><\/p>\n

\/\/ ===== \u8ddf\u968f\u53c2\u6570 =====<\/span>
\nconst<\/span> float<\/span> FOLLOW_DIST =<\/span> 1.5<\/span>;<\/span> \/\/ \u76ee\u6807\u8ddf\u968f\u8ddd\u79bb (m)<\/span>
\nconst<\/span> float<\/span> MIN_SAFE_DIST =<\/span> 0.4<\/span>;<\/span> \/\/ \u6700\u5c0f\u5b89\u5168\u8ddd\u79bb (m)<\/span>
\nconst<\/span> float<\/span> TARGET_TIMEOUT =<\/span> 5000<\/span>;<\/span> \/\/ \u76ee\u6807\u4e22\u5931\u8d85\u65f6 (ms)<\/span><\/p>\n

\/\/ \u76ee\u6807\u4f4d\u7f6e（UWB\u6216\u89c6\u89c9\u8f93\u5165）<\/span>
\nfloat<\/span> targetX =<\/span> 0<\/span>,<\/span> targetY =<\/span> 0<\/span>;<\/span>
\nunsigned<\/span> long<\/span> lastTargetSeen =<\/span> 0<\/span>;<\/span><\/p>\n

\/\/ \u5de1\u903b\u822a\u70b9<\/span>
\nconst<\/span> int<\/span> WAYPOINT_COUNT =<\/span> 3<\/span>;<\/span>
\nfloat<\/span> waypoints[<\/span>WAYPOINT_COUNT]<\/span>[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>{<\/span>2.0<\/span>,<\/span> 0<\/span>}<\/span>,<\/span> {<\/span>2.0<\/span>,<\/span> 2.0<\/span>}<\/span>,<\/span> {<\/span>0<\/span>,<\/span> 2.0<\/span>}<\/span>}<\/span>;<\/span>
\nint<\/span> currentWaypoint =<\/span> 0<\/span>;<\/span><\/p>\n

\/\/ \u8d85\u58f0\u6ce2<\/span>
\nconst<\/span> int<\/span> SONAR_TRIG =<\/span> 22<\/span>,<\/span> SONAR_ECHO =<\/span> 23<\/span>;<\/span><\/p>\n

float<\/span> measureDistance<\/span>(<\/span>)<\/span> {<\/span>
\n digitalWrite<\/span>(<\/span>SONAR_TRIG,<\/span> LOW)<\/span>;<\/span> delayMicroseconds<\/span>(<\/span>2<\/span>)<\/span>;<\/span>
\n digitalWrite<\/span>(<\/span>SONAR_TRIG,<\/span> HIGH)<\/span>;<\/span> delayMicroseconds<\/span>(<\/span>10<\/span>)<\/span>;<\/span>
\n digitalWrite<\/span>(<\/span>SONAR_TRIG,<\/span> LOW)<\/span>;<\/span>
\n long<\/span> dur =<\/span> pulseIn<\/span>(<\/span>SONAR_ECHO,<\/span> HIGH,<\/span> 20000<\/span>)<\/span>;<\/span>
\n return<\/span> (<\/span>dur ==<\/span> 0<\/span>)<\/span> ?<\/span> 999.0<\/span> :<\/span> dur *<\/span> 0.0343<\/span> \/<\/span> 2.0<\/span>;<\/span>
\n}<\/span><\/p>\n

void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
\n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
\n pinMode<\/span>(<\/span>SONAR_TRIG,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>SONAR_ECHO,<\/span> INPUT)<\/span>;<\/span>
\n motorL.<\/span>linkSensor<\/span>(<\/span>&<\/span>encL)<\/span>;<\/span> motorR.<\/span>linkSensor<\/span>(<\/span>&<\/span>encR)<\/span>;<\/span>
\n motorL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorL.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
\n motorR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
\n motorL.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span>
\n motorR.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span>
\n}<\/span><\/p>\n

\/\/ \u8ddf\u968f\u63a7\u5236（\u8ddd\u79bb+\u89d2\u5ea6）<\/span>
\nvoid<\/span> executeFollow<\/span>(<\/span>float<\/span> frontDist)<\/span> {<\/span>
\n \/\/ 1. \u907f\u969c\u786c\u4f18\u5148\u7ea7<\/span>
\n if<\/span> (<\/span>frontDist <<\/span> MIN_SAFE_DIST &&<\/span> frontDist ><\/span> 0<\/span>)<\/span> {<\/span>
\n motorL.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span> motorR.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span>
\n Serial.<\/span>println<\/span>(<\/span>"\u8ddf\u968f\u907f\u969c\u89e6\u53d1"<\/span>)<\/span>;<\/span>
\n return<\/span>;<\/span>
\n }<\/span><\/p>\n

\/\/ 2. \u8ba1\u7b97\u76f8\u5bf9\u8ddd\u79bb\u548c\u89d2\u5ea6<\/span>
\n float<\/span> dist =<\/span> sqrt<\/span>(<\/span>targetX *<\/span> targetX +<\/span> targetY *<\/span> targetY)<\/span>;<\/span>
\n float<\/span> angle =<\/span> atan2<\/span>(<\/span>targetY,<\/span> targetX)<\/span> *<\/span> 180<\/span> \/<\/span> PI;<\/span><\/p>\n

\/\/ 3. \u8ddd\u79bb\u63a7\u5236（P\u63a7\u5236）<\/span>
\n float<\/span> distError =<\/span> dist –<\/span> FOLLOW_DIST;<\/span>
\n float<\/span> speed =<\/span> constrain<\/span>(<\/span>distError *<\/span> 0.5<\/span>,<\/span> –<\/span>0.3<\/span>,<\/span> 0.8<\/span>)<\/span>;<\/span><\/p>\n

\/\/ 4. \u89d2\u5ea6\u63a7\u5236（\u5e26\u6b7b\u533a\u9632\u6296\u52a8）<\/span>
\n if<\/span> (<\/span>abs<\/span>(<\/span>angle)<\/span> <<\/span> 3.0<\/span>)<\/span> angle =<\/span> 0<\/span>;<\/span>
\n float<\/span> turn =<\/span> constrain<\/span>(<\/span>angle *<\/span> 0.02<\/span>,<\/span> –<\/span>0.5<\/span>,<\/span> 0.5<\/span>)<\/span>;<\/span><\/p>\n

\/\/ 5. \u5dee\u901f\u9a71\u52a8<\/span>
\n motorL.<\/span>move<\/span>(<\/span>speed –<\/span> turn)<\/span>;<\/span>
\n motorR.<\/span>move<\/span>(<\/span>speed +<\/span> turn)<\/span>;<\/span><\/p>\n

Serial.<\/span>print<\/span>(<\/span>"Follow Dist:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>dist)<\/span>;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>" Angle:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>angle)<\/span>;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>" Speed:"<\/span>)<\/span>;<\/span> Serial.<\/span>println<\/span>(<\/span>speed)<\/span>;<\/span>
\n}<\/span><\/p>\n

\/\/ \u5de1\u903b\u63a7\u5236（\u6cbf\u822a\u70b9）<\/span>
\nvoid<\/span> executePatrol<\/span>(<\/span>)<\/span> {<\/span>
\n float<\/span> dx =<\/span> waypoints[<\/span>currentWaypoint]<\/span>[<\/span>0<\/span>]<\/span> –<\/span> 0<\/span>;<\/span> \/\/ \u7b80\u5316：\u5047\u8bbe\u5f53\u524d\u4f4d\u7f6e(0,0)<\/span>
\n float<\/span> dy =<\/span> waypoints[<\/span>currentWaypoint]<\/span>[<\/span>1<\/span>]<\/span> –<\/span> 0<\/span>;<\/span>
\n float<\/span> dist =<\/span> sqrt<\/span>(<\/span>dx*<\/span>dx +<\/span> dy*<\/span>dy)<\/span>;<\/span><\/p>\n

if<\/span> (<\/span>dist <<\/span> 0.3<\/span>)<\/span> {<\/span>
\n currentWaypoint =<\/span> (<\/span>currentWaypoint +<\/span> 1<\/span>)<\/span> %<\/span> WAYPOINT_COUNT;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>"\u5207\u6362\u5230\u822a\u70b9:"<\/span>)<\/span>;<\/span> Serial.<\/span>println<\/span>(<\/span>currentWaypoint)<\/span>;<\/span>
\n return<\/span>;<\/span>
\n }<\/span><\/p>\n

\/\/ \u671d\u5411\u822a\u70b9<\/span>
\n float<\/span> targetAngle =<\/span> atan2<\/span>(<\/span>dy,<\/span> dx)<\/span> *<\/span> 180<\/span> \/<\/span> PI;<\/span>
\n float<\/span> turn =<\/span> constrain<\/span>(<\/span>targetAngle *<\/span> 0.02<\/span>,<\/span> –<\/span>0.4<\/span>,<\/span> 0.4<\/span>)<\/span>;<\/span><\/p>\n

motorL.<\/span>move<\/span>(<\/span>0.4<\/span> –<\/span> turn)<\/span>;<\/span>
\n motorR.<\/span>move<\/span>(<\/span>0.4<\/span> +<\/span> turn)<\/span>;<\/span>
\n}<\/span><\/p>\n

void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
\n motorL.<\/span>loopFOC<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>loopFOC<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

float<\/span> frontDist =<\/span> measureDistance<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

\/\/ \u72b6\u6001\u673a<\/span>
\n switch<\/span> (<\/span>currentState)<\/span> {<\/span>
\n case<\/span> STATE_PATROL:<\/span>
\n executePatrol<\/span>(<\/span>)<\/span>;<\/span>
\n \/\/ \u68c0\u6d4b\u5230\u76ee\u6807（\u7b80\u5316：\u4e32\u53e3\u8f93\u5165）<\/span>
\n if<\/span> (<\/span>Serial.<\/span>available<\/span>(<\/span>)<\/span>)<\/span> {<\/span>
\n String cmd =<\/span> Serial.<\/span>readStringUntil<\/span>(<\/span>'\\\\n'<\/span>)<\/span>;<\/span>
\n if<\/span> (<\/span>cmd ==<\/span> "TARGET"<\/span>)<\/span> {<\/span>
\n currentState =<\/span> STATE_FOLLOW;<\/span>
\n lastTargetSeen =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
\n Serial.<\/span>println<\/span>(<\/span>"\u5207\u6362\u5230\u8ddf\u968f\u6a21\u5f0f"<\/span>)<\/span>;<\/span>
\n }<\/span>
\n }<\/span>
\n break<\/span>;<\/span><\/p>\n

case<\/span> STATE_FOLLOW:<\/span>
\n executeFollow<\/span>(<\/span>frontDist)<\/span>;<\/span>
\n \/\/ \u76ee\u6807\u4e22\u5931\u8d85\u65f6<\/span>
\n if<\/span> (<\/span>millis<\/span>(<\/span>)<\/span> –<\/span> lastTargetSeen ><\/span> TARGET_TIMEOUT)<\/span> {<\/span>
\n currentState =<\/span> STATE_PATROL;<\/span>
\n Serial.<\/span>println<\/span>(<\/span>"\u76ee\u6807\u4e22\u5931，\u6062\u590d\u5de1\u903b"<\/span>)<\/span>;<\/span>
\n }<\/span>
\n break<\/span>;<\/span><\/p>\n

case<\/span> STATE_GUARD:<\/span>
\n motorL.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span> motorR.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span>
\n break<\/span>;<\/span>
\n }<\/span><\/p>\n

delay<\/span>(<\/span>20<\/span>)<\/span>;<\/span>
\n}<\/span><\/p>\n

\u8981\u70b9：\u8be5\u4ee3\u7801\u5b9e\u73b0\u4e86\u5de1\u903b-\u8ddf\u968f\u53cc\u6a21\u5f0f\u72b6\u6001\u673a\u3002\u6838\u5fc3\u903b\u8f91\u53c2\u8003\u201c\u65e0\u76ee\u6807\u65f6\u81ea\u52a8\u5de1\u903b，\u68c0\u6d4b\u5230\u76ee\u6807\u65f6\u5207\u6362\u8ddf\u968f，\u5165\u4fb5\u68c0\u6d4b\u65f6\u8fdb\u5165\u5b88\u536b\u6a21\u5f0f\u201d\u3002\u8ddf\u968f\u63a7\u5236\u91c7\u7528\u201c\u8ddd\u79bbP\u63a7\u5236 + \u89d2\u5ea6\u6b7b\u533a\u201d\u7684\u7ec4\u5408，\u907f\u514d\u76ee\u6807\u5728\u6b63\u524d\u65b9\u65f6\u673a\u5668\u4eba\u6296\u52a8\u3002\u907f\u969c\u4f5c\u4e3a\u786c\u4f18\u5148\u7ea7，\u524d\u65b9\u8ddd\u79bb\u8fc7\u8fd1\u65f6\u65e0\u6761\u4ef6\u4e2d\u65ad\u8ddf\u968f\u3002<\/p>\n

2\u3001UWB+\u89c6\u89c9\u591a\u6a21\u6001\u6297\u906e\u6321\u8ddf\u968f（\u52a8\u6001\u73af\u5883）
\n\u9002\u7528\u573a\u666f：\u56ed\u533a\u73af\u5883\u4e2d\u5de1\u903b\u4eba\u5458\u53ef\u80fd\u88ab\u6811\u6728\u3001\u8f66\u8f86\u906e\u6321，\u5355\u4e00\u4f20\u611f\u5668（UWB\u6216\u89c6\u89c9）\u4f1a\u4e22\u5931\u76ee\u6807\u3002\u672c\u6848\u4f8b\u91c7\u7528\u591a\u6a21\u6001\u878d\u5408，\u5f53\u76ee\u6807\u88ab\u906e\u6321\u65f6\u5207\u6362\u81f3\u60ef\u6027\u822a\u4f4d\u63a8\u7b97\u7ef4\u6301\u8ddf\u968f，\u786e\u4fdd\u4e0d\u4e22\u5931\u3002<\/p>\n

\/**
\n * \u56ed\u533a\u5de1\u903b\u8ddf\u968f – UWB+\u89c6\u89c9\u591a\u6a21\u6001\u6297\u906e\u6321
\n * \u6838\u5fc3：UWB\u4e0e\u89c6\u89c9\u4f4d\u7f6e\u878d\u5408 -> \u906e\u6321\u68c0\u6d4b -> \u822a\u4f4d\u63a8\u7b97\u7ef4\u6301\u8ddf\u968f
\n *\/<\/span><\/p>\n

#<\/span>include<\/span> <SimpleFOC.h><\/span><\/span><\/p>\n

BLDCMotor motorL<\/span>(<\/span>7<\/span>)<\/span>,<\/span> motorR<\/span>(<\/span>7<\/span>)<\/span>;<\/span><\/p>\n

\/\/ ===== \u4f20\u611f\u5668\u6570\u636e =====<\/span>
\nfloat<\/span> uwbPos[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>0<\/span>,<\/span> 0<\/span>}<\/span>;<\/span> \/\/ UWB\u76ee\u6807\u4f4d\u7f6e<\/span>
\nfloat<\/span> visionPos[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>0<\/span>,<\/span> 0<\/span>}<\/span>;<\/span> \/\/ \u89c6\u89c9\u76ee\u6807\u4f4d\u7f6e<\/span>
\nfloat<\/span> fusedPos[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>0<\/span>,<\/span> 0<\/span>}<\/span>;<\/span> \/\/ \u878d\u5408\u4f4d\u7f6e<\/span>
\nfloat<\/span> odoX =<\/span> 0<\/span>,<\/span> odoY =<\/span> 0<\/span>,<\/span> odoYaw =<\/span> 0<\/span>;<\/span> \/\/ \u91cc\u7a0b\u8ba1<\/span><\/p>\n

const<\/span> float<\/span> OCCLUSION_THRESH =<\/span> 1.0<\/span>;<\/span> \/\/ \u906e\u6321\u5224\u5b9a\u9608\u503c (m)<\/span>
\nbool<\/span> isOccluded =<\/span> false<\/span>;<\/span><\/p>\n

\/\/ \u76ee\u6807\u4e22\u5931\u5904\u7406<\/span>
\nunsigned<\/span> long<\/span> lastValidTarget =<\/span> 0<\/span>;<\/span>
\nconst<\/span> float<\/span> TARGET_TIMEOUT =<\/span> 3000<\/span>;<\/span><\/p>\n

\/\/ \u8ddf\u968f\u53c2\u6570<\/span>
\nconst<\/span> float<\/span> FOLLOW_DIST =<\/span> 1.5<\/span>;<\/span><\/p>\n

void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
\n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
\n motorL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
\n motorL.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
\n motorL.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span>
\n motorR.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span>
\n}<\/span><\/p>\n

\/\/ \u591a\u6a21\u6001\u6297\u906e\u6321\u878d\u5408<\/span>
\nvoid<\/span> fuseTargetPosition<\/span>(<\/span>)<\/span> {<\/span>
\n \/\/ 1. \u8ba1\u7b97UWB\u4e0e\u89c6\u89c9\u7684\u504f\u5dee<\/span>
\n float<\/span> distDiff =<\/span> sqrt<\/span>(<\/span>pow<\/span>(<\/span>uwbPos[<\/span>0<\/span>]<\/span>–<\/span>visionPos[<\/span>0<\/span>]<\/span>,<\/span> 2<\/span>)<\/span> +<\/span> pow<\/span>(<\/span>uwbPos[<\/span>1<\/span>]<\/span>–<\/span>visionPos[<\/span>1<\/span>]<\/span>,<\/span> 2<\/span>)<\/span>)<\/span>;<\/span><\/p>\n

\/\/ 2. \u906e\u6321\u5224\u5b9a<\/span>
\n isOccluded =<\/span> (<\/span>distDiff ><\/span> OCCLUSION_THRESH)<\/span>;<\/span><\/p>\n

if<\/span> (<\/span>!<\/span>isOccluded)<\/span> {<\/span>
\n \/\/ \u6b63\u5e38\u878d\u5408：\u52a0\u6743\u5e73\u5747<\/span>
\n fusedPos[<\/span>0<\/span>]<\/span> =<\/span> 0.6<\/span> *<\/span> uwbPos[<\/span>0<\/span>]<\/span> +<\/span> 0.4<\/span> *<\/span> visionPos[<\/span>0<\/span>]<\/span>;<\/span>
\n fusedPos[<\/span>1<\/span>]<\/span> =<\/span> 0.6<\/span> *<\/span> uwbPos[<\/span>1<\/span>]<\/span> +<\/span> 0.4<\/span> *<\/span> visionPos[<\/span>1<\/span>]<\/span>;<\/span>
\n lastValidTarget =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
\n }<\/span> else<\/span> {<\/span>
\n \/\/ \u906e\u6321：\u5207\u6362\u81f3\u822a\u4f4d\u63a8\u7b97（\u57fa\u4e8e\u4e0a\u6b21\u6709\u6548\u4f4d\u7f6e+\u822a\u5411）<\/span>
\n \/\/ \u7b80\u5316：\u6cbf\u673a\u5668\u4eba\u5f53\u524d\u822a\u5411\u4fdd\u6301\u8ddd\u79bb<\/span>
\n fusedPos[<\/span>0<\/span>]<\/span> =<\/span> odoX +<\/span> cos<\/span>(<\/span>odoYaw)<\/span> *<\/span> FOLLOW_DIST;<\/span>
\n fusedPos[<\/span>1<\/span>]<\/span> =<\/span> odoY +<\/span> sin<\/span>(<\/span>odoYaw)<\/span> *<\/span> FOLLOW_DIST;<\/span>
\n Serial.<\/span>println<\/span>(<\/span>"\u906e\u6321：\u822a\u4f4d\u63a8\u7b97\u7ef4\u6301"<\/span>)<\/span>;<\/span>
\n }<\/span>
\n}<\/span><\/p>\n

void<\/span> executeFollow<\/span>(<\/span>)<\/span> {<\/span>
\n float<\/span> dx =<\/span> fusedPos[<\/span>0<\/span>]<\/span> –<\/span> odoX;<\/span>
\n float<\/span> dy =<\/span> fusedPos[<\/span>1<\/span>]<\/span> –<\/span> odoY;<\/span>
\n float<\/span> dist =<\/span> sqrt<\/span>(<\/span>dx*<\/span>dx +<\/span> dy*<\/span>dy)<\/span>;<\/span>
\n float<\/span> angle =<\/span> atan2<\/span>(<\/span>dy,<\/span> dx)<\/span> *<\/span> 180<\/span> \/<\/span> PI;<\/span><\/p>\n

float<\/span> speed =<\/span> constrain<\/span>(<\/span>(<\/span>dist –<\/span> FOLLOW_DIST)<\/span> *<\/span> 0.5<\/span>,<\/span> –<\/span>0.2<\/span>,<\/span> 0.8<\/span>)<\/span>;<\/span>
\n if<\/span> (<\/span>abs<\/span>(<\/span>angle)<\/span> <<\/span> 3.0<\/span>)<\/span> angle =<\/span> 0<\/span>;<\/span>
\n float<\/span> turn =<\/span> constrain<\/span>(<\/span>angle *<\/span> 0.02<\/span>,<\/span> –<\/span>0.5<\/span>,<\/span> 0.5<\/span>)<\/span>;<\/span><\/p>\n

motorL.<\/span>move<\/span>(<\/span>speed –<\/span> turn)<\/span>;<\/span>
\n motorR.<\/span>move<\/span>(<\/span>speed +<\/span> turn)<\/span>;<\/span>
\n}<\/span><\/p>\n

void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
\n motorL.<\/span>loopFOC<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>loopFOC<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

\/\/ \u6a21\u62df\u4f20\u611f\u5668\u66f4\u65b0（\u5b9e\u9645\u4ece\u4e32\u53e3\/\u4f20\u611f\u5668\u8bfb\u53d6）<\/span>
\n \/\/ uwbPos, visionPos \u5e94\u5728\u6b64\u5904\u66f4\u65b0<\/span><\/p>\n

fuseTargetPosition<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

\/\/ \u76ee\u6807\u8d85\u65f6\u68c0\u67e5<\/span>
\n if<\/span> (<\/span>millis<\/span>(<\/span>)<\/span> –<\/span> lastValidTarget ><\/span> TARGET_TIMEOUT &&<\/span> !<\/span>isOccluded)<\/span> {<\/span>
\n motorL.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span> motorR.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span>
\n Serial.<\/span>println<\/span>(<\/span>"\u76ee\u6807\u4e22\u5931，\u505c\u6b62"<\/span>)<\/span>;<\/span>
\n return<\/span>;<\/span>
\n }<\/span><\/p>\n

executeFollow<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

\/\/ \u66f4\u65b0\u91cc\u7a0b\u8ba1（\u7b80\u5316）<\/span>
\n odoX +=<\/span> 0.01<\/span>;<\/span>
\n odoY +=<\/span> 0.005<\/span>;<\/span><\/p>\n

Serial.<\/span>print<\/span>(<\/span>"Fused:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>fusedPos[<\/span>0<\/span>]<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>","<\/span>)<\/span>;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>fusedPos[<\/span>1<\/span>]<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>" Occluded:"<\/span>)<\/span>;<\/span> Serial.<\/span>println<\/span>(<\/span>isOccluded)<\/span>;<\/span><\/p>\n

delay<\/span>(<\/span>20<\/span>)<\/span>;<\/span>
\n}<\/span><\/p>\n

\u8981\u70b9：\u8be5\u6848\u4f8b\u5b9e\u73b0\u4e86\u591a\u6a21\u6001\u6297\u906e\u6321\u8ddf\u968f\u3002\u6838\u5fc3\u903b\u8f91\u53c2\u8003\u201cUWB+\u89c6\u89c9\u591a\u6a21\u6001\u6297\u906e\u6321\u8ddf\u968f\u7b56\u7565，\u5f53\u76ee\u6807\u88ab\u906e\u6321\u65f6\u5207\u6362\u81f3\u60ef\u6027\u822a\u4f4d\u63a8\u7b97\u7ef4\u6301\u8ddf\u968f\u201d\u3002\u906e\u6321\u5224\u5b9a\u57fa\u4e8eUWB\u4e0e\u89c6\u89c9\u4f4d\u7f6e\u7684\u504f\u5dee\u2014\u2014\u5f53\u4e24\u8005\u5dee\u5f02\u8d85\u8fc7\u9608\u503c\u65f6，\u5224\u5b9a\u4e3a\u89c6\u89c9\u88ab\u906e\u6321\u3002\u822a\u4f4d\u63a8\u7b97\u5229\u7528\u673a\u5668\u4eba\u81ea\u8eab\u91cc\u7a0b\u8ba1\u548c\u822a\u5411，\u5728\u76ee\u6807\u6682\u65f6\u4e0d\u53ef\u89c1\u65f6\u7ef4\u6301\u8ddf\u968f\u65b9\u5411，\u907f\u514d\u56e0\u5355\u5e27\u4e22\u5931\u800c\u201c\u6025\u5239\u8f66\u201d\u3002<\/p>\n

3\u3001A\u5168\u5c40\u8def\u5f84\u89c4\u5212 + DWA\u5c40\u90e8\u907f\u969c\u8ddf\u968f（\u8def\u5f84\u4f18\u5316）
\n\u9002\u7528\u573a\u666f：\u56ed\u533a\u590d\u6742\u73af\u5883\u4e2d，\u5de1\u903b\u8ddf\u968f\u9700\u8981\u5168\u5c40\u6700\u4f18\u8def\u5f84\u4e0e\u5c40\u90e8\u52a8\u6001\u907f\u969c\u76f8\u7ed3\u5408\u3002A\u63d0\u4f9b\u4ece\u8d77\u70b9\u5230\u5404\u5de1\u903b\u70b9\u7684\u5168\u5c40\u8def\u5f84，DWA\u5728\u8ddf\u968f\u8fc7\u7a0b\u4e2d\u5b9e\u65f6\u907f\u5f00\u884c\u4eba\u3001\u8f66\u8f86\u7b49\u52a8\u6001\u969c\u788d\u7269\u3002<\/p>\n

\/**
\n * \u56ed\u533a\u5de1\u903b\u8ddf\u968f – A*\u5168\u5c40\u89c4\u5212 + DWA\u5c40\u90e8\u907f\u969c
\n * \u53c2\u8003：A*\u751f\u6210\u5168\u5c40\u8def\u5f84\u8282\u70b9 -> DWA\u4ee5\u8def\u5f84\u8282\u70b9\u4e3a\u4e34\u65f6\u76ee\u6807
\n *\/<\/span><\/p>\n

#<\/span>include<\/span> <SimpleFOC.h><\/span><\/span><\/p>\n

BLDCMotor motorL<\/span>(<\/span>7<\/span>)<\/span>,<\/span> motorR<\/span>(<\/span>7<\/span>)<\/span>;<\/span><\/p>\n

\/\/ ===== \u5168\u5c40\u8def\u5f84（\u7b80\u5316：\u9884\u5b9a\u4e49\u8282\u70b9） =====<\/span>
\n#<\/span>define<\/span> PATH_LEN<\/span> 5<\/span><\/span><\/span>
\nfloat<\/span> globalPath[<\/span>PATH_LEN]<\/span>[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>
\n {<\/span>0<\/span>,<\/span> 0<\/span>}<\/span>,<\/span> {<\/span>2<\/span>,<\/span> 0<\/span>}<\/span>,<\/span> {<\/span>2<\/span>,<\/span> 2<\/span>}<\/span>,<\/span> {<\/span>0<\/span>,<\/span> 2<\/span>}<\/span>,<\/span> {<\/span>0<\/span>,<\/span> 0<\/span>}<\/span> \/\/ \u77e9\u5f62\u5de1\u903b\u8def\u5f84<\/span>
\n}<\/span>;<\/span>
\nint<\/span> pathIdx =<\/span> 0<\/span>;<\/span><\/p>\n

\/\/ ===== DWA\u53c2\u6570 =====<\/span>
\nconst<\/span> float<\/span> MAX_VEL =<\/span> 0.5<\/span>;<\/span>
\nconst<\/span> float<\/span> VEL_STEP =<\/span> 0.1<\/span>;<\/span>
\nconst<\/span> float<\/span> W_STEP =<\/span> 0.2<\/span>;<\/span>
\nconst<\/span> float<\/span> SIM_TIME =<\/span> 1.5<\/span>;<\/span><\/p>\n

\/\/ \u76ee\u6807\u4f4d\u7f6e（UWB\/\u89c6\u89c9）<\/span>
\nfloat<\/span> targetX =<\/span> 0<\/span>,<\/span> targetY =<\/span> 0<\/span>;<\/span><\/p>\n

\/\/ \u673a\u5668\u4eba\u72b6\u6001<\/span>
\nfloat<\/span> robotX =<\/span> 0<\/span>,<\/span> robotY =<\/span> 0<\/span>,<\/span> robotTheta =<\/span> 0<\/span>;<\/span><\/p>\n

\/\/ \u8d85\u58f0\u6ce2<\/span>
\nconst<\/span> int<\/span> SONAR_TRIG =<\/span> 22<\/span>,<\/span> SONAR_ECHO =<\/span> 23<\/span>;<\/span>
\nfloat<\/span> frontDist =<\/span> 999<\/span>;<\/span><\/p>\n

float<\/span> measureDistance<\/span>(<\/span>)<\/span> {<\/span>
\n digitalWrite<\/span>(<\/span>SONAR_TRIG,<\/span> LOW)<\/span>;<\/span> delayMicroseconds<\/span>(<\/span>2<\/span>)<\/span>;<\/span>
\n digitalWrite<\/span>(<\/span>SONAR_TRIG,<\/span> HIGH)<\/span>;<\/span> delayMicroseconds<\/span>(<\/span>10<\/span>)<\/span>;<\/span>
\n digitalWrite<\/span>(<\/span>SONAR_TRIG,<\/span> LOW)<\/span>;<\/span>
\n long<\/span> dur =<\/span> pulseIn<\/span>(<\/span>SONAR_ECHO,<\/span> HIGH,<\/span> 20000<\/span>)<\/span>;<\/span>
\n return<\/span> (<\/span>dur ==<\/span> 0<\/span>)<\/span> ?<\/span> 999.0<\/span> :<\/span> dur *<\/span> 0.0343<\/span> \/<\/span> 2.0<\/span>;<\/span>
\n}<\/span><\/p>\n

\/\/ DWA\u8bc4\u4ef7<\/span>
\nfloat<\/span> evaluateTrajectory<\/span>(<\/span>float<\/span> v,<\/span> float<\/span> w,<\/span> float<\/span> goalX,<\/span> float<\/span> goalY)<\/span> {<\/span>
\n float<\/span> simX =<\/span> robotX,<\/span> simY =<\/span> robotY,<\/span> simTheta =<\/span> robotTheta;<\/span><\/p>\n

for<\/span> (<\/span>float<\/span> t =<\/span> 0<\/span>;<\/span> t <<\/span> SIM_TIME;<\/span> t +=<\/span> 0.1<\/span>)<\/span> {<\/span>
\n simTheta +=<\/span> w *<\/span> 0.1<\/span>;<\/span>
\n simX +=<\/span> v *<\/span> cos<\/span>(<\/span>simTheta)<\/span> *<\/span> 0.1<\/span>;<\/span>
\n simY +=<\/span> v *<\/span> sin<\/span>(<\/span>simTheta)<\/span> *<\/span> 0.1<\/span>;<\/span>
\n }<\/span><\/p>\n

\/\/ \u78b0\u649e\u68c0\u67e5<\/span>
\n if<\/span> (<\/span>frontDist <<\/span> 0.2<\/span> &&<\/span> v ><\/span> 0.1<\/span>)<\/span> return<\/span> –<\/span>9999<\/span>;<\/span><\/p>\n

\/\/ \u671d\u5411\u76ee\u6807<\/span>
\n float<\/span> dx =<\/span> goalX –<\/span> simX;<\/span>
\n float<\/span> dy =<\/span> goalY –<\/span> simY;<\/span>
\n float<\/span> heading =<\/span> atan2<\/span>(<\/span>dy,<\/span> dx)<\/span>;<\/span>
\n float<\/span> headingDiff =<\/span> fabs<\/span>(<\/span>heading –<\/span> simTheta)<\/span>;<\/span>
\n if<\/span> (<\/span>headingDiff ><\/span> PI)<\/span> headingDiff =<\/span> 2<\/span>*<\/span>PI –<\/span> headingDiff;<\/span>
\n float<\/span> headingScore =<\/span> 1.0<\/span> –<\/span> headingDiff \/<\/span> PI;<\/span><\/p>\n

\/\/ \u969c\u788d\u7269\u8ddd\u79bb<\/span>
\n float<\/span> distScore =<\/span> min<\/span>(<\/span>frontDist \/<\/span> 1.0<\/span>,<\/span> 1.0<\/span>)<\/span>;<\/span><\/p>\n

\/\/ \u901f\u5ea6<\/span>
\n float<\/span> velScore =<\/span> v \/<\/span> MAX_VEL;<\/span><\/p>\n

return<\/span> 0.4<\/span> *<\/span> headingScore +<\/span> 0.4<\/span> *<\/span> distScore +<\/span> 0.2<\/span> *<\/span> velScore;<\/span>
\n}<\/span><\/p>\n

\/\/ DWA\u89c4\u5212<\/span>
\nvoid<\/span> dwaPlan<\/span>(<\/span>float<\/span> goalX,<\/span> float<\/span> goalY,<\/span> float<\/span> &<\/span>bestV,<\/span> float<\/span> &<\/span>bestW)<\/span> {<\/span>
\n bestV =<\/span> 0<\/span>;<\/span> bestW =<\/span> 0<\/span>;<\/span>
\n float<\/span> bestScore =<\/span> –<\/span>9999<\/span>;<\/span><\/p>\n

for<\/span> (<\/span>float<\/span> v =<\/span> 0.1<\/span>;<\/span> v <=<\/span> MAX_VEL;<\/span> v +=<\/span> VEL_STEP)<\/span> {<\/span>
\n for<\/span> (<\/span>float<\/span> w =<\/span> –<\/span>1.0<\/span>;<\/span> w <=<\/span> 1.0<\/span>;<\/span> w +=<\/span> W_STEP)<\/span> {<\/span>
\n float<\/span> score =<\/span> evaluateTrajectory<\/span>(<\/span>v,<\/span> w,<\/span> goalX,<\/span> goalY)<\/span>;<\/span>
\n if<\/span> (<\/span>score ><\/span> bestScore)<\/span> {<\/span>
\n bestScore =<\/span> score;<\/span>
\n bestV =<\/span> v;<\/span>
\n bestW =<\/span> w;<\/span>
\n }<\/span>
\n }<\/span>
\n }<\/span>
\n}<\/span><\/p>\n

void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
\n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
\n pinMode<\/span>(<\/span>SONAR_TRIG,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>SONAR_ECHO,<\/span> INPUT)<\/span>;<\/span>
\n motorL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
\n motorL.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
\n motorL.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span>
\n motorR.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span>
\n}<\/span><\/p>\n

void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
\n motorL.<\/span>loopFOC<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>loopFOC<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

\/\/ 1. \u611f\u77e5<\/span>
\n frontDist =<\/span> measureDistance<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

\/\/ 2. \u83b7\u53d6\u5f53\u524d\u5168\u5c40\u8def\u5f84\u76ee\u6807\u70b9<\/span>
\n float<\/span> goalX =<\/span> globalPath[<\/span>pathIdx]<\/span>[<\/span>0<\/span>]<\/span>;<\/span>
\n float<\/span> goalY =<\/span> globalPath[<\/span>pathIdx]<\/span>[<\/span>1<\/span>]<\/span>;<\/span><\/p>\n

\/\/ 3. \u5230\u8fbe\u5224\u65ad<\/span>
\n float<\/span> distToGoal =<\/span> sqrt<\/span>(<\/span>pow<\/span>(<\/span>goalX –<\/span> robotX,<\/span> 2<\/span>)<\/span> +<\/span> pow<\/span>(<\/span>goalY –<\/span> robotY,<\/span> 2<\/span>)<\/span>)<\/span>;<\/span>
\n if<\/span> (<\/span>distToGoal <<\/span> 0.3<\/span>)<\/span> {<\/span>
\n pathIdx =<\/span> (<\/span>pathIdx +<\/span> 1<\/span>)<\/span> %<\/span> PATH_LEN;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>"\u5207\u6362\u5230\u8def\u5f84\u70b9:"<\/span>)<\/span>;<\/span> Serial.<\/span>println<\/span>(<\/span>pathIdx)<\/span>;<\/span>
\n }<\/span><\/p>\n

\/\/ 4. DWA\u5c40\u90e8\u89c4\u5212（\u671d\u5411\u8def\u5f84\u70b9，\u907f\u969c）<\/span>
\n float<\/span> bestV,<\/span> bestW;<\/span>
\n dwaPlan<\/span>(<\/span>goalX,<\/span> goalY,<\/span> bestV,<\/span> bestW)<\/span>;<\/span><\/p>\n

\/\/ 5. \u5dee\u901f\u6267\u884c<\/span>
\n float<\/span> vL =<\/span> bestV –<\/span> bestW *<\/span> 0.15<\/span>;<\/span>
\n float<\/span> vR =<\/span> bestV +<\/span> bestW *<\/span> 0.15<\/span>;<\/span>
\n motorL.<\/span>move<\/span>(<\/span>vL \/<\/span> 0.05<\/span>)<\/span>;<\/span>
\n motorR.<\/span>move<\/span>(<\/span>vR \/<\/span> 0.05<\/span>)<\/span>;<\/span><\/p>\n

\/\/ 6. \u66f4\u65b0\u4f4d\u59ff<\/span>
\n robotTheta +=<\/span> bestW *<\/span> 0.05<\/span>;<\/span>
\n robotX +=<\/span> bestV *<\/span> cos<\/span>(<\/span>robotTheta)<\/span> *<\/span> 0.05<\/span>;<\/span>
\n robotY +=<\/span> bestV *<\/span> sin<\/span>(<\/span>robotTheta)<\/span> *<\/span> 0.05<\/span>;<\/span><\/p>\n

Serial.<\/span>print<\/span>(<\/span>"V:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>bestV)<\/span>;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>" W:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>bestW)<\/span>;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>" Goal:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>pathIdx)<\/span>;<\/span>
\n Serial.<\/span>print<\/span>(<\/span>" Dist:"<\/span>)<\/span>;<\/span> Serial.<\/span>println<\/span>(<\/span>frontDist)<\/span>;<\/span><\/p>\n

delay<\/span>(<\/span>50<\/span>)<\/span>;<\/span>
\n}<\/span><\/p>\n

\u8981\u70b9：\u8be5\u6848\u4f8b\u5b9e\u73b0\u4e86\u5168\u5c40A* + \u5c40\u90e8DWA\u7684\u5206\u5c42\u5bfc\u822a\u67b6\u6784\u3002\u53c2\u8003\u201cA*\u751f\u6210\u5168\u5c40\u8def\u5f84\u8282\u70b9，DWA\u4ee5\u8def\u5f84\u8282\u70b9\u4e3a\u4e34\u65f6\u76ee\u6807，\u52a8\u6001\u91cd\u89c4\u5212\u201d\u3002\u5168\u5c40\u8def\u5f84\u63d0\u4f9b\u5b8f\u89c2\u5f15\u5bfc，DWA\u7684heading\u8bc4\u5206\u4e0d\u6307\u5411\u6700\u7ec8\u76ee\u6807\u800c\u662f\u6307\u5411\u8def\u5f84\u4e0a\u7684\u4e0b\u4e00\u4e2a\u8282\u70b9\u3002\u5f53DWA\u68c0\u6d4b\u5230\u65b0\u969c\u788d\u7269\u65f6，\u901a\u8fc7\u8bc4\u4ef7\u51fd\u6570\u7684\u78b0\u649e\u68c0\u67e5\u9879\u81ea\u52a8\u9009\u62e9\u7ed5\u884c\u8f68\u8ff9，\u5b9e\u73b0\u201c\u5168\u5c40\u7c97\u89c4\u5212 + \u5c40\u90e8\u7cbe\u907f\u969c\u201d\u3002<\/p>\n

\u8981\u70b9\u89e3\u8bfb<\/p>\n

  • \n

    \u5de1\u903b\u8ddf\u968f\u7684\u6838\u5fc3\u662f\u201c\u72b6\u6001\u673a\u7ba1\u7406\u201d，\u800c\u975e\u7b80\u5355\u7684\u201c\u68c0\u6d4b\u5230\u5c31\u8ddf\u968f\u201d\u3002 \u56ed\u533a\u5b89\u9632\u573a\u666f\u9700\u8981\u4e09\u79cd\u72b6\u6001\u534f\u540c：\u5de1\u903b\u6a21\u5f0f（\u65e0\u76ee\u6807\u65f6\u6cbf\u822a\u70b9\u5de1\u822a）\u3001\u8ddf\u968f\u6a21\u5f0f（\u68c0\u6d4b\u5230\u5de1\u903b\u4eba\u5458\u540e\u4fdd\u6301\u8ddd\u79bb）\u3001\u5b88\u536b\u6a21\u5f0f（\u68c0\u6d4b\u5230\u5165\u4fb5\u65f6\u505c\u6b62\u5e76\u62a5\u8b66）\u3002\u72b6\u6001\u5207\u6362\u5fc5\u987b\u6709\u660e\u786e\u7684\u89e6\u53d1\u6761\u4ef6\u548c\u8d85\u65f6\u673a\u5236（\u5982\u76ee\u6807\u4e22\u59315\u79d2\u540e\u6062\u590d\u5de1\u903b），\u5426\u5219\u673a\u5668\u4eba\u4f1a\u5728\u201c\u8ddf\u968f-\u5de1\u903b\u201d\u4e4b\u95f4\u9891\u7e41\u6296\u52a8\u3002<\/p>\n<\/li>\n

  • \n

    \u591a\u6a21\u6001\u6297\u906e\u6321\u662f\u56ed\u533a\u73af\u5883\u7684\u5fc5\u8981\u8bbe\u8ba1，\u5355\u4e00\u4f20\u611f\u5668\u4e0d\u53ef\u9760\u3002 \u56ed\u533a\u4e2d\u6811\u6728\u3001\u8f66\u8f86\u3001\u5efa\u7b51\u906e\u6321\u9891\u7e41，\u7eaf\u89c6\u89c9\u65b9\u6848\u5728\u906e\u6321\u65f6\u4e22\u5931\u76ee\u6807，\u7eafUWB\u65b9\u6848\u5728\u975e\u89c6\u8ddd\u73af\u5883\u4e0b\u7cbe\u5ea6\u4e0b\u964d\u3002UWB+\u89c6\u89c9\u878d\u5408 + \u822a\u4f4d\u63a8\u7b97\u515c\u5e95\u7684\u4e09\u5c42\u67b6\u6784\u662f\u52a1\u5b9e\u9009\u62e9：\u6b63\u5e38\u65f6\u878d\u5408\u4e24\u8005，\u906e\u6321\u65f6\u5207\u6362\u822a\u4f4d\u63a8\u7b97，\u5b8c\u5168\u4e22\u5931\u65f6\u505c\u6b62\u7b49\u5f85\u3002\u906e\u6321\u5224\u5b9a\u53ef\u57fa\u4e8e\u4e24\u79cd\u4f20\u611f\u5668\u4f4d\u7f6e\u7684\u504f\u5dee\u7a0b\u5ea6\u3002<\/p>\n<\/li>\n

  • \n

    \u5168\u5c40\u8def\u5f84\u89c4\u5212\u4e0e\u5c40\u90e8\u907f\u969c\u5fc5\u987b\u5206\u5c42，\u4e0d\u80fd\u6df7\u4e3a\u4e00\u8c08\u3002 A*\u8d1f\u8d23\u201c\u4ece\u8d77\u70b9\u5230\u5de1\u903b\u70b9\u7684\u6700\u4f18\u8def\u7ebf\u201d，DWA\u8d1f\u8d23\u201c\u6cbf\u8def\u7ebf\u884c\u9a76\u65f6\u907f\u5f00\u52a8\u6001\u969c\u788d\u201d\u3002\u5982\u679c\u53ea\u7528DWA\u671d\u5411\u6700\u7ec8\u76ee\u6807，\u5728\u590d\u6742\u56ed\u533a\u4e2d\u4f1a\u9677\u5165\u5c40\u90e8\u6700\u4f18（\u5982\u88ab\u5efa\u7b51\u6321\u4f4f\u53bb\u8def，DWA\u53ea\u4f1a\u53cd\u590d\u5c1d\u8bd5\u7ed5\u884c\u800c\u65e0\u6cd5\u627e\u5230\u6b63\u786e\u65b9\u5411）\u3002\u5206\u5c42\u67b6\u6784\u8ba9DWA\u671d\u5411\u201c\u8def\u5f84\u4e0a\u7684\u4e0b\u4e00\u4e2a\u8282\u70b9\u201d\u800c\u975e\u6700\u7ec8\u76ee\u6807，\u907f\u514d\u4e86\u5c40\u90e8\u6700\u4f18\u95ee\u9898\u3002<\/p>\n<\/li>\n

  • \n

    \u907f\u969c\u662f\u786c\u4f18\u5148\u7ea7，\u5fc5\u987b\u5728\u8ddf\u968f\u63a7\u5236\u4e4b\u524d\u68c0\u67e5\u3002 \u8ddf\u968f\u903b\u8f91\u65e0\u8bba\u591a\u4e48\u7cbe\u5bc6，\u524d\u65b9\u6709\u969c\u788d\u7269\u65f6\u5fc5\u987b\u8ba9\u4f4d\u4e8e\u5b89\u5168\u3002\u4ee3\u7801\u4e2d\u7684 if (frontDist < MIN_SAFE_DIST) { stop; return; } \u4f53\u73b0\u4e86\u8fd9\u4e00\u539f\u5219\u3002\u5173\u952e\u7ec6\u8282：\u907f\u969c\u89e6\u53d1\u540e\u4e0d\u80fd\u7acb\u5373\u6062\u590d\u8ddf\u968f，\u5e94\u7b49\u5f85\u969c\u788d\u7269\u79bb\u5f00\u5b89\u5168\u8ddd\u79bb\u5e76\u4fdd\u6301\u77ed\u6682\u5ef6\u65f6（\u5982500ms），\u9632\u6b62\u673a\u5668\u4eba\u5728\u969c\u788d\u7269\u8fb9\u7f18\u53cd\u590d\u542f\u505c\u3002<\/p>\n<\/li>\n

  • \n

    \u56ed\u533a\u5de1\u903b\u7684\u8def\u5f84\u4f18\u5316\u5e94\u5173\u6ce8\u201c\u8986\u76d6\u6548\u7387\u201d\u800c\u975e\u201c\u6700\u77ed\u8def\u5f84\u201d\u3002 \u5b89\u9632\u5de1\u903b\u7684\u76ee\u6807\u662f\u6700\u5927\u5316\u533a\u57df\u8986\u76d6，\u800c\u975e\u6700\u5c0f\u5316\u884c\u9a76\u8ddd\u79bb\u3002\u7b80\u5355\u7684A*\u6700\u77ed\u8def\u5f84\u53ef\u80fd\u5bfc\u81f4\u67d0\u4e9b\u533a\u57df\u88ab\u53cd\u590d\u7ecf\u8fc7\u800c\u53e6\u4e00\u4e9b\u533a\u57df\u88ab\u5ffd\u7565\u3002\u5b9e\u7528\u7684\u4f18\u5316\u7b56\u7565\u5305\u62ec：\u4f7f\u7528\u201c\u5de1\u903b\u70b9\u8f6e\u6362\u201d\u673a\u5236（\u6bcf\u6b21\u5de1\u903b\u987a\u5e8f\u4e0d\u540c）\u3001\u5f15\u5165\u201c\u8986\u76d6\u5ea6\u8bc4\u5206\u201d（\u4f18\u5148\u524d\u5f80\u8fd1\u671f\u672a\u5de1\u903b\u7684\u533a\u57df）\u3001\u591a\u673a\u5668\u4eba\u534f\u540c\u65f6\u7684\u8d1f\u8f7d\u5747\u8861\u3002<\/p>\n<\/li>\n

    \"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"
    \n4\u3001\u56ed\u533a\u57fa\u7840\u5de1\u903b\u8ddf\u968f+\u7b80\u5355\u8def\u5f84\u4f18\u5316\u7a0b\u5e8f（\u6838\u5fc3：\u4eba\u5458\u8ddf\u968f+\u57fa\u7840\u5de1\u903b）
    \n\u9002\u7528\u573a\u666f：\u56ed\u533a\u65e5\u5e38\u5de1\u903b，\u9700\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u6216\u9884\u8bbe\u8def\u7ebf，\u5b9e\u73b0\u57fa\u7840\u8ddf\u968f\u4e0e\u8def\u5f84\u89c4\u5212，\u9002\u914d\u5f00\u9614\u56ed\u533a\u73af\u5883\u3002<\/p>\n

    \u6838\u5fc3\u903b\u8f91
    \n\u901a\u8fc7\u5c04\u9891\u8bc6\u522b\u6216\u7ea2\u5916\u8ddf\u968f，\u5b9e\u65f6\u9501\u5b9a\u4eba\u5458\u4f4d\u7f6e，\u8c03\u6574\u884c\u9a76\u65b9\u5411\u4e0e\u901f\u5ea6
    \n\u7b80\u5316\u8def\u5f84\u89c4\u5212，\u907f\u5f00\u56fa\u5b9a\u969c\u788d，\u5b9e\u73b0\u5de1\u903b\u8def\u7ebf\u5f80\u8fd4\u5faa\u73af
    \n\u7ed3\u5408\u5de1\u903b\u72b6\u6001\u5207\u6362，\u6ca1\u6709\u4eba\u5458\u65f6\u81ea\u52a8\u5de1\u903b，\u6709\u4eba\u8ddf\u968f\u65f6\u5207\u6362\u8ddf\u968f\u6a21\u5f0f\u3002<\/p>\n

    #<\/span>include<\/span> <Arduino.h><\/span><\/span><\/p>\n

    \/\/ ========== \u786c\u4ef6\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> byte PWM_LEFT =<\/span> 9<\/span>,<\/span> DIR_LEFT =<\/span> 8<\/span>;<\/span>
    \nconst<\/span> byte PWM_RIGHT =<\/span> 10<\/span>,<\/span> DIR_RIGHT =<\/span> 11<\/span>;<\/span>
    \nconst<\/span> byte FOLLOW_PIN =<\/span> 2<\/span>;<\/span> \/\/ \u4eba\u5458\u8ddf\u968f\u4f20\u611f\u5668（\u7ea2\u5916\/RFID）<\/span>
    \nconst<\/span> byte OBSTACLE_PIN =<\/span> 3<\/span>;<\/span>\/\/ \u7b80\u6613\u969c\u788d\u68c0\u6d4b<\/span>
    \nconst<\/span> byte LED_ALARM =<\/span> 13<\/span>;<\/span> \/\/ \u5de1\u903b\u72b6\u6001\u6307\u793a\u706f<\/span><\/p>\n

    \/\/ ========== \u53c2\u6570\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> float<\/span> TARGET_FOLLOW_DIST =<\/span> 1.2<\/span>;<\/span> \/\/ \u8ddf\u968f\u8ddd\u79bb，\u5355\u4f4d\u7c73<\/span>
    \nconst<\/span> int<\/span> MOVE_BASE_PWM =<\/span> 120<\/span>;<\/span>
    \nconst<\/span> int<\/span> CORE_CHECK_INTERVAL =<\/span> 30<\/span>;<\/span> \/\/ \u6838\u5fc3\u68c0\u6d4b\u5468\u671f<\/span><\/p>\n

    \/\/ ========== \u72b6\u6001\u53d8\u91cf ==========<\/span>
    \nbool<\/span> followMode =<\/span> false<\/span>;<\/span>
    \nbool<\/span> patrolMode =<\/span> true<\/span>;<\/span>
    \nint<\/span> patrolWaypoints[<\/span>4<\/span>]<\/span>[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>{<\/span>0<\/span>,<\/span>0<\/span>}<\/span>,<\/span>{<\/span>5<\/span>,<\/span>0<\/span>}<\/span>,<\/span>{<\/span>5<\/span>,<\/span>5<\/span>}<\/span>,<\/span>{<\/span>0<\/span>,<\/span>5<\/span>}<\/span>}<\/span>;<\/span> \/\/ \u5de1\u903b\u70b9<\/span>
    \nint<\/span> currentWaypoint =<\/span> 0<\/span>;<\/span>
    \nunsigned<\/span> long<\/span> lastTime =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ ========== \u51fd\u6570\u58f0\u660e ==========<\/span>
    \nbool<\/span> detectPerson<\/span>(<\/span>)<\/span>;<\/span>
    \nbool<\/span> detectObstacle<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> adjustFollowSpeed<\/span>(<\/span>float<\/span> dist)<\/span>;<\/span>
    \nvoid<\/span> pathOptimize<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>int<\/span> leftPWM,<\/span> int<\/span> rightPWM)<\/span>;<\/span><\/p>\n

    void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u56ed\u533a\u5de1\u903b\u8ddf\u968f\u7cfb\u7edf\u542f\u52a8"<\/span>)<\/span>;<\/span><\/p>\n

    pinMode<\/span>(<\/span>PWM_LEFT,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>DIR_LEFT,<\/span> OUTPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>PWM_RIGHT,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>DIR_RIGHT,<\/span> OUTPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>FOLLOW_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>OBSTACLE_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>LED_ALARM,<\/span> OUTPUT)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
    \n unsigned<\/span> long<\/span> dt =<\/span> millis<\/span>(<\/span>)<\/span> –<\/span> lastTime;<\/span>
    \n if<\/span> (<\/span>dt <<\/span> CORE_CHECK_INTERVAL)<\/span> return<\/span>;<\/span>
    \n lastTime =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u68c0\u6d4b\u4eba\u5458\u4e0e\u969c\u788dbool person = detectPerson();<\/span>
    \n bool<\/span> obstacle =<\/span> detectObstacle<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u6a21\u5f0f\u5207\u6362<\/span>
    \n if<\/span> (<\/span>person)<\/span> {<\/span>
    \n followMode =<\/span> true<\/span>;<\/span>
    \n patrolMode =<\/span> false<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n followMode =<\/span> false<\/span>;<\/span>
    \n patrolMode =<\/span> true<\/span>;<\/span>
    \n }<\/span><\/p>\n

    int<\/span> leftPWM =<\/span> MOVE_BASE_PWM,<\/span> rightPWM =<\/span> MOVE_BASE_PWM;<\/span>
    \n if<\/span> (<\/span>obstacle)<\/span> {<\/span>
    \n \/\/ \u7b80\u6613\u907f\u969c，\u53cd\u5411\u5fae\u8f6c\u53d8\u5411<\/span>
    \n rightPWM =<\/span> MOVE_BASE_PWM –<\/span> 40<\/span>;<\/span>
    \n leftPWM =<\/span> MOVE_BASE_PWM –<\/span> 20<\/span>;<\/span>
    \n }<\/span> else<\/span> if<\/span> (<\/span>followMode)<\/span> {<\/span>
    \n \/\/ \u8ddf\u968f\u4eba\u5458，\u8c03\u6574\u901f\u5ea6<\/span>
    \n float<\/span> dist =<\/span> analogRead<\/span>(<\/span>FOLLOW_PIN)<\/span> *<\/span> 0.02<\/span>;<\/span> \/\/ \u6a21\u62df\u8ddd\u79bb<\/span>
    \n adjustFollowSpeed<\/span>(<\/span>dist)<\/span>;<\/span>
    \n leftPWM =<\/span> MOVE_BASE_PWM +<\/span> 10<\/span>;<\/span>
    \n rightPWM =<\/span> MOVE_BASE_PWM +<\/span> 10<\/span>;<\/span>
    \n }<\/span> else<\/span> if<\/span> (<\/span>patrolMode)<\/span> {<\/span>
    \n \/\/ \u8def\u5f84\u4f18\u5316\u540e\u7684\u5de1\u903b\u884c\u9a76<\/span>
    \n pathOptimize<\/span>(<\/span>)<\/span>;<\/span>
    \n leftPWM =<\/span> MOVE_BASE_PWM;<\/span>
    \n rightPWM =<\/span> MOVE_BASE_PWM;<\/span>
    \n }<\/span><\/p>\n

    driveMotor<\/span>(<\/span>leftPWM,<\/span> rightPWM)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>LED_ALARM,<\/span> followMode ?<\/span> HIGH :<\/span> LOW)<\/span>;<\/span><\/p>\n

    Serial.<\/span>print<\/span>(<\/span>"\u6a21\u5f0f："<\/span>)<\/span>;<\/span>Serial.<\/span>print<\/span>(<\/span>followMode ?<\/span> "\u8ddf\u968f"<\/span> :<\/span> "\u5de1\u903b"<\/span>)<\/span>;<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>" \u969c\u788d："<\/span>)<\/span>;<\/span>Serial.<\/span>println<\/span>(<\/span>obstacle)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u4eba\u5458\u68c0\u6d4b<\/span>
    \nbool<\/span> detectPerson<\/span>(<\/span>)<\/span> {<\/span>
    \n return<\/span> digitalRead<\/span>(<\/span>FOLLOW_PIN)<\/span> ==<\/span> LOW;<\/span> \/\/ \u4f20\u611f\u5668\u68c0\u6d4b\u5230\u4eba\u5458\u8fd4\u56detrue<\/span>
    \n}<\/span>\/\/ \u969c\u788d\u68c0\u6d4b<\/span>
    \nbool<\/span> detectObstacle<\/span>(<\/span>)<\/span> {<\/span>
    \n return<\/span> digitalRead<\/span>(<\/span>OBSTACLE_PIN)<\/span> ==<\/span> LOW;<\/span>
    \n}<\/span>\/\/ \u8ddf\u968f\u901f\u5ea6\u81ea\u9002\u5e94\u8c03\u6574<\/span>
    \nvoid<\/span> adjustFollowSpeed<\/span>(<\/span>float<\/span> dist)<\/span> {<\/span>
    \n if<\/span> (<\/span>dist <<\/span> TARGET_FOLLOW_DIST*<\/span>0.8<\/span>)<\/span> {<\/span>
    \n \/\/ \u8ddd\u79bb\u8fc7\u8fd1，\u51cf\u901f<\/span>
    \n MOVE_BASE_PWM =<\/span> 80<\/span>;<\/span>
    \n }<\/span> else<\/span> if<\/span> (<\/span>dist ><\/span> TARGET_FOLLOW_DIST*<\/span>1.2<\/span>)<\/span> {<\/span>
    \n \/\/ \u8ddd\u79bb\u8fc7\u8fdc，\u52a0\u901f<\/span>
    \n MOVE_BASE_PWM =<\/span> 150<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n MOVE_BASE_PWM =<\/span> 120<\/span>;<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7b80\u6613\u8def\u5f84\u4f18\u5316，\u51cf\u5c11\u5de1\u903b\u6298\u8fd4<\/span>
    \nvoid<\/span> pathOptimize<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u6839\u636e\u5f53\u524d\u4f4d\u7f6e\u81ea\u52a8\u5207\u6362\u5de1\u903b\u70b9，\u7b80\u5316\u4e3a\u987a\u5e8f\u5faa\u73af<\/span>
    \n currentWaypoint =<\/span> (<\/span>currentWaypoint +<\/span> 1<\/span>)<\/span> %<\/span> 4<\/span>;<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>"\u524d\u5f80\u5de1\u903b\u70b9："<\/span>)<\/span>;<\/span>Serial.<\/span>print<\/span>(<\/span>currentWaypoint)<\/span>;<\/span>
    \n}<\/span>\/\/ \u7535\u673a\u9a71\u52a8<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>int<\/span> leftPWM,<\/span> int<\/span> rightPWM)<\/span> {<\/span>
    \n leftPWM =<\/span> constrain<\/span>(<\/span>leftPWM,<\/span> 0<\/span>,<\/span> 200<\/span>)<\/span>;<\/span>
    \n rightPWM =<\/span> constrain<\/span>(<\/span>rightPWM,<\/span> 0<\/span>,<\/span> 200<\/span>)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>DIR_LEFT,<\/span> HIGH)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>DIR_RIGHT,<\/span> HIGH)<\/span>;<\/span>
    \n analogWrite<\/span>(<\/span>PWM_LEFT,<\/span> leftPWM)<\/span>;<\/span>
    \n analogWrite<\/span>(<\/span>PWM_RIGHT,<\/span> rightPWM)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \u9002\u7528\u573a\u666f\u4f18\u5316
    \n\u52a0\u5165GPS\/\u5317\u6597\u5b9a\u4f4d，\u5b9e\u73b0\u7cbe\u786e\u70b9\u4f4d\u5de1\u903b\u4e0e\u81ea\u4e3b\u8def\u5f84\u89c4\u5212
    \n\u4f18\u5316\u8ddf\u968f\u4f20\u611f\u5668，\u91c7\u7528\u89c6\u89c9\u6216\u6fc0\u5149\u8ddf\u968f，\u63d0\u5347\u4eba\u5458\u8bc6\u522b\u51c6\u786e\u6027
    \n\u589e\u52a0\u5de1\u903b\u8bb0\u5f55\u529f\u80fd，\u81ea\u52a8\u8bb0\u5f55\u5de1\u903b\u65f6\u95f4\u3001\u8def\u7ebf，\u4fbf\u4e8e\u5b89\u9632\u7ba1\u7406<\/p>\n

    5\u3001\u5b89\u9632\u5f02\u5e38\u8054\u52a8\u5de1\u903b\u8ddf\u968f\u7a0b\u5e8f（\u6838\u5fc3：\u5f02\u5e38\u89e6\u53d1+\u7d27\u6025\u8c03\u6574）
    \n\u9002\u7528\u573a\u666f：\u56ed\u533a\u5b89\u9632\u91cd\u70b9\u533a\u57df，\u9700\u5177\u5907\u5f02\u5e38\u68c0\u6d4b\u80fd\u529b，\u53d1\u73b0\u5165\u4fb5\u3001\u706b\u707e\u7b49\u5f02\u5e38\u65f6，\u81ea\u52a8\u8c03\u6574\u8def\u5f84\u5e76\u62a5\u8b66\u3002<\/p>\n

    \u6838\u5fc3\u903b\u8f91
    \n\u52a0\u5165\u5b89\u9632\u5f02\u5e38\u68c0\u6d4b\u6a21\u5757，\u5b9e\u65f6\u8bc6\u522b\u5165\u4fb5\u3001\u70df\u96fe\u7b49\u5f02\u5e38
    \n\u5f02\u5e38\u89e6\u53d1\u65f6，\u81ea\u52a8\u5207\u6362\u7d27\u6025\u6a21\u5f0f，\u4f18\u5316\u8def\u5f84\u524d\u5f80\u5f02\u5e38\u70b9
    \n\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u540c\u65f6\u76d1\u6d4b\u5f02\u5e38，\u5b9e\u73b0\u4eba\u673a\u534f\u540c\u7684\u5b89\u5168\u9632\u63a7\u3002<\/p>\n

    #<\/span>include<\/span> <Arduino.h><\/span><\/span><\/p>\n

    \/\/ ========== \u786c\u4ef6\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> byte PWM[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>9<\/span>,<\/span>10<\/span>}<\/span>,<\/span> DIR[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>8<\/span>,<\/span>11<\/span>}<\/span>;<\/span>
    \nconst<\/span> byte FOLLOW_PIN =<\/span> 2<\/span>;<\/span>
    \nconst<\/span> byte PIR_PIN =<\/span> 3<\/span>;<\/span> \/\/ \u4eba\u4f53\u5165\u4fb5\u4f20\u611f\u5668<\/span>
    \nconst<\/span> byte SMOKE_PIN =<\/span> 4<\/span>;<\/span> \/\/ \u70df\u96fe\u4f20\u611f\u5668<\/span>
    \nconst<\/span> byte ALARM_PIN =<\/span> 12<\/span>;<\/span> \/\/ \u62a5\u8b66\u5668<\/span>
    \nconst<\/span> byte LED_PIN =<\/span> 13<\/span>;<\/span><\/p>\n

    \/\/ ========== \u53c2\u6570\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> int<\/span> BASE_SPEED =<\/span> 130<\/span>;<\/span>
    \nconst<\/span> int<\/span> EMERGENCY_SPEED =<\/span> 180<\/span>;<\/span>
    \nconst<\/span> byte CHECK_INTERVAL =<\/span> 25<\/span>;<\/span><\/p>\n

    \/\/ ========== \u72b6\u6001\u53d8\u91cf ==========<\/span>
    \nbool<\/span> alertMode =<\/span> false<\/span>;<\/span>
    \nbool<\/span> followMode =<\/span> false<\/span>;<\/span>
    \nint<\/span> alertPos[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>0<\/span>,<\/span>0<\/span>}<\/span>;<\/span> \/\/ \u5f02\u5e38\u4f4d\u7f6e<\/span>
    \nunsigned<\/span> long<\/span> lastTime =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ ========== \u51fd\u6570\u58f0\u660e ==========<\/span>
    \nbool<\/span> detectAlert<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> getAlertPosition<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> emergencyPath<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>uint8_t<\/span> idx,<\/span> int<\/span> pwm)<\/span>;<\/span><\/p>\n

    void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u5b89\u9632\u5f02\u5e38\u8054\u52a8\u7cfb\u7edf\u542f\u52a8"<\/span>)<\/span>;<\/span><\/p>\n

    for<\/span> (<\/span>uint8_t<\/span> i =<\/span> 0<\/span>;<\/span> i <<\/span> 2<\/span>;<\/span> i++<\/span>)<\/span> {<\/span>
    \npinMode<\/span>(<\/span>PWM[<\/span>i]<\/span>,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>DIR[<\/span>i]<\/span>,<\/span> OUTPUT)<\/span>;<\/span>
    \n }<\/span>
    \n pinMode<\/span>(<\/span>FOLLOW_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>PIR_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>SMOKE_PIN,<\/span> INPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>ALARM_PIN,<\/span> OUTPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>LED_PIN,<\/span> OUTPUT)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
    \n unsigned<\/span> long<\/span> dt =<\/span> millis<\/span>(<\/span>)<\/span> –<\/span> lastTime;<\/span>
    \n if<\/span> (<\/span>dt <<\/span> CHECK_INTERVAL)<\/span> return<\/span>;<\/span>
    \n lastTime =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u5b89\u9632\u5f02\u5e38\u68c0\u6d4b<\/span>
    \n alertMode =<\/span> detectAlert<\/span>(<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>alertMode)<\/span> {<\/span>
    \n digitalWrite<\/span>(<\/span>ALARM_PIN,<\/span> HIGH)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>LED_PIN,<\/span> HIGH)<\/span>;<\/span>
    \n \/\/ \u5207\u6362\u7d27\u6025\u8def\u5f84，\u524d\u5f80\u5f02\u5e38\u70b9<\/span>
    \n int<\/span> pwm =<\/span> EMERGENCY_SPEED;<\/span>
    \n driveMotor<\/span>(<\/span>0<\/span>,<\/span> pwm)<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>1<\/span>,<\/span> pwm)<\/span>;<\/span>
    \n \/\/ \u8def\u5f84\u4f18\u5316，\u76f4\u8fbe\u5f02\u5e38\u70b9<\/span>
    \n emergencyPath<\/span>(<\/span>)<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n digitalWrite<\/span>(<\/span>ALARM_PIN,<\/span> LOW)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>LED_PIN,<\/span> LOW)<\/span>;<\/span>
    \n \/\/ \u6b63\u5e38\u8ddf\u968f\u6216\u5de1\u903b<\/span>
    \n bool<\/span> person =<\/span> digitalRead<\/span>(<\/span>FOLLOW_PIN)<\/span> ==<\/span> LOW;<\/span>
    \n if<\/span> (<\/span>person)<\/span> {<\/span>
    \n followMode =<\/span> true<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>0<\/span>,<\/span> BASE_SPEED)<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>1<\/span>,<\/span> BASE_SPEED)<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n followMode =<\/span> false<\/span>;<\/span>
    \n \/\/ \u5e38\u89c4\u5de1\u903b<\/span>
    \n driveMotor<\/span>(<\/span>0<\/span>,<\/span> BASE_SPEED –<\/span> 10<\/span>)<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>1<\/span>,<\/span> BASE_SPEED –<\/span> 10<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n }<\/span><\/p>\n

    Serial.<\/span>print<\/span>(<\/span>"\u9884\u8b66："<\/span>)<\/span>;<\/span>Serial.<\/span>println<\/span>(<\/span>alertMode)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u5f02\u5e38\u68c0\u6d4b，\u5165\u4fb5\u6216\u70df\u96fe<\/span>
    \nbool<\/span> detectAlert<\/span>(<\/span>)<\/span> {<\/span>
    \n bool<\/span> pirAlert =<\/span> digitalRead<\/span>(<\/span>PIR_PIN)<\/span> ==<\/span> LOW;<\/span>
    \n bool<\/span> smokeAlert =<\/span> analogRead<\/span>(<\/span>SMOKE_PIN)<\/span> ><\/span> 600<\/span>;<\/span>
    \n return<\/span> pirAlert ||<\/span> smokeAlert;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u83b7\u53d6\u5f02\u5e38\u4f4d\u7f6e，\u7b80\u5316\u4e3a\u56fa\u5b9a\u70b9，\u5b9e\u9645\u7ed3\u5408\u5b9a\u4f4d<\/span>
    \nvoid<\/span> getAlertPosition<\/span>(<\/span>)<\/span> {<\/span>
    \n alertPos[<\/span>0<\/span>]<\/span> =<\/span> 4<\/span>;<\/span> alertPos[<\/span>1<\/span>]<\/span> =<\/span> 3<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7d27\u6025\u8def\u5f84\u4f18\u5316，\u76f4\u8fbe\u5f02\u5e38\u70b9<\/span>
    \nvoid<\/span> emergencyPath<\/span>(<\/span>)<\/span> {<\/span>
    \n getAlertPosition<\/span>(<\/span>)<\/span>;<\/span>
    \n \/\/ \u7b80\u5316\u903b\u8f91，\u6839\u636e\u5f53\u524d\u4f4d\u7f6e\u8c03\u6574\u65b9\u5411<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>"\u524d\u5f80\u5f02\u5e38\u70b9："<\/span>)<\/span>;<\/span>Serial.<\/span>print<\/span>(<\/span>alertPos[<\/span>0<\/span>]<\/span>)<\/span>;<\/span>Serial.<\/span>print<\/span>(<\/span>","<\/span>)<\/span>;<\/span>Serial.<\/span>println<\/span>(<\/span>alertPos[<\/span>1<\/span>]<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7535\u673a\u9a71\u52a8<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>uint8_t<\/span> idx,<\/span> int<\/span> pwm)<\/span> {<\/span>
    \n pwm =<\/span> constrain<\/span>(<\/span>pwm,<\/span> 0<\/span>,<\/span> 200<\/span>)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>DIR[<\/span>idx]<\/span>,<\/span> HIGH)<\/span>;<\/span>
    \n analogWrite<\/span>(<\/span>PWM[<\/span>idx]<\/span>,<\/span> pwm)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \u9002\u7528\u573a\u666f\u4f18\u5316
    \n\u52a0\u5165\u5f02\u5e38\u5b9a\u4f4d\u529f\u80fd，\u7ed3\u5408\u4f20\u611f\u5668\u4e0e\u5b9a\u4f4d\u7cfb\u7edf，\u7cbe\u51c6\u9501\u5b9a\u5f02\u5e38\u70b9\u4f4d
    \n\u6dfb\u52a0\u5f02\u5e38\u4f18\u5148\u7ea7，\u533a\u5206\u4e25\u91cd\u4e0e\u8f7b\u5fae\u5f02\u5e38，\u5206\u7ea7\u8c03\u6574\u54cd\u5e94\u7b56\u7565
    \n\u8054\u52a8\u540e\u53f0\u7ba1\u7406\u7cfb\u7edf，\u5f02\u5e38\u53d1\u751f\u65f6\u81ea\u52a8\u63a8\u9001\u62a5\u8b66\u4fe1\u606f，\u5b9e\u73b0\u8fdc\u7a0b\u76d1\u63a7<\/p>\n

    6\u3001\u591a\u673a\u534f\u540c\u5de1\u903b+\u8ddf\u968f\u8def\u5f84\u4f18\u5316\u7a0b\u5e8f（\u6838\u5fc3：\u591a\u673a\u8054\u52a8+\u9ad8\u6548\u5de1\u903b）
    \n\u9002\u7528\u573a\u666f：\u5927\u578b\u56ed\u533a，\u9700\u591a\u53f0\u673a\u5668\u4eba\u534f\u540c\u5de1\u903b，\u5212\u5206\u533a\u57df\u3001\u914d\u5408\u8ddf\u8fdb，\u63d0\u5347\u5de1\u903b\u8986\u76d6\u6548\u7387\u3002<\/p>\n

    \u6838\u5fc3\u903b\u8f91
    \n\u4e3b\u4ece\u673a\u901a\u4fe1，\u5b9e\u73b0\u591a\u673a\u4f4d\u7f6e\u540c\u6b65\u4e0e\u4efb\u52a1\u5206\u914d
    \n\u4e3b\u673a\u8d1f\u8d23\u8def\u7ebf\u89c4\u5212，\u4ece\u673a\u8ddf\u968f\u4e3b\u673a，\u8986\u76d6\u4e0d\u540c\u533a\u57df
    \n\u7ed3\u5408\u533a\u57df\u5212\u5206，\u4f18\u5316\u591a\u673a\u5de1\u903b\u8def\u5f84，\u907f\u514d\u91cd\u53e0，\u63d0\u5347\u6548\u7387\u3002<\/p>\n

    #<\/span>include<\/span> <Arduino.h><\/span><\/span><\/p>\n

    \/\/ ========== \u786c\u4ef6\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> byte PWM[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>9<\/span>,<\/span>10<\/span>}<\/span>,<\/span> DIR[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>8<\/span>,<\/span>11<\/span>}<\/span>;<\/span>
    \nconst<\/span> byte COM_PIN =<\/span> 2<\/span>;<\/span> \/\/ \u4e32\u53e3\u901a\u4fe1\u5f15\u811a<\/span>
    \nconst<\/span> byte FOLLOW_PIN =<\/span> 3<\/span>;<\/span> \/\/ \u4e3b\u673a\u8ddf\u968f\u68c0\u6d4b<\/span>
    \nconst<\/span> byte ROLE_PIN =<\/span> 4<\/span>;<\/span> \/\/ \u89d2\u8272\u5b9a\u4e49（\u4e3b\u673a\/\u4ece\u673a）<\/span><\/p>\n

    \/\/ ========== \u53c2\u6570\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> int<\/span> BASE_SPEED =<\/span> 120<\/span>;<\/span>
    \nconst<\/span> byte COM_INTERVAL =<\/span> 40<\/span>;<\/span>
    \nbool<\/span> isMaster =<\/span> false<\/span>;<\/span>
    \nint<\/span> slavePos[<\/span>2<\/span>]<\/span> =<\/span> {<\/span>0<\/span>,<\/span>0<\/span>}<\/span>;<\/span>\/\/ \u4ece\u673a\u4f4d\u7f6e<\/span><\/p>\n

    \/\/ ========== \u51fd\u6570\u58f0\u660e ==========<\/span>
    \nvoid<\/span> readRole<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> masterPlan<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> slaveFollow<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> pathOptimizeMulti<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>uint8_t<\/span> idx,<\/span> int<\/span> pwm)<\/span>;<\/span><\/p>\n

    void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>begin<\/span>(<\/span>9600<\/span>)<\/span>;<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u591a\u673a\u534f\u540c\u5de1\u903b\u7cfb\u7edf\u542f\u52a8"<\/span>)<\/span>;<\/span><\/p>\n

    for<\/span> (<\/span>uint8_t<\/span> i =<\/span> 0<\/span>;<\/span> i <<\/span> 2<\/span>;<\/span> i++<\/span>)<\/span> {<\/span>
    \npinMode<\/span>(<\/span>PWM[<\/span>i]<\/span>,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>DIR[<\/span>i]<\/span>,<\/span> OUTPUT)<\/span>;<\/span>
    \n }<\/span>
    \n pinMode<\/span>(<\/span>COM_PIN,<\/span> INPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>FOLLOW_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>ROLE_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n readRole<\/span>(<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
    \n if<\/span> (<\/span>millis<\/span>(<\/span>)<\/span> %<\/span> COM_INTERVAL !=<\/span> 0<\/span>)<\/span> return<\/span>;<\/span><\/p>\n

    if<\/span> (<\/span>isMaster)<\/span> {<\/span>
    \n \/\/ \u4e3b\u673a：\u8def\u5f84\u89c4\u5212+\u901a\u4fe1\u53d1\u9001<\/span>
    \n masterPlan<\/span>(<\/span>)<\/span>;<\/span>
    \n pathOptimizeMulti<\/span>(<\/span>)<\/span>;<\/span>
    \n \/\/ \u5411\u4ece\u673a\u53d1\u9001\u4f4d\u7f6e\u4e0e\u6307\u4ee4<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>"MP:"<\/span>)<\/span>;<\/span>Serial.<\/span>print<\/span>(<\/span>"1,2"<\/span>)<\/span>;<\/span>Serial.<\/span>println<\/span>(<\/span>";"<\/span>)<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n \/\/ \u4ece\u673a：\u63a5\u6536\u4e3b\u673a\u4fe1\u606f+\u8ddf\u968f<\/span>
    \n slaveFollow<\/span>(<\/span>)<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u5171\u540c\u884c\u9a76<\/span>
    \n int<\/span> pwm =<\/span> BASE_SPEED;<\/span>
    \n driveMotor<\/span>(<\/span>0<\/span>,<\/span> pwm)<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>1<\/span>,<\/span> pwm)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u8bfb\u53d6\u673a\u5668\u4eba\u89d2\u8272<\/span>
    \nvoid<\/span> readRole<\/span>(<\/span>)<\/span> {<\/span>
    \n isMaster =<\/span> digitalRead<\/span>(<\/span>ROLE_PIN)<\/span> ==<\/span> HIGH;<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>"\u89d2\u8272："<\/span>)<\/span>;<\/span>Serial.<\/span>println<\/span>(<\/span>isMaster ?<\/span> "\u4e3b\u673a"<\/span> :<\/span> "\u4ece\u673a"<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u4e3b\u673a\u8def\u5f84\u89c4\u5212<\/span>
    \nvoid<\/span> masterPlan<\/span>(<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u4e3b\u673a\u6267\u884c\u5168\u5c40\u8def\u5f84\u89c4\u5212"<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u4ece\u673a\u8ddf\u968f\u4e3b\u673a<\/span>
    \nvoid<\/span> slaveFollow<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u7b80\u5316：\u63a5\u6536\u4e3b\u673a\u6570\u636e\u5e76\u8ddf\u968f<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u4ece\u673a\u540c\u6b65\u4e3b\u673a\u4f4d\u7f6e，\u6267\u884c\u8ddf\u968f"<\/span>)<\/span>;<\/span>
    \n}<\/span>void<\/span> \u591a\u673a\u8def\u5f84\u4f18\u5316，\u5212\u5206\u533a\u57df\u907f\u514d\u91cd\u53e0
    \nvoid<\/span> pathOptimizeMulti<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u7b80\u5316\u533a\u57df\u5212\u5206，\u4e3b\u673a\u8d1f\u8d23\u5317\u533a，\u4ece\u673a\u8d1f\u8d23\u5357\u533a<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u8def\u5f84\u4f18\u5316：\u591a\u673a\u5206\u533a\u5de1\u903b，\u907f\u514d\u91cd\u590d\u8986\u76d6"<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7535\u673a\u9a71\u52a8<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>uint8_t<\/span> idx,<\/span> int<\/span> pwm)<\/span> {<\/span>
    \n pwm =<\/span> constrain<\/span>(<\/span>pwm,<\/span> 0<\/span>,<\/span> 200<\/span>)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>DIR[<\/span>idx]<\/span>,<\/span> HIGH)<\/span>;<\/span>
    \n analogWrite<\/span>(<\/span>PWM[<\/span>idx]<\/span>,<\/span> pwm)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \u9002\u7528\u573a\u666f\u4f18\u5316
    \n\u4f18\u5316\u901a\u4fe1\u534f\u8bae，\u5b9e\u73b0\u9ad8\u901f\u7a33\u5b9a\u7684\u591a\u673a\u6570\u636e\u4ea4\u4e92，\u964d\u4f4e\u5ef6\u8fdf
    \n\u52a0\u5165\u52a8\u6001\u533a\u57df\u5206\u914d，\u6839\u636e\u673a\u5668\u4eba\u4f4d\u7f6e\u5b9e\u65f6\u8c03\u6574\u5de1\u903b\u533a\u57df，\u63d0\u5347\u7075\u6d3b\u6027
    \n\u589e\u52a0\u591a\u673a\u907f\u969c\u903b\u8f91，\u907f\u514d\u673a\u5668\u4eba\u4e4b\u95f4\u78b0\u649e，\u4fdd\u969c\u534f\u540c\u5b89\u5168<\/p>\n

    \u8981\u70b9\u89e3\u8bfb
    \n\u8981\u70b91：\u8ddf\u968f\u5b9a\u4f4d\u7cbe\u51c6\u5ea6\u51b3\u5b9a\u8ddf\u968f\u7a33\u5b9a\u6027
    \n\u8ddf\u968f\u662f\u56ed\u533a\u5de1\u903b\u7684\u6838\u5fc3\u529f\u80fd，\u5b9a\u4f4d\u7cbe\u51c6\u624d\u80fd\u5b9e\u73b0\u7a33\u5b9a\u8ddf\u968f\u3002
    \n\u591a\u4f20\u611f\u5668\u878d\u5408\u5b9a\u4f4d：\u7ed3\u5408\u7ea2\u5916\u3001\u6fc0\u5149\u3001\u89c6\u89c9\u4e0e\u5b9a\u4f4d\u7cfb\u7edf，\u63d0\u5347\u4eba\u5458\u4f4d\u7f6e\u68c0\u6d4b\u7cbe\u5ea6，\u51cf\u5c11\u8ddf\u968f\u504f\u5dee
    \n\u52a8\u6001\u901f\u5ea6\u5339\u914d：\u6839\u636e\u8ddf\u968f\u8ddd\u79bb\u5b9e\u65f6\u8c03\u6574\u8f66\u901f，\u8fc7\u8fd1\u51cf\u901f\u3001\u8fc7\u8fdc\u52a0\u901f，\u4fdd\u6301\u7a33\u5b9a\u8ddf\u968f\u8ddd\u79bb
    \n\u6297\u5e72\u6270\u8bbe\u8ba1：\u56ed\u533a\u73af\u5883\u4eba\u6d41\u3001\u969c\u788d\u7269\u591a，\u9700\u5bf9\u4f20\u611f\u5668\u6570\u636e\u6ee4\u6ce2，\u907f\u514d\u8bef\u8ddf\u968f\u3001\u9519\u8ddf\u968f
    \n\u8981\u70b92：\u8def\u5f84\u4f18\u5316\u9700\u517c\u987e\u6548\u7387\u4e0e\u8986\u76d6
    \n\u8def\u5f84\u4f18\u5316\u7684\u6838\u5fc3\u662f\u63d0\u5347\u5de1\u903b\u6548\u7387，\u540c\u65f6\u4fdd\u8bc1\u56ed\u533a\u5168\u9762\u8986\u76d6\u3002
    \n\u5168\u5c40\u4e0e\u5c40\u90e8\u7ed3\u5408：\u4e3b\u673a\u8d1f\u8d23\u5168\u5c40\u533a\u57df\u5212\u5206\u4e0e\u5b8f\u89c2\u8def\u5f84\u89c4\u5212，\u4ece\u673a\u8d1f\u8d23\u5c40\u90e8\u8ddf\u968f，\u5b9e\u73b0\u9ad8\u6548\u534f\u540c
    \n\u51cf\u5c11\u65e0\u6548\u6298\u8fd4：\u901a\u8fc7\u5408\u7406\u89c4\u5212\u5de1\u903b\u70b9\u4e0e\u8def\u7ebf，\u51cf\u5c11\u91cd\u590d\u8def\u6bb5，\u63d0\u5347\u5de1\u903b\u8986\u76d6\u6548\u7387
    \n\u52a8\u6001\u8c03\u6574\u8def\u5f84：\u9047\u5230\u5f02\u5e38\u6216\u4eba\u5458\u65f6，\u4e34\u65f6\u4f18\u5316\u8def\u5f84，\u4f18\u5148\u5904\u7406\u5b89\u9632\u4e8b\u4ef6，\u517c\u987e\u7075\u6d3b\u4e0e\u9ad8\u6548
    \n\u8981\u70b93：\u5b89\u9632\u5f02\u5e38\u8054\u52a8\u9700\u5feb\u901f\u54cd\u5e94
    \n\u56ed\u533a\u5b89\u9632\u7684\u6838\u5fc3\u662f\u5feb\u901f\u53d1\u73b0\u5e76\u5904\u7406\u5f02\u5e38，\u8054\u52a8\u673a\u5236\u81f3\u5173\u91cd\u8981\u3002
    \n\u591a\u5f02\u5e38\u68c0\u6d4b\u878d\u5408：\u96c6\u6210\u5165\u4fb5\u3001\u70df\u96fe\u3001\u706b\u707e\u7b49\u5b89\u9632\u4f20\u611f\u5668，\u5168\u65b9\u4f4d\u76d1\u6d4b\u5f02\u5e38，\u907f\u514d\u6f0f\u68c0
    \n\u5206\u7ea7\u54cd\u5e94\u673a\u5236：\u533a\u5206\u5f02\u5e38\u7b49\u7ea7，\u7d27\u6025\u5f02\u5e38\u9ad8\u901f\u524d\u5f80，\u8f7b\u5fae\u5f02\u5e38\u964d\u901f\u6838\u67e5，\u63d0\u5347\u54cd\u5e94\u5408\u7406\u6027
    \n\u4eba\u673a\u534f\u540c\u8054\u52a8：\u5f02\u5e38\u65f6\u81ea\u52a8\u8c03\u6574\u8def\u5f84，\u540c\u65f6\u8054\u52a8\u5b89\u4fdd\u4eba\u5458，\u5b9e\u73b0\u4eba\u673a\u534f\u540c\u7684\u5feb\u901f\u5904\u7f6e
    \n\u8981\u70b94：\u591a\u673a\u534f\u540c\u9700\u4fdd\u969c\u540c\u6b65\u4e0e\u907f\u78b0
    \n\u591a\u673a\u5668\u4eba\u534f\u540c\u53ef\u5927\u5e45\u63d0\u5347\u5de1\u903b\u6548\u7387，\u9700\u89e3\u51b3\u540c\u6b65\u3001\u907f\u78b0\u4e0e\u8c03\u5ea6\u95ee\u9898\u3002
    \n\u65f6\u949f\u540c\u6b65：\u4e3b\u673a\u4e0e\u4ece\u673a\u7edf\u4e00\u63a7\u5236\u5468\u671f，\u4fdd\u969c\u6570\u636e\u540c\u6b65\u4e0e\u52a8\u4f5c\u4e00\u81f4\u6027，\u907f\u514d\u4efb\u52a1\u51b2\u7a81
    \n\u901a\u4fe1\u7a33\u5b9a\u53ef\u9760：\u91c7\u7528\u7a33\u5b9a\u7684\u901a\u4fe1\u534f\u8bae，\u786e\u4fdd\u4f4d\u7f6e\u3001\u6307\u4ee4\u51c6\u786e\u4f20\u8f93，\u907f\u514d\u901a\u4fe1\u4e2d\u65ad\u5bfc\u81f4\u5931\u63a7
    \n\u534f\u540c\u907f\u78b0\u89c4\u5219：\u9884\u8bbe\u591a\u673a\u5668\u4eba\u907f\u78b0\u903b\u8f91，\u907f\u514d\u76f8\u4e92\u78b0\u649e，\u4fdd\u969c\u534f\u540c\u8fc7\u7a0b\u5b89\u5168
    \n\u8981\u70b95：\u7cfb\u7edf\u7a33\u5b9a\u4e0e\u7eed\u822a\u4fdd\u969c，\u9002\u914d\u957f\u65f6\u95f4\u5de1\u903b
    \n\u56ed\u533a\u5de1\u903b\u591a\u4e3a\u957f\u65f6\u95f4\u4f5c\u4e1a，\u9700\u4fdd\u969c\u7cfb\u7edf\u7a33\u5b9a\u4e0e\u7eed\u822a\u80fd\u529b\u3002
    \n\u4f4e\u529f\u8017\u8bbe\u8ba1：\u4f18\u5316\u7535\u673a\u4e0e\u4f20\u611f\u5668\u529f\u8017，\u91c7\u7528\u8282\u80fd\u6a21\u5f0f，\u5ef6\u957f\u5355\u6b21\u5145\u7535\u7eed\u822a\u65f6\u95f4
    \n\u6545\u969c\u81ea\u68c0\u4e0e\u5bb9\u9519：\u52a8\u6001\u76d1\u6d4b\u7cfb\u7edf\u72b6\u6001，\u53d1\u73b0\u5f02\u5e38\u53ca\u65f6\u964d\u901f\u6216\u8fd4\u822a，\u907f\u514d\u4efb\u52a1\u4e2d\u65ad
    \n\u7eed\u822a\u667a\u80fd\u7ba1\u7406：\u7535\u91cf\u8fc7\u4f4e\u65f6\u81ea\u52a8\u89c4\u5212\u8fd4\u822a\u8def\u5f84，\u8fd4\u56de\u5145\u7535，\u4fdd\u969c\u5de1\u903b\u4efb\u52a1\u6301\u7eed\u8fdb\u884c<\/p>\n

    \u8bf7\u6ce8\u610f：\u4ee5\u4e0a\u6848\u4f8b\u4ec5\u4f5c\u4e3a\u601d\u8def\u62d3\u5c55\u7684\u53c2\u8003\u793a\u4f8b，\u4e0d\u4fdd\u8bc1\u5b8c\u5168\u6b63\u786e\u3001\u9002\u914d\u6240\u6709\u573a\u666f\u6216\u53ef\u76f4\u63a5\u7f16\u8bd1\u8fd0\u884c\u3002\u7531\u4e8e\u786c\u4ef6\u5e73\u53f0\u3001\u5b9e\u9645\u4f7f\u7528\u573a\u666f\u3001Arduino \u7248\u672c\u7684\u5dee\u5f02，\u5747\u53ef\u80fd\u5f71\u54cd\u4ee3\u7801\u7684\u9002\u914d\u6027\u4e0e\u4f7f\u7528\u65b9\u6cd5\u7684\u9009\u62e9\u3002\u5728\u5b9e\u9645\u7f16\u7a0b\u5f00\u53d1\u65f6，\u8bf7\u52a1\u5fc5\u6839\u636e\u81ea\u8eab\u786c\u4ef6\u914d\u7f6e\u3001\u4f7f\u7528\u573a\u666f\u53ca\u5177\u4f53\u529f\u80fd\u9700\u6c42\u8fdb\u884c\u9488\u5bf9\u6027\u8c03\u6574，\u5e76\u901a\u8fc7\u591a\u6b21\u5b9e\u6d4b\u9a8c\u8bc1\u6548\u679c；\u540c\u65f6\u9700\u786e\u4fdd\u786c\u4ef6\u63a5\u7ebf\u6b63\u786e，\u5145\u5206\u4e86\u89e3\u6240\u7528\u4f20\u611f\u5668\u3001\u6267\u884c\u5668\u7b49\u8bbe\u5907\u7684\u6280\u672f\u89c4\u8303\u4e0e\u6838\u5fc3\u7279\u6027\u3002\u5bf9\u4e8e\u6d89\u53ca\u786c\u4ef6\u64cd\u4f5c\u7684\u4ee3\u7801，\u4f7f\u7528\u524d\u52a1\u5fc5\u6838\u5bf9\u5f15\u811a\u5b9a\u4e49\u3001\u7535\u5e73\u53c2\u6570\u7b49\u5173\u952e\u4fe1\u606f\u7684\u51c6\u786e\u6027\u4e0e\u5b89\u5168\u6027，\u907f\u514d\u56e0\u53c2\u6570\u9519\u8bef\u5bfc\u81f4\u786c\u4ef6\u635f\u574f\u6216\u8fd0\u884c\u5f02\u5e38\u3002<\/p>\n

    \"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"<\/p>\n","protected":false},"excerpt":{"rendered":"

    Arduino BLDC \u4e4b\u56ed\u533a\u5de1\u903b\u8ddf\u968f \u8def\u5f84\u4f18\u5316\u7684\u672c\u8d28，\u662f\u628a\u201c\u56fa\u5b9a\u5de1\u903b\u8def\u5f84\u3001\u52a8\u6001\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u3001\u5b9e\u65f6\u907f\u969c\u4e0e\u8def\u5f84\u91cd\u89c4\u5212\u201d\u6574\u5408\u5230\u4e00\u4e2a\u5206\u5c42\u95ed\u73af\u4e2d：\u4e0a\u4f4d\u673a\u8d1f\u8d23\u5730\u56fe\u3001\u8def\u5f84\u4e0e\u76ee\u6807\u8bc6\u522b，\u4e0b\u4f4d\u673a\u8d1f\u8d23 BLDC \u5dee\u901f\u95ed\u73af\u3001\u8ddf\u968f\u63a7\u5236\u548c\u5c40\u90e8\u907f\u969c\u3002 \u5b83\u65e2\u8981\u80fd\u6309\u9884\u8bbe\u8def\u7ebf\u5de1\u822a，\u4e5f\u8981\u80fd\u5728\u9047\u5230\u884c\u4eba\u3001\u8f66\u8f86\u3001\u4e34\u65f6\u969c\u788d\u6216\u5b89\u4fdd\u4eba\u5458\u79fb\u52a8\u65f6\u5e73\u6ed1\u5207\u6362\u884c\u4e3a，\u800c\u4e0d\u662f\u7b80\u5355\u5730\u5728\u201c\u5de1\u903b\u201d\u548c\u201c\u8ddf\u968f\u201d\u4e4b\u95f4\u786c\u5207\u6362\u3002
    \n\u4e3b\u8981\u7279\u70b9
    \n. \u5de1\u903b\u8ddf\u968f\u53cc\u6a21\u5f0f\u534f\u540c
    \n\u7cfb\u7edf\u901a\u5e38\u5177\u5907\u81f3\u5c11<\/p>\n","protected":false},"author":2,"featured_media":111776,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[12720,12719,55,270,269],"topic":[],"class_list":["post-111780","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-server","tag-12720","tag-12719","tag-c","tag-270","tag-269"],"yoast_head":"\n\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3<\/title>\n<meta name=\"robots\" content=\"index, follow, max-snippet:-1, max-image-preview:large, max-video-preview:-1\" \/>\n<link rel=\"canonical\" href=\"https:\/\/www.wsisp.com\/helps\/111780.html\" \/>\n<meta property=\"og:locale\" content=\"zh_CN\" \/>\n<meta property=\"og:type\" content=\"article\" \/>\n<meta property=\"og:title\" content=\"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"og:description\" content=\"Arduino BLDC \u4e4b\u56ed\u533a\u5de1\u903b\u8ddf\u968f \u8def\u5f84\u4f18\u5316\u7684\u672c\u8d28，\u662f\u628a\u201c\u56fa\u5b9a\u5de1\u903b\u8def\u5f84\u3001\u52a8\u6001\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u3001\u5b9e\u65f6\u907f\u969c\u4e0e\u8def\u5f84\u91cd\u89c4\u5212\u201d\u6574\u5408\u5230\u4e00\u4e2a\u5206\u5c42\u95ed\u73af\u4e2d：\u4e0a\u4f4d\u673a\u8d1f\u8d23\u5730\u56fe\u3001\u8def\u5f84\u4e0e\u76ee\u6807\u8bc6\u522b，\u4e0b\u4f4d\u673a\u8d1f\u8d23 BLDC \u5dee\u901f\u95ed\u73af\u3001\u8ddf\u968f\u63a7\u5236\u548c\u5c40\u90e8\u907f\u969c\u3002 \u5b83\u65e2\u8981\u80fd\u6309\u9884\u8bbe\u8def\u7ebf\u5de1\u822a，\u4e5f\u8981\u80fd\u5728\u9047\u5230\u884c\u4eba\u3001\u8f66\u8f86\u3001\u4e34\u65f6\u969c\u788d\u6216\u5b89\u4fdd\u4eba\u5458\u79fb\u52a8\u65f6\u5e73\u6ed1\u5207\u6362\u884c\u4e3a，\u800c\u4e0d\u662f\u7b80\u5355\u5730\u5728\u201c\u5de1\u903b\u201d\u548c\u201c\u8ddf\u968f\u201d\u4e4b\u95f4\u786c\u5207\u6362\u3002 \u4e3b\u8981\u7279\u70b9 . \u5de1\u903b\u8ddf\u968f\u53cc\u6a21\u5f0f\u534f\u540c \u7cfb\u7edf\u901a\u5e38\u5177\u5907\u81f3\u5c11\" \/>\n<meta property=\"og:url\" content=\"https:\/\/www.wsisp.com\/helps\/111780.html\" \/>\n<meta property=\"og:site_name\" content=\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"article:published_time\" content=\"2026-10-01T12:41:05+00:00\" \/>\n<meta property=\"og:image\" content=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/10\/20261001124102-6abe54deb0cf1.gif\" \/>\n<meta name=\"author\" content=\"admin\" \/>\n<meta name=\"twitter:card\" content=\"summary_large_image\" \/>\n<meta name=\"twitter:label1\" content=\"\u4f5c\u8005\" \/>\n\t<meta name=\"twitter:data1\" content=\"admin\" \/>\n\t<meta name=\"twitter:label2\" content=\"\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4\" \/>\n\t<meta name=\"twitter:data2\" content=\"11 \u5206\" \/>\n<script type=\"application\/ld+json\" class=\"yoast-schema-graph\">{\"@context\":\"https:\/\/schema.org\",\"@graph\":[{\"@type\":\"WebPage\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/111780.html\",\"url\":\"https:\/\/www.wsisp.com\/helps\/111780.html\",\"name\":\"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"isPartOf\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\"},\"datePublished\":\"2026-10-01T12:41:05+00:00\",\"dateModified\":\"2026-10-01T12:41:05+00:00\",\"author\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\"},\"breadcrumb\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/111780.html#breadcrumb\"},\"inLanguage\":\"zh-Hans\",\"potentialAction\":[{\"@type\":\"ReadAction\",\"target\":[\"https:\/\/www.wsisp.com\/helps\/111780.html\"]}]},{\"@type\":\"BreadcrumbList\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/111780.html#breadcrumb\",\"itemListElement\":[{\"@type\":\"ListItem\",\"position\":1,\"name\":\"\u9996\u9875\",\"item\":\"https:\/\/www.wsisp.com\/helps\"},{\"@type\":\"ListItem\",\"position\":2,\"name\":\"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f\"}]},{\"@type\":\"WebSite\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\",\"url\":\"https:\/\/www.wsisp.com\/helps\/\",\"name\":\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"description\":\"\u9999\u6e2f\u670d\u52a1\u5668_\u9999\u6e2f\u4e91\u670d\u52a1\u5668\u8d44\u8baf_\u670d\u52a1\u5668\u5e2e\u52a9\u6587\u6863_\u670d\u52a1\u5668\u6559\u7a0b\",\"potentialAction\":[{\"@type\":\"SearchAction\",\"target\":{\"@type\":\"EntryPoint\",\"urlTemplate\":\"https:\/\/www.wsisp.com\/helps\/?s={search_term_string}\"},\"query-input\":\"required name=search_term_string\"}],\"inLanguage\":\"zh-Hans\"},{\"@type\":\"Person\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\",\"name\":\"admin\",\"image\":{\"@type\":\"ImageObject\",\"inLanguage\":\"zh-Hans\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/image\/\",\"url\":\"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery\",\"contentUrl\":\"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery\",\"caption\":\"admin\"},\"sameAs\":[\"http:\/\/wp.wsisp.com\"],\"url\":\"https:\/\/www.wsisp.com\/helps\/author\/admin\"}]}<\/script>\n<!-- \/ Yoast SEO plugin. -->","yoast_head_json":{"title":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","robots":{"index":"index","follow":"follow","max-snippet":"max-snippet:-1","max-image-preview":"max-image-preview:large","max-video-preview":"max-video-preview:-1"},"canonical":"https:\/\/www.wsisp.com\/helps\/111780.html","og_locale":"zh_CN","og_type":"article","og_title":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","og_description":"Arduino BLDC \u4e4b\u56ed\u533a\u5de1\u903b\u8ddf\u968f \u8def\u5f84\u4f18\u5316\u7684\u672c\u8d28，\u662f\u628a\u201c\u56fa\u5b9a\u5de1\u903b\u8def\u5f84\u3001\u52a8\u6001\u8ddf\u968f\u5b89\u4fdd\u4eba\u5458\u3001\u5b9e\u65f6\u907f\u969c\u4e0e\u8def\u5f84\u91cd\u89c4\u5212\u201d\u6574\u5408\u5230\u4e00\u4e2a\u5206\u5c42\u95ed\u73af\u4e2d：\u4e0a\u4f4d\u673a\u8d1f\u8d23\u5730\u56fe\u3001\u8def\u5f84\u4e0e\u76ee\u6807\u8bc6\u522b，\u4e0b\u4f4d\u673a\u8d1f\u8d23 BLDC \u5dee\u901f\u95ed\u73af\u3001\u8ddf\u968f\u63a7\u5236\u548c\u5c40\u90e8\u907f\u969c\u3002 \u5b83\u65e2\u8981\u80fd\u6309\u9884\u8bbe\u8def\u7ebf\u5de1\u822a，\u4e5f\u8981\u80fd\u5728\u9047\u5230\u884c\u4eba\u3001\u8f66\u8f86\u3001\u4e34\u65f6\u969c\u788d\u6216\u5b89\u4fdd\u4eba\u5458\u79fb\u52a8\u65f6\u5e73\u6ed1\u5207\u6362\u884c\u4e3a，\u800c\u4e0d\u662f\u7b80\u5355\u5730\u5728\u201c\u5de1\u903b\u201d\u548c\u201c\u8ddf\u968f\u201d\u4e4b\u95f4\u786c\u5207\u6362\u3002 \u4e3b\u8981\u7279\u70b9 . \u5de1\u903b\u8ddf\u968f\u53cc\u6a21\u5f0f\u534f\u540c \u7cfb\u7edf\u901a\u5e38\u5177\u5907\u81f3\u5c11","og_url":"https:\/\/www.wsisp.com\/helps\/111780.html","og_site_name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","article_published_time":"2026-10-01T12:41:05+00:00","og_image":[{"url":"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/10\/20261001124102-6abe54deb0cf1.gif"}],"author":"admin","twitter_card":"summary_large_image","twitter_misc":{"\u4f5c\u8005":"admin","\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4":"11 \u5206"},"schema":{"@context":"https:\/\/schema.org","@graph":[{"@type":"WebPage","@id":"https:\/\/www.wsisp.com\/helps\/111780.html","url":"https:\/\/www.wsisp.com\/helps\/111780.html","name":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","isPartOf":{"@id":"https:\/\/www.wsisp.com\/helps\/#website"},"datePublished":"2026-10-01T12:41:05+00:00","dateModified":"2026-10-01T12:41:05+00:00","author":{"@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41"},"breadcrumb":{"@id":"https:\/\/www.wsisp.com\/helps\/111780.html#breadcrumb"},"inLanguage":"zh-Hans","potentialAction":[{"@type":"ReadAction","target":["https:\/\/www.wsisp.com\/helps\/111780.html"]}]},{"@type":"BreadcrumbList","@id":"https:\/\/www.wsisp.com\/helps\/111780.html#breadcrumb","itemListElement":[{"@type":"ListItem","position":1,"name":"\u9996\u9875","item":"https:\/\/www.wsisp.com\/helps"},{"@type":"ListItem","position":2,"name":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u673a\u5668\u4eba\u56ed\u533a\u5de1\u903b\u8ddf\u968f+\u8def\u5f84\u4f18\u5316\u2014\u2014\u56ed\u533a\u5b89\u9632\u573a\u666f"}]},{"@type":"WebSite","@id":"https:\/\/www.wsisp.com\/helps\/#website","url":"https:\/\/www.wsisp.com\/helps\/","name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","description":"\u9999\u6e2f\u670d\u52a1\u5668_\u9999\u6e2f\u4e91\u670d\u52a1\u5668\u8d44\u8baf_\u670d\u52a1\u5668\u5e2e\u52a9\u6587\u6863_\u670d\u52a1\u5668\u6559\u7a0b","potentialAction":[{"@type":"SearchAction","target":{"@type":"EntryPoint","urlTemplate":"https:\/\/www.wsisp.com\/helps\/?s={search_term_string}"},"query-input":"required name=search_term_string"}],"inLanguage":"zh-Hans"},{"@type":"Person","@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41","name":"admin","image":{"@type":"ImageObject","inLanguage":"zh-Hans","@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/image\/","url":"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery","contentUrl":"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery","caption":"admin"},"sameAs":["http:\/\/wp.wsisp.com"],"url":"https:\/\/www.wsisp.com\/helps\/author\/admin"}]}},"_links":{"self":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/111780","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/users\/2"}],"replies":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/comments?post=111780"}],"version-history":[{"count":0,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/111780\/revisions"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media\/111776"}],"wp:attachment":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media?parent=111780"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/categories?post=111780"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/tags?post=111780"},{"taxonomy":"topic","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/topic?post=111780"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}