{"id":109892,"date":"2026-09-25T05:17:13","date_gmt":"2026-09-24T21:17:13","guid":{"rendered":"https:\/\/www.wsisp.com\/helps\/109892.html"},"modified":"2026-09-25T05:17:13","modified_gmt":"2026-09-24T21:17:13","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%e5%ae%b6%e5%ba%ad%e6%9c%8d%e5%8a%a1%e6%9c%ba%e5%99%a8%e4%ba%ba-%e7%ba%a2%e5%a4%96imu%e8%9e%8d","status":"publish","type":"post","link":"https:\/\/www.wsisp.com\/helps\/109892.html","title":{"rendered":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7"},"content":{"rendered":"

\"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"
\n\u201cArduino BLDC\u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7\u201d\u662f\u4e00\u4e2a\u5178\u578b\u7684\u4f4e\u6210\u672c\u3001\u9ad8\u96c6\u6210\u5ea6\u7684\u5d4c\u5165\u5f0f\u673a\u5668\u4eba\u7cfb\u7edf\u3002\u5b83\u5229\u7528Arduino\u4f5c\u4e3a\u6838\u5fc3\u63a7\u5236\u5668，\u7ed3\u5408\u65e0\u5237\u76f4\u6d41\u7535\u673a（BLDC）\u7684\u9ad8\u52a8\u6001\u6027\u80fd，\u901a\u8fc7\u7ea2\u5916\u4f20\u611f\u5668\u4e0e\u60ef\u6027\u6d4b\u91cf\u5355\u5143（IMU）\u7684\u6570\u636e\u878d\u5408，\u5b9e\u73b0\u8f7b\u91cf\u7ea7\u7684\u540c\u6b65\u5b9a\u4f4d\u4e0e\u5730\u56fe\u6784\u5efa（SLAM）\u4ee5\u53ca\u52a8\u6001\u907f\u969c\u529f\u80fd\u3002\u4ee5\u4e0b\u4ece\u4e3b\u8981\u7279\u70b9\u3001\u5e94\u7528\u573a\u666f\u53ca\u6ce8\u610f\u4e8b\u9879\u4e09\u4e2a\u65b9\u9762\u8fdb\u884c\u8be6\u7ec6\u89e3\u6790\u3002<\/p>\n

\u4e3b\u8981\u7279\u70b9<\/p>\n

  • \u9ad8\u52a8\u6001BLDC\u79fb\u52a8\u5e95\u76d8
    \n\u5e95\u76d8\u662f\u673a\u5668\u4eba\u7684\u201c\u53cc\u817f\u201d，\u5176\u6027\u80fd\u76f4\u63a5\u51b3\u5b9a\u4e86\u673a\u5668\u4eba\u7684\u673a\u52a8\u6027\u4e0e\u9002\u5e94\u6027\u3002
    \n\u5353\u8d8a\u7684\u52a8\u529b\u5b66\u6027\u80fd：\u91c7\u7528BLDC\u8f6e\u6bc2\u7535\u673a\u6216\u5173\u8282\u7535\u673a，\u76f8\u8f83\u4e8e\u6709\u5237\u7535\u673a，\u5177\u5907\u66f4\u9ad8\u7684\u529f\u7387\u5bc6\u5ea6\u3001\u66f4\u5feb\u7684\u52a8\u6001\u54cd\u5e94\u901f\u5ea6\u548c\u66f4\u957f\u7684\u4f7f\u7528\u5bff\u547d\u3002\u8fd9\u4f7f\u5f97\u673a\u5668\u4eba\u80fd\u591f\u5728\u62e5\u6324\u7684\u73af\u5883\u4e2d\u5feb\u901f\u542f\u52a8\u3001\u5236\u52a8\u548c\u7075\u6d3b\u8f6c\u5411，\u6709\u6548\u907f\u5f00\u7a81\u7136\u51fa\u73b0\u7684\u969c\u788d\u7269\u3002
    \n\u4f4e\u566a\u58f0\u4e0e\u9ad8\u6548\u7387：BLDC\u7535\u673a\u7684\u65e0\u5237\u7ed3\u6784\u548c\u6b63\u5f26\u6ce2\u9a71\u52a8（\u5982FOC\u63a7\u5236）\u663e\u8457\u964d\u4f4e\u4e86\u8fd0\u884c\u566a\u97f3\u548c\u8f6c\u77e9\u8109\u52a8\u3002\u8fd9\u5728\u533b\u9662\u3001\u9152\u5e97\u7b49\u9700\u8981\u4fdd\u6301\u5b89\u9759\u7684\u670d\u52a1\u73af\u5883\u4e2d\u81f3\u5173\u91cd\u8981，\u540c\u65f6\u9ad8\u6548\u7387\u4e5f\u5ef6\u957f\u4e86\u5355\u6b21\u5145\u7535\u7684\u7eed\u822a\u65f6\u95f4\u3002
    \n\u5168\u5411\u79fb\u52a8\u80fd\u529b：\u901a\u8fc7\u5dee\u901f\u9a71\u52a8\u6216\u9ea6\u514b\u7eb3\u59c6\u8f6e\/\u5168\u5411\u8f6e\u5e03\u5c40，\u914d\u5408BLDC\u7535\u673a\u7684\u7cbe\u786e\u63a7\u5236，\u673a\u5668\u4eba\u53ef\u5b9e\u73b0\u539f\u5730\u8f6c\u5411\u3001\u4fa7\u5411\u79fb\u52a8\u7b49\u5168\u5411\u8fd0\u52a8，\u6781\u5927\u589e\u5f3a\u4e86\u5728\u72ed\u7a84\u7a7a\u95f4\u5185\u7684\u901a\u8fc7\u6027\u548c\u5b9a\u4f4d\u7cbe\u5ea6\u3002<\/li>\n
  • \u7ea2\u5916\u4e0eIMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a
    \n\u8fd9\u662f\u673a\u5668\u4eba\u5b9e\u73b0\u201c\u667a\u80fd\u5316\u201d\u7684\u6838\u5fc3，\u4f7f\u5176\u80fd\u591f\u5728\u590d\u6742\u3001\u52a8\u6001\u7684\u73af\u5883\u4e2d\u201c\u770b\u6e05\u201d\u5e76\u201c\u7406\u89e3\u201d\u5468\u56f4\u4e16\u754c\u3002
    \n\u5f02\u6784\u4f20\u611f\u5668\u9635\u5217：\u7cfb\u7edf\u878d\u5408\u4e86\u591a\u79cd\u4f20\u611f\u5668\u7684\u4f18\u52bf，\u6784\u5efa\u4e86\u5bf9\u73af\u5883\u7684\u7acb\u4f53\u8ba4\u77e5\u3002\u4f8b\u5982，\u7ea2\u5916\u4f20\u611f\u5668\u4f5c\u4e3a\u8fd1\u8ddd\u79bb\u8865\u5145，\u7528\u4e8e\u68c0\u6d4b\u73bb\u7483\u3001\u955c\u9762\u7b49\u6fc0\u5149\u96f7\u8fbe\u96be\u4ee5\u8bc6\u522b\u7684\u7269\u4f53；IMU（\u60ef\u6027\u6d4b\u91cf\u5355\u5143）\u63d0\u4f9b\u9ad8\u9891\u7684\u59ff\u6001\u548c\u52a0\u901f\u5ea6\u6570\u636e，\u7528\u4e8e\u5728\u4f20\u611f\u5668\u6570\u636e\u66f4\u65b0\u95f4\u9699\u8fdb\u884c\u72b6\u6001\u4f30\u8ba1\u548c\u8fd0\u52a8\u8865\u507f\u3002
    \n\u5b9e\u65f6SLAM\u4e0e\u52a8\u6001\u907f\u969c：\u57fa\u4e8e\u878d\u5408\u540e\u7684\u4f20\u611f\u5668\u6570\u636e，\u673a\u5668\u4eba\u8fd0\u884cSLAM（\u5373\u65f6\u5b9a\u4f4d\u4e0e\u5730\u56fe\u6784\u5efa）\u7b97\u6cd5，\u5b9e\u65f6\u6784\u5efa\u5e76\u66f4\u65b0\u73af\u5883\u5730\u56fe，\u786e\u5b9a\u81ea\u8eab\u4f4d\u7f6e\u3002\u9488\u5bf9\u52a8\u6001\u969c\u788d\u7269，\u91c7\u7528\u5982DWA（\u52a8\u6001\u7a97\u53e3\u6cd5）\u6216TEB（\u65f6\u95f4\u5f39\u6027\u5e26）\u7b49\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u7b97\u6cd5，\u5b9e\u65f6\u9884\u6d4b\u969c\u788d\u7269\u8fd0\u52a8\u8d8b\u52bf\u5e76\u751f\u6210\u5e73\u6ed1\u3001\u5b89\u5168\u7684\u907f\u969c\u8f68\u8ff9\u3002<\/li>\n
  • \u5206\u5c42\u5f0f\u63a7\u5236\u67b6\u6784\u4e0e\u4efb\u52a1\u8c03\u5ea6
    \n\u4e3a\u4e86\u534f\u8c03\u590d\u6742\u7684\u786c\u4ef6\u548c\u7b97\u6cd5，\u7cfb\u7edf\u901a\u5e38\u91c7\u7528\u5206\u5c42\u5f0f\u63a7\u5236\u67b6\u6784，\u5b9e\u73b0\u8ba1\u7b97\u8d44\u6e90\u7684\u5408\u7406\u5206\u914d\u3002
    \n\u4e0a\u4f4d\u673a（\u51b3\u7b56\u5c42）：\u91c7\u7528\u9ad8\u6027\u80fd\u8ba1\u7b97\u5e73\u53f0（\u5982NVIDIA Jetson\u6216Raspberry Pi），\u8fd0\u884c\u673a\u5668\u4eba\u64cd\u4f5c\u7cfb\u7edf（ROS），\u8d1f\u8d23\u8fd0\u884cSLAM\u3001\u5168\u5c40\u8def\u5f84\u89c4\u5212\u3001\u4efb\u52a1\u8c03\u5ea6\u548c\u9ad8\u7ea7\u4eba\u673a\u4ea4\u4e92\u903b\u8f91\u3002
    \n\u4e0b\u4f4d\u673a（\u6267\u884c\u5c42 – Arduino）：\u4ee5\u9ad8\u6027\u80fdArduino（\u5982Teensy 4.0\/4.1\u6216Arduino Mega）\u4f5c\u4e3a\u5fae\u63a7\u5236\u5668，\u8d1f\u8d23\u5e95\u5c42\u7684\u5b9e\u65f6\u63a7\u5236\u4efb\u52a1\u3002\u5b83\u63a5\u6536\u6765\u81ea\u4e0a\u4f4d\u673a\u7684\u8fd0\u52a8\u6307\u4ee4（\u5982\u76ee\u6807\u901f\u5ea6\u3001\u8f6c\u5411\u89d2），\u901a\u8fc7PID\u6216\u66f4\u9ad8\u7ea7\u7684\u63a7\u5236\u7b97\u6cd5，\u7cbe\u786e\u63a7\u5236BLDC\u7535\u673a\u7684\u8f6c\u901f\u548c\u4f4d\u7f6e，\u540c\u65f6\u5b9e\u65f6\u91c7\u96c6\u7f16\u7801\u5668\u3001IMU\u548c\u5e95\u5c42\u4f20\u611f\u5668\u6570\u636e，\u8fdb\u884c\u6545\u969c\u68c0\u6d4b\u548c\u7d27\u6025\u5236\u52a8\u5904\u7406\u3002<\/li>\n

    \u5e94\u7528\u573a\u666f
    \n\u8be5\u7c7b\u673a\u5668\u4eba\u51ed\u501f\u5176\u5f3a\u5927\u7684\u73af\u5883\u9002\u5e94\u6027\u548c\u591a\u529f\u80fd\u6027，\u4e3b\u8981\u5e94\u7528\u4e8e\u4ee5\u4e0b\u52a8\u6001\u670d\u52a1\u9886\u57df：
    \n\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba：\u5728\u5ba2\u5385\u3001\u5367\u5ba4\u95f4\u81ea\u4e3b\u79fb\u52a8（\u5982\u9012\u9001\u7269\u54c1），\u901a\u8fc7\u7ea2\u5916+\u8d85\u58f0\u6ce2\u68c0\u6d4b\u5bb6\u5177\u3001\u95e8\u69db\u7b49\u969c\u788d，\u907f\u969c\u7cbe\u5ea6\u9700\u63a7\u5236\u5728\u00b15cm\u5185，\u907f\u514d\u78b0\u649e\u5bb6\u5c45\u3002
    \n\u5b9e\u9a8c\u5ba4\u6837\u54c1\u8fd0\u8f93：\u5728\u56fa\u5b9a\u5b9e\u9a8c\u53f0\u4e4b\u95f4\u8fd0\u8f93\u8bd5\u5242，\u6cbf\u9884\u8bbe\u8def\u7ebf\u884c\u9a76，\u68c0\u6d4b\u5230\u5b9e\u9a8c\u4eba\u5458\u6216\u8bbe\u5907\u65f6\u81ea\u52a8\u907f\u8ba9，\u786e\u4fdd\u6837\u54c1\u5b89\u5168\u3002
    \n\u4ed3\u50a8\u8d27\u67b6\u5de1\u68c0：\u5728\u4ed3\u5e93\u8d27\u67b6\u95f4（\u5bbd\u5ea61-1.5\u7c73）\u79fb\u52a8，\u68c0\u6d4b\u8d27\u67b6\u7acb\u67f1\u3001\u6258\u76d8\u7b49\u969c\u788d，\u901a\u8fc7\u8fd1\u8ddd\u79bb\u7ea2\u5916\u4f20\u611f\u5668\u786e\u4fdd\u4e0d\u5250\u8e6d\u8d27\u67b6\u3002
    \n\u56ed\u533a\u5de1\u68c0\u673a\u5668\u4eba：\u5728\u5de5\u5382\u56ed\u533a\u3001\u6821\u56ed\u5185\u5de1\u903b，\u68c0\u6d4b\u56f4\u680f\u3001\u8def\u706f\u3001\u884c\u4eba\u7b49\u969c\u788d，\u7ed3\u5408GPS\u8f85\u52a9\u5bfc\u822a，\u5b9e\u73b0\u5927\u8303\u56f4\u81ea\u4e3b\u79fb\u52a8\u3002
    \n\u519c\u4e1a\u6e29\u5ba4\u5de1\u68c0：\u5728\u6e29\u5ba4\u5927\u68da\u5185\u68c0\u6d4b\u4f5c\u7269\u957f\u52bf，\u907f\u5f00\u704c\u6e89\u7ba1\u9053\u3001\u80b2\u82d7\u67b6\u7b49\u969c\u788d，BLDC\u7535\u673a\u9700\u9002\u5e94\u571f\u58e4\u3001\u5730\u819c\u7b49\u590d\u6742\u5730\u9762（\u9632\u6253\u6ed1）\u3002<\/p>\n

    \u9700\u8981\u6ce8\u610f\u7684\u4e8b\u9879
    \n\u5728\u5b9e\u73b0\u8be5\u7cfb\u7edf\u7684\u81ea\u4e3b\u5bfc\u822a\u7cfb\u7edf\u65f6，\u9700\u8981\u5173\u6ce8\u4ee5\u4e0b\u4e8b\u9879：
    \n\u4f20\u611f\u5668\u9009\u62e9\u4e0e\u5e03\u5c40：\u9009\u62e9\u5408\u9002\u7684\u4f20\u611f\u5668，\u5e76\u5408\u7406\u5e03\u5c40\u4ee5\u786e\u4fdd\u83b7\u53d6\u51c6\u786e\u7684\u73af\u5883\u6570\u636e，\u907f\u514d\u76f2\u533a\u3002
    \n\u7b97\u6cd5\u590d\u6742\u5ea6：\u5728\u9009\u62e9\u5bfc\u822a\u7b97\u6cd5\u65f6，\u8003\u8651\u5176\u8ba1\u7b97\u590d\u6742\u5ea6\u548c\u5b9e\u65f6\u6027\u80fd，\u786e\u4fdd\u7cfb\u7edf\u80fd\u591f\u5feb\u901f\u54cd\u5e94\u52a8\u6001\u73af\u5883\u53d8\u5316\u3002
    \n\u6570\u636e\u5904\u7406\u4e0e\u878d\u5408：\u91c7\u7528\u6709\u6548\u7684\u6570\u636e\u5904\u7406\u548c\u878d\u5408\u65b9\u6cd5，\u51cf\u5c11\u4f20\u611f\u5668\u566a\u58f0\u548c\u8bef\u5dee\u5bf9\u5bfc\u822a\u7cbe\u5ea6\u7684\u5f71\u54cd\u3002
    \n\u7535\u6e90\u7ba1\u7406：\u786e\u4fdd\u7cfb\u7edf\u5177\u5907\u826f\u597d\u7684\u7535\u6e90\u7ba1\u7406，\u9632\u6b62\u7535\u6c60\u4e0d\u8db3\u5bfc\u81f4\u7684\u5bfc\u822a\u4e2d\u65ad，\u63d0\u5347\u5c0f\u8f66\u7684\u7eed\u822a\u80fd\u529b\u3002
    \n\u8c03\u8bd5\u4e0e\u6d4b\u8bd5：\u5728\u4e0d\u540c\u73af\u5883\u4e0b\u8fdb\u884c\u5145\u5206\u7684\u8c03\u8bd5\u4e0e\u6d4b\u8bd5，\u786e\u4fdd\u7cfb\u7edf\u5728\u5b9e\u9645\u5e94\u7528\u4e2d\u7684\u7a33\u5b9a\u6027\u548c\u53ef\u9760\u6027\u3002
    \n\u5b89\u5168\u673a\u5236\u8bbe\u8ba1：\u8bbe\u8ba1\u5fc5\u8981\u7684\u5b89\u5168\u673a\u5236，\u5982\u78b0\u649e\u68c0\u6d4b\u3001\u6025\u505c\u529f\u80fd\u7b49，\u4ee5\u786e\u4fdd\u5c0f\u8f66\u5728\u5f02\u5e38\u60c5\u51b5\u4e0b\u80fd\u591f\u5b89\u5168\u505c\u673a，\u4fdd\u62a4\u4eba\u5458\u548c\u8bbe\u5907\u5b89\u5168\u3002
    \n\u4e25\u683c\u7684\u7535\u6e90\u7ba1\u7406\u4e0e\u7535\u78c1\u517c\u5bb9（EMC）：BLDC\u7535\u673a\u5728\u542f\u505c\u548c\u5dee\u901f\u8f6c\u5411\u65f6\u4f1a\u4ea7\u751f\u6781\u5927\u7684\u7535\u6d41\u51b2\u51fb\u548c\u9ad8\u9891\u7535\u78c1\u566a\u58f0，\u6781\u6613\u5bfc\u81f4Arduino\u4e3b\u63a7\u590d\u4f4d\u6216IMU\u6570\u636e\u5931\u771f\u3002\u5fc5\u987b\u91c7\u7528\u9694\u79bbDC-DC\u6a21\u5757\u4e3a\u63a7\u5236\u7535\u8def\u72ec\u7acb\u4f9b\u7535，\u5e76\u5728\u7535\u673a\u9a71\u52a8\u7aef\u5e76\u8054\u5927\u5bb9\u91cf\u4f4eESR\u7535\u5bb9\u5438\u6536\u53cd\u7535\u52a8\u52bf\u3002IMU\u4e0e\u89c6\u89c9\u6a21\u5757\u7684\u4fe1\u53f7\u7ebf\u9700\u4f7f\u7528\u5c4f\u853d\u7ebf，\u5e76\u8fdc\u79bb\u52a8\u529b\u7ebf\u5e03\u7ebf\u3002
    \n\u4e3b\u63a7\u7b97\u529b\u5206\u914d\u4e0e\u5b9e\u65f6\u6027\u4fdd\u969c：\u89c6\u89c9\u7279\u5f81\u89e3\u7b97\u3001IMU\u59ff\u6001\u878d\u5408\u4e0e\u591a\u8defBLDC\u7684FOC\u63a7\u5236\u5bf9\u7b97\u529b\u8981\u6c42\u6781\u9ad8\u3002\u6807\u51c6Arduino Uno\u96be\u4ee5\u80dc\u4efb，\u5efa\u8bae\u91c7\u7528ESP32\u3001STM32\u6216\u6811\u8393\u6d3e+Arduino\u7684\u5f02\u6784\u67b6\u6784\u3002\u63a7\u5236\u56de\u8def\u5fc5\u987b\u4f7f\u7528\u786c\u4ef6\u5b9a\u65f6\u5668\u4e2d\u65ad\u6216millis()\u975e\u963b\u585e\u5b9a\u65f6（\u5efa\u8bae\u63a7\u5236\u9891\u7387\u226550Hz），\u4e25\u7981\u4f7f\u7528delay()\u51fd\u6570，\u4ee5\u786e\u4fdd\u89c6\u89c9\u6570\u636e\u4e0e\u7535\u673a\u63a7\u5236\u7684\u4e25\u683c\u540c\u6b65\u3002
    \nIMU\u5b89\u88c5\u4e0e\u89c6\u89c9\u6297\u5e72\u6270\u8bbe\u8ba1：IMU\u5fc5\u987b\u521a\u6027\u56fa\u5b9a\u5728\u5e95\u76d8\u91cd\u5fc3\u9644\u8fd1，\u5e76\u52a0\u7845\u80f6\u51cf\u9707\u57ab\u4ee5\u9694\u79bb\u7535\u673a\u9ad8\u9891\u632f\u52a8\u3002\u5728\u89c6\u89c9\u8ddf\u968f\u7b97\u6cd5\u4e2d，\u9700\u8bbe\u7f6e\u5408\u7406\u7684\u7f6e\u4fe1\u5ea6\u9608\u503c，\u8fc7\u6ee4\u4f4e\u7f6e\u4fe1\u5ea6\u7684\u76ee\u6807\u68c0\u6d4b\u7ed3\u679c，\u907f\u514d\u8bef\u5c06\u5899\u58c1\u6216\u80cc\u666f\u8bc6\u522b\u4e3a\u8ddf\u968f\u76ee\u6807\u3002\u540c\u65f6，\u89c6\u89c9\u4f20\u611f\u5668\u7684\u5b89\u88c5\u4f4d\u7f6e\u9700\u907f\u514d\u88ab\u673a\u5668\u4eba\u81ea\u8eab\u7ed3\u6784\u906e\u6321，\u786e\u4fdd\u8db3\u591f\u7684\u89c6\u573a\u89d2（FOV）\u3002
    \nPID\u53c2\u6570\u6574\u5b9a\u4e0e\u5b89\u5168\u5197\u4f59\u673a\u5236：\u8ddf\u968f\u63a7\u5236\u4e2d\u7684\u8ddd\u79bbPID\u4e0e\u8f6c\u5411PID\u53c2\u6570\u9700\u6839\u636e\u5b9e\u9645\u673a\u68b0\u7ed3\u6784\u8fdb\u884c\u7cbe\u7ec6\u6807\u5b9a，\u53c2\u6570\u8fc7\u5927\u4f1a\u5bfc\u81f4\u8ddf\u968f\u6296\u52a8，\u8fc7\u5c0f\u5219\u54cd\u5e94\u8fdf\u7f13\u3002\u6b64\u5916，\u5fc5\u987b\u8bbe\u7f6e\u8f6f\u4ef6\u7ea7\u9650\u901f\u4e0e\u503e\u89d2\u8d85\u9650\u4fdd\u62a4，\u5e76\u914d\u5907\u7269\u7406\u6025\u505c\u6309\u94ae\u6216\u9632\u649e\u6761\u4f5c\u4e3a\u6700\u540e\u7684\u5b89\u5168\u9632\u7ebf，\u786e\u4fdd\u5728\u7cfb\u7edf\u5931\u63a7\u65f6\u80fd\u77ac\u95f4\u5207\u65ad\u7535\u673a\u52a8\u529b\u3002<\/p>\n

    \"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"<\/p>\n

    1\u3001\u7ea2\u5916+IMU\u4e92\u8865\u6ee4\u6ce2\u59ff\u6001\u878d\u5408\u4e0e\u8f7b\u91cf\u6805\u683c\u5efa\u56fe
    \n\u573a\u666f：\u5bb6\u5ead\u673a\u5668\u4eba\u4ee5\u4f4e\u901f\u5de1\u822a，\u7ea2\u5916\u4f20\u611f\u5668\u9635\u5217\u626b\u63cf\u5468\u56f4\u73af\u5883，IMU\u63d0\u4f9b\u822a\u5411\u89d2\u8865\u507f\u91cc\u7a0b\u8ba1\u6f02\u79fb，\u6784\u5efa\u7b80\u5355\u7684\u5360\u636e\u6805\u683c\u5730\u56fe\u7528\u4e8e\u8def\u5f84\u89c4\u5212\u3002<\/p>\n

    \/* ===== \u7ea2\u5916+IMU\u4e92\u8865\u6ee4\u6ce2 + \u8f7b\u91cf\u6805\u683c\u5efa\u56fe =====
    \n * \u786c\u4ef6：Arduino Mega\/ESP32 + SimpleFOC BLDC + MPU6050 + \u7ea2\u5916\u9635\u5217 + \u7f16\u7801\u5668
    \n * \u6838\u5fc3：\u4e92\u8865\u6ee4\u6ce2\u878d\u5408\u59ff\u6001 + \u7ea2\u5916\u626b\u63cf\u66f4\u65b0\u6805\u683c
    \n *\/<\/span>
    \n#<\/span>include<\/span> <SimpleFOC.h><\/span><\/span>
    \n#<\/span>include<\/span> <Wire.h><\/span><\/span>
    \n#<\/span>include<\/span> <MPU6050.h><\/span><\/span><\/p>\n

    \/\/ — BLDC \u5dee\u901f\u5e95\u76d8 —<\/span>
    \nBLDCMotor motorL<\/span>(<\/span>7<\/span>)<\/span>,<\/span> motorR<\/span>(<\/span>7<\/span>)<\/span>;<\/span>
    \nBLDCDriver3PWM driverL<\/span>(<\/span>9<\/span>,<\/span> 10<\/span>,<\/span> 11<\/span>,<\/span> 8<\/span>)<\/span>;<\/span>
    \nBLDCDriver3PWM driverR<\/span>(<\/span>3<\/span>,<\/span> 5<\/span>,<\/span> 6<\/span>,<\/span> 7<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ — \u7ea2\u5916\u4f20\u611f\u5668\u9635\u5217（\u524d\u3001\u5de6\u3001\u53f3\u3001\u5e95\u90e8\u60ac\u5d16）—<\/span>
    \nconst<\/span> int<\/span> IR_FRONT =<\/span> A0;<\/span>
    \nconst<\/span> int<\/span> IR_LEFT =<\/span> A1;<\/span>
    \nconst<\/span> int<\/span> IR_RIGHT =<\/span> A2;<\/span>
    \nconst<\/span> int<\/span> IR_BOTTOM =<\/span> A3;<\/span> \/\/ \u60ac\u5d16\u68c0\u6d4b<\/span><\/p>\n

    MPU6050 imu;<\/span><\/p>\n

    \/\/ — \u7f16\u7801\u5668（\u4e2d\u65ad\u8bfb\u53d6）—<\/span>
    \nvolatile<\/span> long<\/span> encL =<\/span> 0<\/span>,<\/span> encR =<\/span> 0<\/span>;<\/span>
    \nconst<\/span> int<\/span> ENC_L_PIN =<\/span> 2<\/span>,<\/span> ENC_R_PIN =<\/span> 3<\/span>;<\/span>
    \nconst<\/span> float<\/span> WHEEL_RADIUS =<\/span> 0.05<\/span>;<\/span>
    \nconst<\/span> float<\/span> WHEEL_BASE =<\/span> 0.30<\/span>;<\/span>
    \nconst<\/span> int<\/span> PULSES_PER_REV =<\/span> 500<\/span>;<\/span><\/p>\n

    \/\/ — \u4f4d\u59ff\u72b6\u6001 —<\/span>
    \nfloat<\/span> posX =<\/span> 0<\/span>,<\/span> posY =<\/span> 0<\/span>;<\/span>
    \nfloat<\/span> yaw =<\/span> 0<\/span>;<\/span>
    \nfloat<\/span> yawIMU =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ — \u4e92\u8865\u6ee4\u6ce2\u53c2\u6570 —<\/span>
    \nconst<\/span> float<\/span> ALPHA =<\/span> 0.98<\/span>;<\/span> \/\/ \u9640\u87ba\u4eea\u6743\u91cd<\/span><\/p>\n

    \/\/ — \u6805\u683c\u5730\u56fe —<\/span>
    \n#<\/span>define<\/span> GRID_SIZE<\/span> 30<\/span><\/span><\/span>
    \n#<\/span>define<\/span> CELL_SIZE<\/span> 0.20<\/span> <\/span>\/\/ 20cm\/\u683c<\/span><\/span>
    \nbyte grid[<\/span>GRID_SIZE]<\/span>[<\/span>GRID_SIZE]<\/span>;<\/span> \/\/ 0=\u672a\u77e5, 1=\u7a7a\u95f2, 2=\u969c\u788d<\/span>
    \nint<\/span> robotGridX =<\/span> 15<\/span>,<\/span> robotGridY =<\/span> 15<\/span>;<\/span><\/p>\n

    void<\/span> IRAM_ATTR encLISR<\/span>(<\/span>)<\/span> {<\/span> encL++<\/span>;<\/span> }<\/span>
    \nvoid<\/span> IRAM_ATTR encRISR<\/span>(<\/span>)<\/span> {<\/span> encR++<\/span>;<\/span> }<\/span><\/p>\n

    \/\/ \u8bfb\u53d6\u7ea2\u5916\u8ddd\u79bb（\u7b80\u5316\u6620\u5c04）<\/span>
    \nfloat<\/span> readIRDistance<\/span>(<\/span>int<\/span> pin)<\/span> {<\/span>
    \n int<\/span> val =<\/span> analogRead<\/span>(<\/span>pin)<\/span>;<\/span>
    \n \/\/ \u7ea2\u5916\u8ddd\u79bb\u4e0e\u6a21\u62df\u503c\u8fd1\u4f3c\u53cd\u6bd4<\/span>
    \n float<\/span> dist =<\/span> 1000.0<\/span> \/<\/span> (<\/span>val +<\/span> 1<\/span>)<\/span>;<\/span> \/\/ cm<\/span>
    \n if<\/span> (<\/span>dist ><\/span> 80<\/span>)<\/span> dist =<\/span> 80<\/span>;<\/span>
    \n if<\/span> (<\/span>dist <<\/span> 5<\/span>)<\/span> dist =<\/span> 5<\/span>;<\/span>
    \n return<\/span> dist \/<\/span> 100.0<\/span>;<\/span> \/\/ \u8f6c\u7c73<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u4e92\u8865\u6ee4\u6ce2\u66f4\u65b0\u822a\u5411\u89d2<\/span>
    \nvoid<\/span> updateYaw<\/span>(<\/span>float<\/span> dt)<\/span> {<\/span>
    \n int16_t<\/span> gz;<\/span>
    \n imu.<\/span>getRotation<\/span>(<\/span>nullptr<\/span>,<\/span> nullptr<\/span>,<\/span> &<\/span>gz)<\/span>;<\/span>
    \n float<\/span> gyroRate =<\/span> gz \/<\/span> 131.0<\/span>;<\/span> \/\/ \u5ea6\/\u79d2<\/span><\/p>\n

    \/\/ \u9640\u87ba\u4eea\u79ef\u5206<\/span>
    \n yawIMU +=<\/span> gyroRate *<\/span> dt;<\/span><\/p>\n

    \/\/ \u4e92\u8865\u6ee4\u6ce2：\u9640\u87ba\u4eea\u77ed\u671f\u79ef\u5206 + \u7f16\u7801\u5668\u957f\u671f\u822a\u5411<\/span>
    \n float<\/span> encYaw =<\/span> yaw;<\/span> \/\/ \u7f16\u7801\u5668\u63a8\u7b97\u7684\u822a\u5411<\/span>
    \n yaw =<\/span> ALPHA *<\/span> (<\/span>yaw +<\/span> gyroRate *<\/span> dt)<\/span> +<\/span> (<\/span>1<\/span> –<\/span> ALPHA)<\/span> *<\/span> yawIMU;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7f16\u7801\u5668\u91cc\u7a0b\u8ba1\u66f4\u65b0<\/span>
    \nvoid<\/span> updateOdometry<\/span>(<\/span>)<\/span> {<\/span>
    \n static<\/span> long<\/span> lastEncL =<\/span> 0<\/span>,<\/span> lastEncR =<\/span> 0<\/span>;<\/span>
    \n long<\/span> dL =<\/span> encL –<\/span> lastEncL;<\/span>
    \n long<\/span> dR =<\/span> encR –<\/span> lastEncR;<\/span>
    \n lastEncL =<\/span> encL;<\/span>
    \n lastEncR =<\/span> encR;<\/span><\/p>\n

    float<\/span> distL =<\/span> dL *<\/span> 2<\/span> *<\/span> PI *<\/span> WHEEL_RADIUS \/<\/span> PULSES_PER_REV;<\/span>
    \n float<\/span> distR =<\/span> dR *<\/span> 2<\/span> *<\/span> PI *<\/span> WHEEL_RADIUS \/<\/span> PULSES_PER_REV;<\/span><\/p>\n

    float<\/span> dCenter =<\/span> (<\/span>distL +<\/span> distR)<\/span> \/<\/span> 2.0<\/span>;<\/span>
    \n float<\/span> dYaw =<\/span> (<\/span>distR –<\/span> distL)<\/span> \/<\/span> WHEEL_BASE;<\/span><\/p>\n

    yaw +=<\/span> dYaw;<\/span>
    \n posX +=<\/span> dCenter *<\/span> cos<\/span>(<\/span>yaw)<\/span>;<\/span>
    \n posY +=<\/span> dCenter *<\/span> sin<\/span>(<\/span>yaw)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7ea2\u5916\u626b\u63cf\u66f4\u65b0\u6805\u683c\u5730\u56fe<\/span>
    \nvoid<\/span> updateGrid<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> dFront =<\/span> readIRDistance<\/span>(<\/span>IR_FRONT)<\/span>;<\/span>
    \n float<\/span> dLeft =<\/span> readIRDistance<\/span>(<\/span>IR_LEFT)<\/span>;<\/span>
    \n float<\/span> dRight =<\/span> readIRDistance<\/span>(<\/span>IR_RIGHT)<\/span>;<\/span><\/p>\n

    \/\/ \u673a\u5668\u4eba\u6240\u5728\u6805\u683c<\/span>
    \n int<\/span> rx =<\/span> (<\/span>int<\/span>)<\/span>(<\/span>posX \/<\/span> CELL_SIZE)<\/span> +<\/span> 15<\/span>;<\/span>
    \n int<\/span> ry =<\/span> (<\/span>int<\/span>)<\/span>(<\/span>posY \/<\/span> CELL_SIZE)<\/span> +<\/span> 15<\/span>;<\/span><\/p>\n

    \/\/ \u524d\u65b9\u626b\u63cf<\/span>
    \n int<\/span> cellsF =<\/span> (<\/span>int<\/span>)<\/span>(<\/span>dFront \/<\/span> CELL_SIZE)<\/span>;<\/span>
    \n for<\/span> (<\/span>int<\/span> i =<\/span> 1<\/span>;<\/span> i <=<\/span> cellsF;<\/span> i++<\/span>)<\/span> {<\/span>
    \n int<\/span> gx =<\/span> rx +<\/span> (<\/span>int<\/span>)<\/span>(<\/span>i *<\/span> cos<\/span>(<\/span>yaw)<\/span>)<\/span>;<\/span>
    \n int<\/span> gy =<\/span> ry +<\/span> (<\/span>int<\/span>)<\/span>(<\/span>i *<\/span> sin<\/span>(<\/span>yaw)<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>gx >=<\/span> 0<\/span> &&<\/span> gx <<\/span> GRID_SIZE &&<\/span> gy >=<\/span> 0<\/span> &&<\/span> gy <<\/span> GRID_SIZE)<\/span> {<\/span>
    \n grid[<\/span>gx]<\/span>[<\/span>gy]<\/span> =<\/span> (<\/span>i ==<\/span> cellsF)<\/span> ?<\/span> 2<\/span> :<\/span> 1<\/span>;<\/span> \/\/ \u672b\u7aef\u4e3a\u969c\u788d，\u4e2d\u95f4\u4e3a\u7a7a\u95f2<\/span>
    \n }<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u5de6\u4fa7\u626b\u63cf<\/span>
    \n int<\/span> cellsL =<\/span> (<\/span>int<\/span>)<\/span>(<\/span>dLeft \/<\/span> CELL_SIZE)<\/span>;<\/span>
    \n float<\/span> leftAngle =<\/span> yaw +<\/span> PI \/<\/span> 2<\/span>;<\/span>
    \n for<\/span> (<\/span>int<\/span> i =<\/span> 1<\/span>;<\/span> i <=<\/span> cellsL;<\/span> i++<\/span>)<\/span> {<\/span>
    \n int<\/span> gx =<\/span> rx +<\/span> (<\/span>int<\/span>)<\/span>(<\/span>i *<\/span> cos<\/span>(<\/span>leftAngle)<\/span>)<\/span>;<\/span>
    \n int<\/span> gy =<\/span> ry +<\/span> (<\/span>int<\/span>)<\/span>(<\/span>i *<\/span> sin<\/span>(<\/span>leftAngle)<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>gx >=<\/span> 0<\/span> &&<\/span> gx <<\/span> GRID_SIZE &&<\/span> gy >=<\/span> 0<\/span> &&<\/span> gy <<\/span> GRID_SIZE)<\/span> {<\/span>
    \n grid[<\/span>gx]<\/span>[<\/span>gy]<\/span> =<\/span> (<\/span>i ==<\/span> cellsL)<\/span> ?<\/span> 2<\/span> :<\/span> 1<\/span>;<\/span>
    \n }<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u53f3\u4fa7\u626b\u63cf<\/span>
    \n int<\/span> cellsR =<\/span> (<\/span>int<\/span>)<\/span>(<\/span>dRight \/<\/span> CELL_SIZE)<\/span>;<\/span>
    \n float<\/span> rightAngle =<\/span> yaw –<\/span> PI \/<\/span> 2<\/span>;<\/span>
    \n for<\/span> (<\/span>int<\/span> i =<\/span> 1<\/span>;<\/span> i <=<\/span> cellsR;<\/span> i++<\/span>)<\/span> {<\/span>
    \n int<\/span> gx =<\/span> rx +<\/span> (<\/span>int<\/span>)<\/span>(<\/span>i *<\/span> cos<\/span>(<\/span>rightAngle)<\/span>)<\/span>;<\/span>
    \n int<\/span> gy =<\/span> ry +<\/span> (<\/span>int<\/span>)<\/span>(<\/span>i *<\/span> sin<\/span>(<\/span>rightAngle)<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>gx >=<\/span> 0<\/span> &&<\/span> gx <<\/span> GRID_SIZE &&<\/span> gy >=<\/span> 0<\/span> &&<\/span> gy <<\/span> GRID_SIZE)<\/span> {<\/span>
    \n grid[<\/span>gx]<\/span>[<\/span>gy]<\/span> =<\/span> (<\/span>i ==<\/span> cellsR)<\/span> ?<\/span> 2<\/span> :<\/span> 1<\/span>;<\/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 Wire.<\/span>begin<\/span>(<\/span>)<\/span>;<\/span>
    \n imu.<\/span>initialize<\/span>(<\/span>)<\/span>;<\/span>
    \n imu.<\/span>setFullScaleGyroRange<\/span>(<\/span>MPU6050_GYRO_FS_250)<\/span>;<\/span><\/p>\n

    pinMode<\/span>(<\/span>ENC_L_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>ENC_R_PIN,<\/span> INPUT_PULLUP)<\/span>;<\/span>
    \n attachInterrupt<\/span>(<\/span>digitalPinToInterrupt<\/span>(<\/span>ENC_L_PIN)<\/span>,<\/span> encLISR,<\/span> RISING)<\/span>;<\/span>
    \n attachInterrupt<\/span>(<\/span>digitalPinToInterrupt<\/span>(<\/span>ENC_R_PIN)<\/span>,<\/span> encRISR,<\/span> RISING)<\/span>;<\/span><\/p>\n

    for<\/span> (<\/span>int<\/span> x =<\/span> 0<\/span>;<\/span> x <<\/span> GRID_SIZE;<\/span> x++<\/span>)<\/span>
    \n for<\/span> (<\/span>int<\/span> y =<\/span> 0<\/span>;<\/span> y <<\/span> GRID_SIZE;<\/span> y++<\/span>)<\/span>
    \n grid[<\/span>x]<\/span>[<\/span>y]<\/span> =<\/span> 0<\/span>;<\/span><\/p>\n

    driverL.<\/span>voltage_power_supply =<\/span> 24<\/span>;<\/span> driverL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>linkDriver<\/span>(<\/span>&<\/span>driverL)<\/span>;<\/span> motorL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorL.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span><\/p>\n

    driverR.<\/span>voltage_power_supply =<\/span> 24<\/span>;<\/span> driverR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>linkDriver<\/span>(<\/span>&<\/span>driverR)<\/span>;<\/span> motorR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/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

    static<\/span> unsigned<\/span> long<\/span> lastTime =<\/span> 0<\/span>;<\/span>
    \n unsigned<\/span> long<\/span> now =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
    \n float<\/span> dt =<\/span> (<\/span>now –<\/span> lastTime)<\/span> \/<\/span> 1000.0<\/span>;<\/span>
    \n lastTime =<\/span> now;<\/span>
    \n if<\/span> (<\/span>dt ><\/span> 0.5<\/span>)<\/span> dt =<\/span> 0.05<\/span>;<\/span><\/p>\n

    updateOdometry<\/span>(<\/span>)<\/span>;<\/span>
    \n updateYaw<\/span>(<\/span>dt)<\/span>;<\/span>
    \n updateGrid<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u7b80\u5355\u907f\u969c：\u524d\u65b9\u7ea2\u5916\u68c0\u6d4b\u5230\u969c\u788d\u5219\u8f6c\u5411<\/span>
    \n float<\/span> dFront =<\/span> readIRDistance<\/span>(<\/span>IR_FRONT)<\/span>;<\/span>
    \n float<\/span> v =<\/span> 0.25<\/span>,<\/span> w =<\/span> 0<\/span>;<\/span><\/p>\n

    if<\/span> (<\/span>dFront <<\/span> 0.25<\/span>)<\/span> {<\/span>
    \n v =<\/span> 0.1<\/span>;<\/span>
    \n w =<\/span> (<\/span>readIRDistance<\/span>(<\/span>IR_LEFT)<\/span> ><\/span> readIRDistance<\/span>(<\/span>IR_RIGHT)<\/span>)<\/span> ?<\/span> 0.5<\/span> :<\/span> –<\/span>0.5<\/span>;<\/span>
    \n }<\/span><\/p>\n

    motorL.<\/span>move<\/span>(<\/span>v –<\/span> w *<\/span> WHEEL_BASE \/<\/span> 2<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>move<\/span>(<\/span>v +<\/span> w *<\/span> WHEEL_BASE \/<\/span> 2<\/span>)<\/span>;<\/span><\/p>\n

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

    \u6838\u5fc3\u8981\u70b9：\u4e92\u8865\u6ee4\u6ce2\u7684\u6838\u5fc3\u662f\u201c\u9640\u87ba\u4eea\u77ed\u671f\u7cbe\u786e\u3001\u7f16\u7801\u5668\u957f\u671f\u7a33\u5b9a\u201d\u3002\u9640\u87ba\u4eea\u79ef\u5206\u5728\u51e0\u79d2\u5185\u975e\u5e38\u51c6\u786e，\u4f46\u4f1a\u7d2f\u79ef\u6f02\u79fb；\u7f16\u7801\u5668\u822a\u5411\u5728\u8f6c\u5f2f\u65f6\u53ef\u9760，\u4f46\u76f4\u7ebf\u884c\u9a76\u65f6\u56e0\u8f6e\u5b50\u6253\u6ed1\u4f1a\u9010\u6e10\u504f\u822a\u3002ALPHA = 0.98 \u610f\u5473\u7740 98% \u4fe1\u4efb\u9640\u87ba\u4eea\u77ed\u671f\u53d8\u5316，2% \u7528\u957f\u671f\u57fa\u51c6\u4fee\u6b63\u6f02\u79fb\u3002\u6805\u683c\u5730\u56fe\u7684\u589e\u91cf\u66f4\u65b0\u53ea\u6807\u8bb0\u7ea2\u5916\u626b\u63cf\u5230\u7684\u6805\u683c，\u4e0d\u8fdb\u884c\u5168\u56fe\u91cd\u7b97，\u9002\u5408 Arduino \u6709\u9650\u7b97\u529b\u3002<\/p>\n

    2\u3001\u4e09\u5c42\u5b89\u5168\u76d1\u63a7\u4e0e\u5206\u7ea7\u54cd\u5e94
    \n\u573a\u666f：\u5bb6\u5ead\u73af\u5883\u4e2d\u673a\u5668\u4eba\u9700\u8981\u201c\u8fdc-\u4e2d-\u8fd1\u201d\u4e09\u5c42\u9632\u62a4：SLAM \u5168\u5c40\u8def\u5f84\u907f\u5f00\u5df2\u77e5\u969c\u788d，\u7ea2\u5916\u68c0\u6d4b\u4e2d\u8ddd\u52a8\u6001\u969c\u788d，\u5e95\u90e8\u7ea2\u5916\u68c0\u6d4b\u60ac\u5d16\u9632\u6b62\u8dcc\u843d，\u6bcf\u5c42\u89e6\u53d1\u4e0d\u540c\u54cd\u5e94\u7ea7\u522b\u3002<\/p>\n

    \/* ===== \u4e09\u5c42\u5b89\u5168\u76d1\u63a7 + \u5206\u7ea7\u54cd\u5e94 =====
    \n * \u786c\u4ef6：Arduino + SimpleFOC BLDC + \u7ea2\u5916\u9635\u5217（\u524d\/\u5e95） + IMU
    \n * \u6838\u5fc3：\u8fdc(SLAM) \u2192 \u4e2d(\u7ea2\u5916) \u2192 \u8fd1(\u60ac\u5d16) \u4e09\u7ea7\u5b89\u5168\u94fe
    \n *\/<\/span>
    \n#<\/span>include<\/span> <SimpleFOC.h><\/span><\/span>
    \n#<\/span>include<\/span> <Wire.h><\/span><\/span>
    \n#<\/span>include<\/span> <MPU6050.h><\/span><\/span><\/p>\n

    BLDCMotor motorL<\/span>(<\/span>7<\/span>)<\/span>,<\/span> motorR<\/span>(<\/span>7<\/span>)<\/span>;<\/span>
    \nBLDCDriver3PWM driverL<\/span>(<\/span>9<\/span>,<\/span> 10<\/span>,<\/span> 11<\/span>,<\/span> 8<\/span>)<\/span>;<\/span>
    \nBLDCDriver3PWM driverR<\/span>(<\/span>3<\/span>,<\/span> 5<\/span>,<\/span> 6<\/span>,<\/span> 7<\/span>)<\/span>;<\/span><\/p>\n

    MPU6050 imu;<\/span><\/p>\n

    \/\/ — \u7ea2\u5916\u4f20\u611f\u5668 —<\/span>
    \nconst<\/span> int<\/span> IR_FRONT =<\/span> A0;<\/span> \/\/ \u524d\u65b9\u4e2d\u8ddd<\/span>
    \nconst<\/span> int<\/span> IR_LEFT =<\/span> A1;<\/span> \/\/ \u5de6\u4fa7<\/span>
    \nconst<\/span> int<\/span> IR_RIGHT =<\/span> A2;<\/span> \/\/ \u53f3\u4fa7<\/span>
    \nconst<\/span> int<\/span> IR_BOTTOM_L =<\/span> A3;<\/span> \/\/ \u5e95\u90e8\u60ac\u5d16\u5de6<\/span>
    \nconst<\/span> int<\/span> IR_BOTTOM_R =<\/span> A4;<\/span> \/\/ \u5e95\u90e8\u60ac\u5d16\u53f3<\/span><\/p>\n

    \/\/ — \u5b89\u5168\u7b49\u7ea7 —<\/span>
    \nenum<\/span> SafetyLevel<\/span> {<\/span> LEVEL_NORMAL,<\/span> LEVEL_CAUTION,<\/span> LEVEL_AVOID,<\/span> LEVEL_EMERGENCY }<\/span>;<\/span>
    \nSafetyLevel safetyLevel =<\/span> LEVEL_NORMAL;<\/span><\/p>\n

    \/\/ — \u53c2\u6570 —<\/span>
    \nconst<\/span> float<\/span> SAFE_FRONT =<\/span> 0.30<\/span>;<\/span> \/\/ \u524d\u65b9\u5b89\u5168\u8ddd\u79bb (m)<\/span>
    \nconst<\/span> float<\/span> CAUTION_FRONT =<\/span> 0.50<\/span>;<\/span> \/\/ \u524d\u65b9\u8b66\u6212\u8ddd\u79bb<\/span>
    \nconst<\/span> float<\/span> CLIFF_THRESHOLD =<\/span> 0.15<\/span>;<\/span> \/\/ \u60ac\u5d16\u68c0\u6d4b\u9608\u503c (m)<\/span><\/p>\n

    float<\/span> yaw =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ \u8bfb\u53d6\u7ea2\u5916\u8ddd\u79bb<\/span>
    \nfloat<\/span> readIR<\/span>(<\/span>int<\/span> pin)<\/span> {<\/span>
    \n int<\/span> val =<\/span> analogRead<\/span>(<\/span>pin)<\/span>;<\/span>
    \n float<\/span> dist =<\/span> 1000.0<\/span> \/<\/span> (<\/span>val +<\/span> 1<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>dist ><\/span> 80<\/span>)<\/span> dist =<\/span> 80<\/span>;<\/span>
    \n if<\/span> (<\/span>dist <<\/span> 3<\/span>)<\/span> dist =<\/span> 3<\/span>;<\/span>
    \n return<\/span> dist \/<\/span> 100.0<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u4e09\u5c42\u5b89\u5168\u8bc4\u4f30<\/span>
    \nvoid<\/span> evaluateSafety<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> dFront =<\/span> readIR<\/span>(<\/span>IR_FRONT)<\/span>;<\/span>
    \n float<\/span> dLeft =<\/span> readIR<\/span>(<\/span>IR_LEFT)<\/span>;<\/span>
    \n float<\/span> dRight =<\/span> readIR<\/span>(<\/span>IR_RIGHT)<\/span>;<\/span>
    \n float<\/span> dCliffL =<\/span> readIR<\/span>(<\/span>IR_BOTTOM_L)<\/span>;<\/span>
    \n float<\/span> dCliffR =<\/span> readIR<\/span>(<\/span>IR_BOTTOM_R)<\/span>;<\/span><\/p>\n

    \/\/ \u7b2c\u4e00\u5c42：\u60ac\u5d16\u68c0\u6d4b（\u6700\u9ad8\u4f18\u5148\u7ea7）<\/span>
    \n if<\/span> (<\/span>dCliffL <<\/span> CLIFF_THRESHOLD ||<\/span> dCliffR <<\/span> CLIFF_THRESHOLD)<\/span> {<\/span>
    \n safetyLevel =<\/span> LEVEL_EMERGENCY;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u7b2c\u4e8c\u5c42：\u8fd1\u8ddd\u969c\u788d<\/span>
    \n if<\/span> (<\/span>dFront <<\/span> SAFE_FRONT)<\/span> {<\/span>
    \n safetyLevel =<\/span> LEVEL_AVOID;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u7b2c\u4e09\u5c42：\u4e2d\u8ddd\u8b66\u6212<\/span>
    \n if<\/span> (<\/span>dFront <<\/span> CAUTION_FRONT)<\/span> {<\/span>
    \n safetyLevel =<\/span> LEVEL_CAUTION;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    safetyLevel =<\/span> LEVEL_NORMAL;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u5206\u7ea7\u54cd\u5e94\u6267\u884c<\/span>
    \nvoid<\/span> executeResponse<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> dFront =<\/span> readIR<\/span>(<\/span>IR_FRONT)<\/span>;<\/span>
    \n float<\/span> dLeft =<\/span> readIR<\/span>(<\/span>IR_LEFT)<\/span>;<\/span>
    \n float<\/span> dRight =<\/span> readIR<\/span>(<\/span>IR_RIGHT)<\/span>;<\/span><\/p>\n

    float<\/span> v =<\/span> 0<\/span>,<\/span> w =<\/span> 0<\/span>;<\/span><\/p>\n

    switch<\/span> (<\/span>safetyLevel)<\/span> {<\/span>
    \n case<\/span> LEVEL_NORMAL:<\/span>
    \n v =<\/span> 0.30<\/span>;<\/span>
    \n w =<\/span> 0<\/span>;<\/span>
    \n break<\/span>;<\/span><\/p>\n

    case<\/span> LEVEL_CAUTION:<\/span>
    \n v =<\/span> 0.20<\/span>;<\/span> \/\/ \u51cf\u901f<\/span>
    \n w =<\/span> 0<\/span>;<\/span>
    \n break<\/span>;<\/span><\/p>\n

    case<\/span> LEVEL_AVOID:<\/span>
    \n v =<\/span> 0.10<\/span>;<\/span> \/\/ \u6781\u4f4e\u901f<\/span>
    \n \/\/ \u54ea\u8fb9\u7a7a\u95f4\u5927\u5f80\u54ea\u8fb9\u8f6c<\/span>
    \n if<\/span> (<\/span>dLeft ><\/span> dRight)<\/span> w =<\/span> 0.6<\/span>;<\/span>
    \n else<\/span> w =<\/span> –<\/span>0.6<\/span>;<\/span>
    \n break<\/span>;<\/span><\/p>\n

    case<\/span> LEVEL_EMERGENCY:<\/span>
    \n v =<\/span> –<\/span>0.15<\/span>;<\/span> \/\/ \u540e\u9000<\/span>
    \n w =<\/span> 0<\/span>;<\/span>
    \n \/\/ \u540e\u9000 300ms \u540e\u505c\u6b62<\/span>
    \n motorL.<\/span>move<\/span>(<\/span>v)<\/span>;<\/span>
    \n motorR.<\/span>move<\/span>(<\/span>v)<\/span>;<\/span>
    \n delay<\/span>(<\/span>300<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>move<\/span>(<\/span>0<\/span>)<\/span>;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    float<\/span> wheelBase =<\/span> 0.30<\/span>;<\/span>
    \n motorL.<\/span>move<\/span>(<\/span>v –<\/span> w *<\/span> wheelBase \/<\/span> 2<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>move<\/span>(<\/span>v +<\/span> w *<\/span> wheelBase \/<\/span> 2<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
    \n Wire.<\/span>begin<\/span>(<\/span>)<\/span>;<\/span>
    \n imu.<\/span>initialize<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    driverL.<\/span>voltage_power_supply =<\/span> 24<\/span>;<\/span> driverL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>linkDriver<\/span>(<\/span>&<\/span>driverL)<\/span>;<\/span> motorL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorL.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span><\/p>\n

    driverR.<\/span>voltage_power_supply =<\/span> 24<\/span>;<\/span> driverR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>linkDriver<\/span>(<\/span>&<\/span>driverR)<\/span>;<\/span> motorR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/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

    evaluateSafety<\/span>(<\/span>)<\/span>;<\/span>
    \n executeResponse<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u8c03\u8bd5<\/span>
    \n static<\/span> unsigned<\/span> long<\/span> lastPrint =<\/span> 0<\/span>;<\/span>
    \n if<\/span> (<\/span>millis<\/span>(<\/span>)<\/span> –<\/span> lastPrint ><\/span> 300<\/span>)<\/span> {<\/span>
    \n lastPrint =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>"Level:"<\/span>)<\/span>;<\/span> Serial.<\/span>print<\/span>(<\/span>safetyLevel)<\/span>;<\/span>
    \n Serial.<\/span>print<\/span>(<\/span>" Front:"<\/span>)<\/span>;<\/span> Serial.<\/span>println<\/span>(<\/span>readIR<\/span>(<\/span>IR_FRONT)<\/span>,<\/span> 2<\/span>)<\/span>;<\/span>
    \n }<\/span><\/p>\n

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

    \u6838\u5fc3\u8981\u70b9：\u4e09\u5c42\u5b89\u5168\u76d1\u63a7\u7684\u5173\u952e\u662f\u4f18\u5148\u7ea7\u660e\u786e\u3001\u54cd\u5e94\u5206\u7ea7\u3002\u60ac\u5d16\u68c0\u6d4b\u4f5c\u4e3a\u7b2c\u4e00\u5c42（LEVEL_EMERGENCY），\u56e0\u4e3a\u8dcc\u843d\u5bf9\u5bb6\u5ead\u673a\u5668\u4eba\u53ef\u80fd\u662f\u81f4\u547d\u6027\u7684；\u8fd1\u8ddd\u969c\u788d\u4f5c\u4e3a\u7b2c\u4e8c\u5c42（LEVEL_AVOID），\u89e6\u53d1\u8f6c\u5411\u907f\u8ba9；\u4e2d\u8ddd\u8b66\u6212\u4f5c\u4e3a\u7b2c\u4e09\u5c42（LEVEL_CAUTION），\u4ec5\u51cf\u901f\u4e0d\u8f6c\u5411\u3002\u5bb6\u5ead\u573a\u666f\u4e2d，\u7ea2\u5916\u4f20\u611f\u5668\u5bf9\u9ed1\u8272\u5bb6\u5177\u3001\u6df1\u8272\u5730\u6bef\u7684\u68c0\u6d4b\u80fd\u529b\u5f31\u4e8e\u8d85\u58f0\u6ce2，\u4f46\u54cd\u5e94\u901f\u5ea6\u66f4\u5feb，\u9002\u5408\u4f5c\u4e3a\u201c\u6700\u540e\u4e00\u9053\u9632\u7ebf\u201d\u3002<\/p>\n

    3\u3001\u7ea2\u5916\u626b\u63cf\u5f0f\u5c40\u90e8\u5b9a\u4f4d\u4e0e\u52a8\u6001\u907f\u969c
    \n\u573a\u666f：\u673a\u5668\u4eba\u4e0d\u4f7f\u7528\u590d\u6742SLAM，\u800c\u662f\u901a\u8fc7\u7ea2\u5916\u4f20\u611f\u5668\u626b\u63cf\u5468\u56f4\u73af\u5883，\u8bc6\u522b\u201c\u5899\u58c1\/\u969c\u788d\u8f6e\u5ed3\u201d，\u7ed3\u5408IMU\u822a\u5411\u89d2\u8fdb\u884c\u7b80\u5355\u7684\u5c40\u90e8\u5b9a\u4f4d\u4e0e\u52a8\u6001\u907f\u969c，\u9002\u5408\u6210\u672c\u654f\u611f\u7684\u5bb6\u5ead\u673a\u5668\u4eba\u3002<\/p>\n

    \/* ===== \u7ea2\u5916\u626b\u63cf\u5c40\u90e8\u5b9a\u4f4d + \u52a8\u6001\u907f\u969c =====
    \n * \u786c\u4ef6：Arduino + \u4f3a\u670d\u4e91\u53f0 + \u7ea2\u5916\u6d4b\u8ddd + IMU
    \n * \u6838\u5fc3：\u626b\u63cf\u5f0f\u73af\u5883\u611f\u77e5 + \u5c40\u90e8\u6781\u5750\u6807\u5b9a\u4f4d
    \n *\/<\/span>
    \n#<\/span>include<\/span> <SimpleFOC.h><\/span><\/span>
    \n#<\/span>include<\/span> <Servo.h><\/span><\/span>
    \n#<\/span>include<\/span> <Wire.h><\/span><\/span>
    \n#<\/span>include<\/span> <MPU6050.h><\/span><\/span><\/p>\n

    BLDCMotor motorL<\/span>(<\/span>7<\/span>)<\/span>,<\/span> motorR<\/span>(<\/span>7<\/span>)<\/span>;<\/span>
    \nBLDCDriver3PWM driverL<\/span>(<\/span>9<\/span>,<\/span> 10<\/span>,<\/span> 11<\/span>,<\/span> 8<\/span>)<\/span>;<\/span>
    \nBLDCDriver3PWM driverR<\/span>(<\/span>3<\/span>,<\/span> 5<\/span>,<\/span> 6<\/span>,<\/span> 7<\/span>)<\/span>;<\/span><\/p>\n

    MPU6050 imu;<\/span>
    \nServo scanServo;<\/span><\/p>\n

    \/\/ — \u626b\u63cf\u53c2\u6570 —<\/span>
    \nconst<\/span> int<\/span> SCAN_MIN =<\/span> 0<\/span>;<\/span> \/\/ \u6700\u5c0f\u89d2\u5ea6<\/span>
    \nconst<\/span> int<\/span> SCAN_MAX =<\/span> 180<\/span>;<\/span> \/\/ \u6700\u5927\u89d2\u5ea6<\/span>
    \nconst<\/span> int<\/span> SCAN_STEP =<\/span> 15<\/span>;<\/span> \/\/ \u6b65\u8fdb\u89d2\u5ea6<\/span>
    \nconst<\/span> int<\/span> SCAN_COUNT =<\/span> (<\/span>SCAN_MAX –<\/span> SCAN_MIN)<\/span> \/<\/span> SCAN_STEP +<\/span> 1<\/span>;<\/span><\/p>\n

    float<\/span> scanDist[<\/span>SCAN_COUNT]<\/span>;<\/span>
    \nint<\/span> scanIdx =<\/span> 0<\/span>;<\/span>
    \nunsigned<\/span> long<\/span> lastScan =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ — \u907f\u969c\u53c2\u6570 —<\/span>
    \nconst<\/span> float<\/span> SAFE_DIST =<\/span> 0.35<\/span>;<\/span> \/\/ \u5b89\u5168\u8ddd\u79bb (m)<\/span>
    \nfloat<\/span> yaw =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ \u7ea2\u5916\u6d4b\u8ddd（\u7b80\u5316）<\/span>
    \nfloat<\/span> readIRDistance<\/span>(<\/span>)<\/span> {<\/span>
    \n int<\/span> val =<\/span> analogRead<\/span>(<\/span>A0)<\/span>;<\/span>
    \n float<\/span> dist =<\/span> 1000.0<\/span> \/<\/span> (<\/span>val +<\/span> 1<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>dist ><\/span> 80<\/span>)<\/span> dist =<\/span> 80<\/span>;<\/span>
    \n if<\/span> (<\/span>dist <<\/span> 5<\/span>)<\/span> dist =<\/span> 5<\/span>;<\/span>
    \n return<\/span> dist \/<\/span> 100.0<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ IMU\u822a\u5411\u89d2\u66f4\u65b0<\/span>
    \nvoid<\/span> updateYaw<\/span>(<\/span>)<\/span> {<\/span>
    \n int16_t<\/span> gz;<\/span>
    \n imu.<\/span>getRotation<\/span>(<\/span>nullptr<\/span>,<\/span> nullptr<\/span>,<\/span> &<\/span>gz)<\/span>;<\/span>
    \n yaw +=<\/span> (<\/span>gz \/<\/span> 131.0<\/span>)<\/span> *<\/span> 0.02<\/span>;<\/span> \/\/ \u5ea6 \u2192 \u5f27\u5ea6\u8fd1\u4f3c<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u626b\u63cf\u66f4\u65b0<\/span>
    \nvoid<\/span> updateScan<\/span>(<\/span>)<\/span> {<\/span>
    \n if<\/span> (<\/span>millis<\/span>(<\/span>)<\/span> –<\/span> lastScan <<\/span> 40<\/span>)<\/span> return<\/span>;<\/span>
    \n lastScan =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    int<\/span> angle =<\/span> SCAN_MIN +<\/span> scanIdx *<\/span> SCAN_STEP;<\/span>
    \n scanServo.<\/span>write<\/span>(<\/span>angle)<\/span>;<\/span>
    \n delay<\/span>(<\/span>15<\/span>)<\/span>;<\/span> \/\/ \u7b49\u5f85\u8235\u673a\u5230\u4f4d<\/span><\/p>\n

    scanDist[<\/span>scanIdx]<\/span> =<\/span> readIRDistance<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    scanIdx =<\/span> (<\/span>scanIdx +<\/span> 1<\/span>)<\/span> %<\/span> SCAN_COUNT;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u67e5\u627e\u6700\u5b89\u5168\u65b9\u5411<\/span>
    \nint<\/span> findSafestDirection<\/span>(<\/span>)<\/span> {<\/span>
    \n int<\/span> bestIdx =<\/span> 0<\/span>;<\/span>
    \n float<\/span> maxDist =<\/span> 0<\/span>;<\/span><\/p>\n

    for<\/span> (<\/span>int<\/span> i =<\/span> 0<\/span>;<\/span> i <<\/span> SCAN_COUNT;<\/span> i++<\/span>)<\/span> {<\/span>
    \n if<\/span> (<\/span>scanDist[<\/span>i]<\/span> ><\/span> maxDist)<\/span> {<\/span>
    \n maxDist =<\/span> scanDist[<\/span>i]<\/span>;<\/span>
    \n bestIdx =<\/span> i;<\/span>
    \n }<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u8f6c\u6362\u4e3a\u76f8\u5bf9\u89d2\u5ea6（0-180 \u2192 -90~90）<\/span>
    \n int<\/span> angle =<\/span> SCAN_MIN +<\/span> bestIdx *<\/span> SCAN_STEP;<\/span>
    \n return<\/span> angle –<\/span> 90<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u524d\u65b9\u969c\u788d\u68c0\u6d4b<\/span>
    \nfloat<\/span> getFrontDistance<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u53d6\u4e2d\u95f4\u4e09\u4e2a\u626b\u63cf\u70b9<\/span>
    \n int<\/span> mid =<\/span> SCAN_COUNT \/<\/span> 2<\/span>;<\/span>
    \n float<\/span> minDist =<\/span> 10<\/span>;<\/span>
    \n for<\/span> (<\/span>int<\/span> i =<\/span> mid –<\/span> 1<\/span>;<\/span> i <=<\/span> mid +<\/span> 1<\/span>;<\/span> i++<\/span>)<\/span> {<\/span>
    \n if<\/span> (<\/span>i >=<\/span> 0<\/span> &&<\/span> i <<\/span> SCAN_COUNT)<\/span> {<\/span>
    \n minDist =<\/span> min<\/span>(<\/span>minDist,<\/span> scanDist[<\/span>i]<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n }<\/span>
    \n return<\/span> minDist;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> setup<\/span>(<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>begin<\/span>(<\/span>115200<\/span>)<\/span>;<\/span>
    \n Wire.<\/span>begin<\/span>(<\/span>)<\/span>;<\/span>
    \n imu.<\/span>initialize<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    scanServo.<\/span>attach<\/span>(<\/span>9<\/span>)<\/span>;<\/span>
    \n scanServo.<\/span>write<\/span>(<\/span>90<\/span>)<\/span>;<\/span><\/p>\n

    driverL.<\/span>voltage_power_supply =<\/span> 24<\/span>;<\/span> driverL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>linkDriver<\/span>(<\/span>&<\/span>driverL)<\/span>;<\/span> motorL.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorL.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
    \n motorL.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span><\/p>\n

    driverR.<\/span>voltage_power_supply =<\/span> 24<\/span>;<\/span> driverR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>linkDriver<\/span>(<\/span>&<\/span>driverR)<\/span>;<\/span> motorR.<\/span>init<\/span>(<\/span>)<\/span>;<\/span> motorR.<\/span>initFOC<\/span>(<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>controller =<\/span> MotionControlType::<\/span>velocity;<\/span><\/p>\n

    \/\/ \u521d\u59cb\u5316\u626b\u63cf\u6570\u636e<\/span>
    \n for<\/span> (<\/span>int<\/span> i =<\/span> 0<\/span>;<\/span> i <<\/span> SCAN_COUNT;<\/span> i++<\/span>)<\/span> scanDist[<\/span>i]<\/span> =<\/span> 2.0<\/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

    updateYaw<\/span>(<\/span>)<\/span>;<\/span>
    \n updateScan<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    float<\/span> frontDist =<\/span> getFrontDistance<\/span>(<\/span>)<\/span>;<\/span>
    \n float<\/span> v =<\/span> 0.25<\/span>,<\/span> w =<\/span> 0<\/span>;<\/span><\/p>\n

    if<\/span> (<\/span>frontDist <<\/span> SAFE_DIST)<\/span> {<\/span>
    \n \/\/ \u524d\u65b9\u6709\u969c\u788d：\u67e5\u627e\u6700\u5b89\u5168\u65b9\u5411<\/span>
    \n int<\/span> safeDir =<\/span> findSafestDirection<\/span>(<\/span>)<\/span>;<\/span>
    \n v =<\/span> 0.10<\/span>;<\/span>
    \n w =<\/span> safeDir *<\/span> 0.015<\/span>;<\/span> \/\/ \u6839\u636e\u5b89\u5168\u65b9\u5411\u8f6c\u5411<\/span><\/p>\n

    \/\/ \u9650\u5e45<\/span>
    \n w =<\/span> constrain<\/span>(<\/span>w,<\/span> –<\/span>0.8<\/span>,<\/span> 0.8<\/span>)<\/span>;<\/span>
    \n }<\/span><\/p>\n

    float<\/span> wheelBase =<\/span> 0.30<\/span>;<\/span>
    \n motorL.<\/span>move<\/span>(<\/span>v –<\/span> w *<\/span> wheelBase \/<\/span> 2<\/span>)<\/span>;<\/span>
    \n motorR.<\/span>move<\/span>(<\/span>v +<\/span> w *<\/span> wheelBase \/<\/span> 2<\/span>)<\/span>;<\/span><\/p>\n

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

    \u6838\u5fc3\u8981\u70b9：\u626b\u63cf\u5f0f\u5b9a\u4f4d\u7684\u4f18\u70b9\u662f\u7528\u4e00\u4e2a\u7ea2\u5916\u4f20\u611f\u5668\u83b7\u5f97 180\u00b0 \u89c6\u91ce，\u6210\u672c\u8fdc\u4f4e\u4e8e\u591a\u4f20\u611f\u5668\u9635\u5217\u3002\u7a0b\u5e8f\u4e2d\u7684 findSafestDirection() \u904d\u5386\u626b\u63cf\u6570\u636e\u627e\u5230\u8ddd\u79bb\u6700\u5927\u7684\u65b9\u5411，\u4f5c\u4e3a\u907f\u969c\u8f6c\u5411\u7684\u76ee\u6807\u3002\u8fd9\u79cd\u65b9\u6cd5\u5728\u5bb6\u5ead\u73af\u5883\u4e2d\u7279\u522b\u6709\u6548，\u56e0\u4e3a\u5bb6\u5177\u548c\u5899\u58c1\u7684\u53cd\u5c04\u7387\u7a33\u5b9a，\u7ea2\u5916\u626b\u63cf\u80fd\u53ef\u9760\u5730\u52fe\u52d2\u51fa\u623f\u95f4\u8f6e\u5ed3\u3002IMU \u822a\u5411\u89d2\u7528\u4e8e\u5c06\u626b\u63cf\u6570\u636e\u4ece\u201c\u673a\u5668\u4eba\u5750\u6807\u7cfb\u201d\u8f6c\u6362\u5230\u201c\u4e16\u754c\u5750\u6807\u7cfb\u201d，\u5b9e\u73b0\u7b80\u5355\u7684\u5c40\u90e8\u5b9a\u4f4d\u3002<\/p>\n

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

  • \n

    \u7ea2\u5916+IMU \u878d\u5408\u7684\u672c\u8d28\u662f\u201c\u4e92\u8865\u201d，\u800c\u975e\u201c\u5197\u4f59\u201d
    \n\u7ea2\u5916\u4f20\u611f\u5668\u63d0\u4f9b\u73af\u5883\u4fe1\u606f（\u969c\u788d\u7269\u8ddd\u79bb\u3001\u60ac\u5d16），\u4f46\u6613\u53d7\u5149\u7167\u548c\u8868\u9762\u53cd\u5c04\u7387\u5f71\u54cd；IMU \u63d0\u4f9b\u81ea\u8eab\u72b6\u6001（\u59ff\u6001\u3001\u822a\u5411），\u4f46\u5b58\u5728\u79ef\u5206\u6f02\u79fb\u3002\u4e24\u8005\u878d\u5408\u7684\u4ef7\u503c\u5728\u4e8e：IMU \u5728\u7ea2\u5916\u5e27\u95f4\u63d0\u4f9b\u9ad8\u9891\u59ff\u6001\u9884\u6d4b，\u7ea2\u5916\u5728\u6821\u6b63\u65f6\u63d0\u4f9b\u7edd\u5bf9\u53c2\u8003\u3002\u4e92\u8865\u6ee4\u6ce2\u4e2d\u7684 ALPHA = 0.98 \u610f\u5473\u7740\u7cfb\u7edf 98% \u4fe1\u4efb\u9640\u87ba\u4eea\u7684\u77ed\u671f\u53d8\u5316，2% \u7528\u957f\u671f\u57fa\u51c6\u4fee\u6b63\u6f02\u79fb，\u8fd9\u662f\u8f7b\u91cf\u7ea7 Arduino \u4e0a\u6700\u5b9e\u7528\u7684\u878d\u5408\u7b56\u7565\u3002<\/p>\n<\/li>\n

  • \n

    \u5bb6\u5ead SLAM \u7684\u201c\u591f\u7528\u539f\u5219\u201d：\u4e0d\u9700\u8981\u5398\u7c73\u7ea7\u7cbe\u5ea6，\u4f46\u9700\u8981\u7edd\u5bf9\u5b89\u5168
    \n\u5de5\u4e1a SLAM \u8ffd\u6c42\u9ad8\u7cbe\u5ea6\u5efa\u56fe，\u5bb6\u5ead SLAM \u7684\u6838\u5fc3\u7ea6\u675f\u662f\u5b89\u5168\u3002\u4e09\u5c42\u5b89\u5168\u76d1\u63a7（\u60ac\u5d16\u2192\u8fd1\u8ddd\u2192\u4e2d\u8ddd）\u662f\u5bb6\u5ead\u673a\u5668\u4eba\u7684\u201c\u751f\u5b58\u5e95\u7ebf\u201d\u3002\u60ac\u5d16\u68c0\u6d4b\u5fc5\u987b\u72ec\u7acb\u4e8e SLAM \u4e3b\u5faa\u73af\u8fd0\u884c，\u56e0\u4e3a\u8dcc\u843d\u5bf9\u5bb6\u5ead\u673a\u5668\u4eba\u53ef\u80fd\u662f\u81f4\u547d\u7684\u3002\u7a0b\u5e8f\u6848\u4f8b\u4e8c\u7684 LEVEL_EMERGENCY \u54cd\u5e94（\u540e\u9000+\u505c\u6b62）\u5fc5\u987b\u4f18\u5148\u4e8e\u6240\u6709\u5176\u4ed6\u903b\u8f91\u3002<\/p>\n<\/li>\n

  • \n

    \u6805\u683c\u5730\u56fe\u7684\u589e\u91cf\u66f4\u65b0\u6bd4\u5168\u56fe\u91cd\u6784\u66f4\u9002\u5408 Arduino
    \n\u5bb6\u5ead\u73af\u5883\u7684\u5730\u56fe\u53d8\u5316\u7f13\u6162（\u5bb6\u5177\u57fa\u672c\u56fa\u5b9a），\u4f46 Arduino \u7b97\u529b\u6709\u9650\u3002\u7a0b\u5e8f\u6848\u4f8b\u4e00\u4e2d\u7684\u6805\u683c\u66f4\u65b0\u53ea\u6807\u8bb0\u7ea2\u5916\u626b\u63cf\u5230\u7684\u6805\u683c，\u4e0d\u8fdb\u884c\u5168\u56fe\u91cd\u7b97\u300220\u00d720 \u7684 byte \u6570\u7ec4\u4ec5\u5360 400 \u5b57\u8282，\u8fdc\u5c0f\u4e8e\u5b8c\u6574\u6982\u7387\u6805\u683c\u5730\u56fe\u7684\u5185\u5b58\u9700\u6c42\u3002\u5bf9\u4e8e\u66f4\u5927\u7684\u5bb6\u5ead\u73af\u5883，\u53ef\u91c7\u7528\u201c\u6eda\u52a8\u7a97\u53e3\u6805\u683c\u201d\u2014\u2014\u53ea\u7ef4\u62a4\u673a\u5668\u4eba\u5468\u56f4\u7684\u5c40\u90e8\u5730\u56fe\u3002<\/p>\n<\/li>\n

  • \n

    \u7ea2\u5916\u4f20\u611f\u5668\u5728\u5bb6\u5ead\u573a\u666f\u4e2d\u7684\u201c\u76f2\u533a\u201d\u5fc5\u987b\u901a\u8fc7\u5e03\u5c40\u8865\u507f
    \n\u7ea2\u5916\u4f20\u611f\u5668\u5bf9\u9ed1\u8272\u7269\u4f53（\u6df1\u8272\u6c99\u53d1\u3001\u9ed1\u8272\u5730\u6bef）\u68c0\u6d4b\u8ddd\u79bb\u4f1a\u663e\u8457\u7f29\u77ed，\u5bf9\u900f\u660e\u7269\u4f53（\u73bb\u7483\u8336\u51e0）\u51e0\u4e4e\u65e0\u53cd\u5c04\u3002\u7a0b\u5e8f\u6848\u4f8b\u4e00\u548c\u6848\u4f8b\u4e8c\u90fd\u90e8\u7f72\u4e86\u591a\u65b9\u5411\u7ea2\u5916（\u524d\/\u5de6\/\u53f3\/\u5e95），\u901a\u8fc7\u7a7a\u95f4\u5197\u4f59\u8865\u507f\u5355\u4e00\u4f20\u611f\u5668\u7684\u76f2\u533a\u3002\u5bb6\u5ead\u673a\u5668\u4eba\u5e94\u4f18\u5148\u9009\u62e9\u6570\u5b57\u7ea2\u5916\u4f20\u611f\u5668（\u5982 E18-D80NK），\u5176\u9608\u503c\u53ef\u8c03，\u5bf9\u9ed1\u8272\u7269\u4f53\u7684\u68c0\u6d4b\u7a33\u5b9a\u6027\u4f18\u4e8e\u6a21\u62df\u7ea2\u5916\u3002<\/p>\n<\/li>\n

  • \n

    BLDC+FOC \u5728\u5bb6\u5ead\u673a\u5668\u4eba\u4e2d\u7684\u4ef7\u503c\u662f\u201c\u5b89\u9759\u4e14\u7cbe\u786e\u201d
    \n\u5bb6\u5ead\u573a\u666f\u5bf9\u566a\u97f3\u654f\u611f，\u6709\u5237\u7535\u673a\u7684\u6362\u76f8\u5578\u53eb\u548c\u9f7f\u69fd\u6548\u5e94\u4f1a\u4ea7\u751f\u660e\u663e\u566a\u97f3\u3002BLDC \u914d\u5408 FOC \u7684\u6b63\u5f26\u9a71\u52a8\u4f7f\u7535\u673a\u8fd0\u884c\u66f4\u5b89\u9759，\u4f4e\u901f\u4e0b\u8f6c\u77e9\u8109\u52a8\u5c0f，\u9002\u5408\u5bb6\u5ead\u591c\u95f4\u5de1\u822a\u3002\u540c\u65f6，FOC \u7684\u901f\u5ea6\u95ed\u73af\u63a7\u5236\u4f7f\u7f16\u7801\u5668\u91cc\u7a0b\u8ba1\u6570\u636e\u66f4\u5e73\u6ed1，\u95f4\u63a5\u6539\u5584\u4e86\u8f7b\u91cf SLAM \u7684\u4f4d\u59ff\u4f30\u8ba1\u8d28\u91cf\u3002SimpleFOC \u5e93\u7684 estimated_current \u6a21\u5f0f\u5728\u65e0\u7535\u6d41\u4f20\u611f\u5668\u65f6\u63d0\u4f9b\u4e86\u8fd1\u4f3c\u529b\u77e9\u63a7\u5236，\u8fdb\u4e00\u6b65\u964d\u4f4e\u4e86\u786c\u4ef6\u6210\u672c\u3002<\/p>\n<\/li>\n

    \"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\"
    \n\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u57fa\u7840\u5bfc\u822a\u7a0b\u5e8f（\u6838\u5fc3：\u573a\u5730\u6784\u5efa\u4e0e\u57fa\u7840\u5b9a\u4f4d）
    \n\u9002\u7528\u573a\u666f：\u5bb6\u5ead\u5ba2\u5385\u3001\u9910\u5385\u7b49\u5f00\u9614\u533a\u57df，\u673a\u5668\u4eba\u5b9e\u73b0\u57fa\u7840\u73af\u5883\u626b\u63cf\u3001\u5730\u56fe\u6784\u5efa\u4e0e\u81ea\u4e3b\u5bfc\u822a，\u5b8c\u6210\u7269\u54c1\u9012\u9001\u3001\u533a\u57df\u5de1\u68c0\u7b49\u57fa\u7840\u670d\u52a1\u3002<\/p>\n

    \u6838\u5fc3\u903b\u8f91
    \n\u878d\u5408\u7ea2\u5916\u6d4b\u8ddd\u4e0eIMU\u59ff\u6001\u6570\u636e，\u5b9e\u73b0\u6f02\u79fb\u8865\u507f，\u63d0\u5347\u5b9a\u4f4d\u7cbe\u5ea6
    \n\u57fa\u4e8e\u7ea2\u5916\u91c7\u96c6\u7684\u8fb9\u754c\u70b9，\u7b80\u5316\u6784\u5efa\u573a\u5730\u8f6e\u5ed3\u5730\u56fe
    \n\u6839\u636e\u5b9a\u4f4d\u7ed3\u679c，\u63a7\u5236\u673a\u5668\u4eba\u907f\u969c\u5e76\u524d\u5f80\u76ee\u6807\u4f4d\u7f6e<\/p>\n

    \/\/ \u7ea2\u5916+IMU\u878d\u5408\u8f7b\u91cfSLAM\u57fa\u7840\u5bfc\u822a\u7a0b\u5e8f<\/span>
    \n\/\/ \u786c\u4ef6：3\u8def\u7ea2\u5916\u6d4b\u8ddd+MPU6050 IMU+2\u8f6eBLDC\u9a71\u52a8<\/span>
    \n\/\/ \u529f\u80fd：\u5bb6\u5ead\u5f00\u9614\u533a\u57df\u5730\u56fe\u6784\u5efa\u3001\u5b9a\u4f4d\u4e0e\u57fa\u7840\u907f\u969c\u5bfc\u822a<\/span><\/p>\n

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

    \/\/ ========== \u786c\u4ef6\u5f15\u811a\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> byte IR_LEFT =<\/span> A0,<\/span> IR_RIGHT =<\/span> A1,<\/span> IR_FRONT =<\/span> A2;<\/span> \/\/ \u7ea2\u5916\u6d4b\u8ddd\u5f15\u811a<\/span>
    \nconst<\/span> byte PWM_LEFT =<\/span> 6<\/span>,<\/span> DIR_LEFT =<\/span> 7<\/span>,<\/span> PWM_RIGHT =<\/span> 8<\/span>,<\/span> DIR_RIGHT =<\/span> 9<\/span>;<\/span> \/\/ BLDC\u9a71\u52a8\u5f15\u811a<\/span><\/p>\n

    \/\/ ========== SLAM\u6838\u5fc3\u53c2\u6570 ==========<\/span>
    \nfloat<\/span> robotX =<\/span> 0.0<\/span>,<\/span> robotY =<\/span> 0.0<\/span>;<\/span> \/\/ \u673a\u5668\u4eba\u5750\u6807（\u57fa\u4e8e\u8d77\u70b9）<\/span>
    \nfloat<\/span> headingAngle =<\/span> 0.0<\/span>;<\/span> \/\/ \u822a\u5411\u89d2（\u5ea6，\u5317\u4e3a0，\u987a\u65f6\u9488\u589e\u52a0）<\/span>
    \nfloat<\/span> moveSpeed =<\/span> 30.0<\/span>;<\/span> \/\/ \u79fb\u52a8\u901f\u5ea6（cm\/s）<\/span>
    \nfloat<\/span> lastGyroAngle =<\/span> 0.0<\/span>;<\/span> \/\/ \u4e0a\u4e00\u6b21\u9640\u87ba\u4eea\u7d2f\u52a0\u89d2\u5ea6<\/span>
    \nconst<\/span> float<\/span> STEP_DISTANCE =<\/span> 2.0<\/span>;<\/span> \/\/ \u6bcf\u6b65\u91c7\u6837\u8ddd\u79bb（cm）<\/span>
    \nconst<\/span> float<\/span> POSITION_UPDATE_RATE =<\/span> 50.0<\/span>;<\/span> \/\/ \u5b9a\u4f4d\u66f4\u65b0\u5468\u671f（ms）<\/span><\/p>\n

    \/\/ \u7b80\u6613\u5730\u56fe（\u8bb0\u5f55\u7ea2\u5916\u68c0\u6d4b\u5230\u7684\u8fb9\u754c，0\u4e3a\u53ef\u901a\u884c，1\u4e3a\u969c\u788d）<\/span>
    \n#<\/span>define<\/span> MAP_SIZE<\/span> 100<\/span><\/span><\/span>
    \nbyte simpleMap[<\/span>MAP_SIZE]<\/span>[<\/span>MAP_SIZE]<\/span>;<\/span>
    \nbool<\/span> mapInitialized =<\/span> false<\/span>;<\/span><\/p>\n

    MPU6050 mpu;<\/span>
    \nunsigned<\/span> long<\/span> lastUpdateTime =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ ========== \u51fd\u6570\u58f0\u660e ==========<\/span>
    \nvoid<\/span> initIMU<\/span>(<\/span>)<\/span>;<\/span>
    \nfloat<\/span> getHeading<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> updateOdometry<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> scanEnvironment<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> updateMap<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span>\u907f\u969cNavigate<\/span>(<\/span>float<\/span> targetX,<\/span> float<\/span> targetY)<\/span>;<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>float<\/span> left,<\/span> float<\/span> right)<\/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>"\u7ea2\u5916+IMU\u878d\u5408SLAM\u5bfc\u822a\u7cfb\u7edf\u521d\u59cb\u5316\u5b8c\u6210"<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u521d\u59cb\u5316IMU<\/span>
    \n Wire.<\/span>begin<\/span>(<\/span>)<\/span>;<\/span>
    \n initIMU<\/span>(<\/span>)<\/span>;<\/span><\/p>\n

    \/\/ \u521d\u59cb\u5316\u5730\u56fe<\/span>
    \n for<\/span> (<\/span>int<\/span> i=<\/span>0<\/span>;<\/span> i<<\/span>MAP_SIZE;<\/span> i++<\/span>)<\/span>{<\/span>
    \n for<\/span> (<\/span>int<\/span> j=<\/span>0<\/span>;<\/span> j<<\/span>MAP_SIZE;<\/span> j++<\/span>)<\/span>{<\/span>
    \n simpleMap[<\/span>i]<\/span>[<\/span>j]<\/span> =<\/span> 0<\/span>;<\/span>
    \n }<\/span> }<\/span>
    \n mapInitialized =<\/span> true<\/span>;<\/span><\/p>\n

    \/\/ \u521d\u59cb\u5316\u9a71\u52a8\u5f15\u811a<\/span>
    \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}<\/span><\/p>\n

    void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
    \n uint32_t<\/span> now =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
    \n \/\/ \u5468\u671f\u66f4\u65b0\u5b9a\u4f4d\u4e0e\u5730\u56fe<\/span>
    \n if<\/span> (<\/span>now –<\/span> lastUpdateTime ><\/span> POSITION_UPDATE_RATE)<\/span> {<\/span>
    \n updateOdometry<\/span>(<\/span>)<\/span>;<\/span> \/\/ 1. \u878d\u5408IMU\u66f4\u65b0\u5b9a\u4f4d<\/span>
    \n scanEnvironment<\/span>(<\/span>)<\/span>;<\/span> \/\/ 2. \u7ea2\u5916\u626b\u63cf\u73af\u5883<\/span>
    \n updateMap<\/span>(<\/span>)<\/span>;<\/span> \/\/ 3. \u66f4\u65b0\u5730\u56fe<\/span>
    \n lastUpdateTime =<\/span> now;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u5bfc\u822a\u5230\u76ee\u6807\u4f4d\u7f6e（\u793a\u4f8b：\u524d\u65b9100cm，\u53f3\u4fa750cm）<\/span>
    \n Navigate<\/span>(<\/span>100.0<\/span>,<\/span> 50.0<\/span>)<\/span>;<\/span>
    \n delay<\/span>(<\/span>10<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u521d\u59cb\u5316MPU6050<\/span>
    \nvoid<\/span> initIMU<\/span>(<\/span>)<\/span> {<\/span>
    \n if<\/span> (<\/span>!<\/span>mpu.<\/span>begin<\/span>(<\/span>MPU6050_SCALE_2000DPS)<\/span>)<\/span> {<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"MPU6050\u521d\u59cb\u5316\u5931\u8d25！"<\/span>)<\/span>;<\/span>
    \n while<\/span> (<\/span>1<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n mpu.<\/span>setGyroRange<\/span>(<\/span>MPU6050_GYRO_2000DPS)<\/span>;<\/span>
    \n mpu.<\/span>setAccelRange<\/span>(<\/span>MPU6050_ACCEL_2G)<\/span>;<\/span>
    \n mpu.<\/span>setSleepEnabled<\/span>(<\/span>false<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u83b7\u53d6\u822a\u5411\u89d2（\u878d\u5408\u9640\u87ba\u4eea，\u6f02\u79fb\u8865\u507f）<\/span>
    \nfloat<\/span> getHeading<\/span>(<\/span>)<\/span> {<\/span>
    \n mpu.<\/span>update<\/span>(<\/span>)<\/span>;<\/span>
    \n float<\/span> gyroZ =<\/span> mpu.<\/span>getGyroZ<\/span>(<\/span>)<\/span>;<\/span> \/\/ \u9640\u87ba\u4eeaZ\u8f74\u89d2\u901f\u5ea6（\u00b0\/s）<\/span>
    \n \/\/ \u89d2\u901f\u5ea6\u79ef\u5206\u66f4\u65b0\u89d2\u5ea6，\u5b9a\u671f\u590d\u4f4d\u57fa\u51c6<\/span>
    \n headingAngle +=<\/span> gyroZ *<\/span> 0.01<\/span>;<\/span> \/\/ \u91c7\u6837\u5468\u671f10ms<\/span>
    \n \/\/ \u7b80\u6613\u6821\u51c6：\u8d85\u8fc7360\u5ea6\u5f52\u96f6，\u51cf\u5c11\u7d2f\u79ef\u8bef\u5dee<\/span>
    \n if<\/span> (<\/span>headingAngle ><\/span> 360.0<\/span>)<\/span> headingAngle -=<\/span> 360.0<\/span>;<\/span>
    \n if<\/span> (<\/span>headingAngle <<\/span> 0.0<\/span>)<\/span> headingAngle +=<\/span> 360.0<\/span>;<\/span>
    \n return<\/span> headingAngle;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u878d\u5408IMU\u4e0e\u7f16\u7801\u5668，\u66f4\u65b0\u91cc\u7a0b\u8ba1\u5b9a\u4f4d<\/span>
    \nvoid<\/span> updateOdometry<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> currentHeading =<\/span> getHeading<\/span>(<\/span>)<\/span>;<\/span>
    \n \/\/ \u6a21\u62df\u7f16\u7801\u5668\u83b7\u53d6\u79fb\u52a8\u8ddd\u79bb（\u6b64\u5904\u7b80\u5316，\u5b9e\u9645\u9700\u63a5\u5165\u7f16\u7801\u5668\u8ba1\u6570）<\/span>
    \n float<\/span> moveDistance =<\/span> getEncoderDistance<\/span>(<\/span>)<\/span>;<\/span> \/\/ \u5b9e\u9645\u573a\u666f\u9700\u5b9e\u73b0\u7f16\u7801\u5668\u8bfb\u53d6\u51fd\u6570<\/span>
    \n \/\/ \u6781\u5750\u6807\u8f6c\u7b1b\u5361\u5c14\u5750\u6807，\u66f4\u65b0\u4f4d\u7f6e<\/span>
    \n float<\/span> deltaX =<\/span> moveDistance *<\/span> cos<\/span>(<\/span>radians<\/span>(<\/span>currentHeading)<\/span>)<\/span>;<\/span>
    \n float<\/span> deltaY =<\/span> moveDistance *<\/span> sin<\/span>(<\/span>radians<\/span>(<\/span>currentHeading)<\/span>)<\/span>;<\/span>
    \n robotX +=<\/span> deltaX \/<\/span> STEP_DISTANCE;<\/span>
    \n robotY +=<\/span> deltaY \/<\/span> STEP_DISTANCE;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7ea2\u5916\u626b\u63cf\u73af\u5883\u969c\u788d<\/span>
    \nvoid<\/span> scanEnvironment<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> irLeft =<\/span> analogRead<\/span>(<\/span>IR_LEFT)<\/span> *<\/span> 0.1<\/span>;<\/span>
    \n float<\/span> irRight =<\/span> analogRead<\/span>(<\/span>IR_RIGHT)<\/span> *<\/span> 0.1<\/span>;<\/span>
    \n float<\/span> irFront =<\/span> analogRead<\/span>(<\/span>IR_FRONT)<\/span> *<\/span> 0.1<\/span>;<\/span>
    \n \/\/ \u68c0\u6d4b\u969c\u788d\u8303\u56f4，\u8d85\u51fa\u5b89\u5168\u8ddd\u79bb\u6807\u8bb0\u4e3a\u969c\u788d<\/span>
    \n if<\/span> (<\/span>irFront <<\/span> 150.0<\/span>)<\/span> {<\/span>
    \n int<\/span> mapX =<\/span> (<\/span>int<\/span>)<\/span>robotX;<\/span>
    \n int<\/span> mapY =<\/span> (<\/span>int<\/span>)<\/span>robotY;<\/span>
    \n if<\/span> (<\/span>mapX >=<\/span>0<\/span> &&<\/span> mapX <<\/span> MAP_SIZE &&<\/span> mapY >=<\/span>0<\/span> &&<\/span> mapY <<\/span> MAP_SIZE)<\/span> {<\/span>
    \n simpleMap[<\/span>mapX]<\/span>[<\/span>mapY]<\/span> =<\/span> 1<\/span>;<\/span> \/\/ \u6807\u8bb0\u524d\u65b9\u969c\u788d<\/span>
    \n }<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u66f4\u65b0\u7b80\u6613\u5730\u56fe，\u7b80\u5316\u6784\u5efa\u573a\u5730\u8f6e\u5ed3<\/span>
    \nvoid<\/span> updateMap<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u5c06\u5f53\u524d\u4f4d\u7f6e\u53ca\u5468\u8fb9\u533a\u57df\u6807\u8bb0\u4e3a\u5df2\u626b\u63cf<\/span>
    \n int<\/span> cx =<\/span> (<\/span>int<\/span>)<\/span>robotX;<\/span>
    \n int<\/span> cy =<\/span> (<\/span>int<\/span>)<\/span>robotY;<\/span>
    \n for<\/span> (<\/span>int<\/span> i=<\/span>max<\/span>(<\/span>0<\/span>,<\/span> cx–<\/span>2<\/span>)<\/span>;<\/span> i<<\/span>min<\/span>(<\/span>MAP_SIZE,<\/span> cx+<\/span>3<\/span>)<\/span>;<\/span> i++<\/span>)<\/span>{<\/span>
    \n for<\/span> (<\/span>int<\/span> j=<\/span>max<\/span>(<\/span>0<\/span>,<\/span> cy–<\/span>2<\/span>)<\/span>;<\/span> j<<\/span>min<\/span>(<\/span>MAP_SIZE,<\/span> cy+<\/span>3<\/span>)<\/span>;<\/span> j++<\/span>)<\/span>{<\/span>
    \n \/\/ \u672a\u6807\u8bb0\u4e14\u65e0\u969c\u788d\u7684\u533a\u57df\u6807\u8bb0\u4e3a\u5df2\u63a2\u7d22<\/span>
    \n if<\/span> (<\/span>simpleMap[<\/span>i]<\/span>[<\/span>j]<\/span> !=<\/span>1<\/span>)<\/span> {<\/span>
    \n simpleMap[<\/span>i]<\/span>[<\/span>j]<\/span> =<\/span> 2<\/span>;<\/span> \/\/ 2\u4e3a\u5df2\u63a2\u7d22\u53ef\u901a\u884c\u533a\u57df<\/span>
    \n }<\/span>
    \n }<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u57fa\u7840\u907f\u969c\u5bfc\u822a：\u8d70\u5411\u76ee\u6807\u4f4d\u7f6e，\u9047\u969c\u907f\u8ba9<\/span>
    \nvoid<\/span> Navigate<\/span>(<\/span>float<\/span> targetX,<\/span> float<\/span> targetY)<\/span> {<\/span>
    \n float<\/span> dx =<\/span> targetX –<\/span> robotX;<\/span>
    \n float<\/span> dy =<\/span> targetY –<\/span> robotY;<\/span>
    \n float<\/span> distance =<\/span> sqrt<\/span>(<\/span>dx*<\/span>dx +<\/span> dy*<\/span>dy)<\/span>;<\/span>
    \n if<\/span> (<\/span>distance <<\/span> 5.0<\/span>)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>0.0<\/span>,<\/span> 0.0<\/span>)<\/span>;<\/span> \/\/ \u5230\u8fbe\u76ee\u6807，\u505c\u6b62<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u5230\u8fbe\u76ee\u6807\u4f4d\u7f6e"<\/span>)<\/span>;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u8ba1\u7b97\u76ee\u6807\u822a\u5411<\/span>
    \n float<\/span> targetHeading =<\/span> degrees<\/span>(<\/span>atan2<\/span>(<\/span>dy,<\/span> dx)<\/span>)<\/span>;<\/span>
    \n float<\/span> headingDiff =<\/span> targetHeading –<\/span> headingAngle;<\/span>
    \n \/\/ \u822a\u5411\u89d2\u5f52\u4e00\u5316，\u907f\u514d\u6b63\u8d1f\u8df3\u53d8<\/span>
    \n if<\/span> (<\/span>headingDiff ><\/span> 180.0<\/span>)<\/span> headingDiff -=<\/span> 360.0<\/span>;<\/span>
    \n if<\/span> (<\/span>headingDiff <<\/span> –<\/span>180.0<\/span>)<\/span> headingDiff +=<\/span> 360.0<\/span>;<\/span><\/p>\n

    \/\/ \u524d\u65b9\u6709\u969c\u788d，\u4f18\u5148\u907f\u8ba9<\/span>
    \n float<\/span> irFront =<\/span> analogRead<\/span>(<\/span>IR_FRONT)<\/span> *<\/span> 0.1<\/span>;<\/span>
    \n if<\/span> (<\/span>irFront <<\/span> 100.0<\/span>)<\/span> {<\/span>
    \n \/\/ \u5de6\u8f6c\u907f\u969c<\/span>
    \n if<\/span> (<\/span>abs<\/span>(<\/span>headingDiff)<\/span> ><\/span> 0<\/span>)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>moveSpeed *<\/span> 0.5<\/span>,<\/span> moveSpeed *<\/span> 1.0<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u8c03\u6574\u822a\u5411\u5e76\u524d\u8fdb<\/span>
    \n if<\/span> (<\/span>abs<\/span>(<\/span>headingDiff)<\/span> ><\/span> 5.0<\/span>)<\/span> {<\/span>
    \n \/\/ \u822a\u5411\u504f\u5dee\u5927，\u5dee\u901f\u8f6c\u5411\u4fee\u6b63<\/span>
    \n if<\/span> (<\/span>headingDiff ><\/span> 0<\/span>)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>moveSpeed *<\/span> 0.7<\/span>,<\/span> moveSpeed)<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>moveSpeed,<\/span> moveSpeed *<\/span> 0.7<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n \/\/ \u822a\u5411\u6b63\u786e，\u76f4\u884c<\/span>
    \n driveMotor<\/span>(<\/span>moveSpeed,<\/span> moveSpeed)<\/span>;<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7535\u673a\u9a71\u52a8\u51fd\u6570<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>float<\/span> left,<\/span> float<\/span> right)<\/span> {<\/span>
    \n left =<\/span> constrain<\/span>(<\/span>left,<\/span> 0.0<\/span>,<\/span> 255.0<\/span>)<\/span>;<\/span>
    \n right =<\/span> constrain<\/span>(<\/span>right,<\/span> 0.0<\/span>,<\/span> 255.0<\/span>)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>DIR_LEFT,<\/span> HIGH)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_LEFT,<\/span> (<\/span>int<\/span>)<\/span>left)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>DIR_RIGHT,<\/span> HIGH)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_RIGHT,<\/span> (<\/span>int<\/span>)<\/span>right)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7f16\u7801\u5668\u6d4b\u8ddd（\u7b80\u5316\u51fd\u6570，\u5b9e\u9645\u9700\u6839\u636e\u7f16\u7801\u5668\u5b9e\u73b0）<\/span>
    \nfloat<\/span> getEncoderDistance<\/span>(<\/span>)<\/span> {<\/span>
    \n static<\/span> unsigned<\/span> long<\/span> prevLeft =<\/span> 0<\/span>,<\/span> prevRight =<\/span> 0<\/span>;<\/span>
    \n unsigned<\/span> long<\/span> leftCount =<\/span> readEncoder<\/span>(<\/span>LEFT_ENCODER)<\/span>;<\/span> \/\/ \u9700\u5b9e\u73b0\u7f16\u7801\u5668\u8bfb\u53d6\u51fd\u6570<\/span>
    \n unsigned<\/span> long<\/span> rightCount =<\/span> readEncoder<\/span>(<\/span>RIGHT_ENCODER)<\/span>;<\/span>
    \n long<\/span> leftDelta =<\/span> leftCount –<\/span> prevLeft;<\/span>
    \n long<\/span> rightDelta =<\/span> rightCount –<\/span> prevRight;<\/span>
    \n prevLeft =<\/span> leftCount;<\/span>
    \n prevRight =<\/span> rightCount;<\/span>
    \n \/\/ \u8ba1\u6570\u8f6c\u6362\u4e3a\u5b9e\u9645\u8ddd\u79bb（\u9700\u6309\u8f6e\u5f84\u6807\u5b9a）<\/span>
    \n return<\/span> (<\/span>leftDelta +<\/span> rightDelta)<\/span> *<\/span> 0.05<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \u9002\u7528\u573a\u666f\u4f18\u5316
    \n\u53ef\u589e\u52a0\u8fb9\u754c\u8ddf\u8e2a\u7b97\u6cd5，\u6cbf\u5899\u58c1\u884c\u9a76，\u63d0\u5347\u573a\u5730\u626b\u63cf\u7684\u5b8c\u6574\u6027
    \n\u4f18\u5316IMU\u6821\u51c6\u903b\u8f91，\u5b9a\u671f\u6839\u636e\u5df2\u77e5\u4f4d\u7f6e\u4fee\u6b63\u7d2f\u79ef\u8bef\u5dee，\u63d0\u5347\u957f\u671f\u5b9a\u4f4d\u7cbe\u5ea6
    \n\u6269\u5c55\u5730\u56fe\u5b58\u50a8，\u8bb0\u5f55\u63a2\u7d22\u8fc7\u7684\u5168\u5c40\u533a\u57df，\u652f\u6301\u591a\u8f6e\u6b21\u5bfc\u822a\u4efb\u52a1<\/p>\n

    5\u3001\u7ea2\u5916+IMU\u878d\u5408SLAM+\u52a8\u6001\u907f\u969c\u76d1\u63a7\u7a0b\u5e8f（\u6838\u5fc3：\u969c\u788d\u8bc6\u522b\u4e0e\u907f\u969c\u63a7\u5236）
    \n\u9002\u7528\u573a\u666f：\u5bb6\u5ead\u8d70\u5eca\u3001\u5bb6\u5177\u5bc6\u96c6\u7684\u5367\u5ba4\u7b49\u573a\u666f，\u673a\u5668\u4eba\u9700\u5b9e\u65f6\u76d1\u63a7\u969c\u788d\u7269，\u5b9e\u73b0\u52a8\u6001\u907f\u969c，\u907f\u514d\u78b0\u649e\u5bb6\u5177\u3001\u5ba0\u7269\u6216\u4eba\u5458\u3002<\/p>\n

    \u6838\u5fc3\u903b\u8f91
    \n\u878d\u5408\u591a\u8def\u7ea2\u5916\u4f20\u611f\u5668，\u5b9e\u73b0\u5168\u65b9\u4f4d\u969c\u788d\u8bc6\u522b
    \n\u57fa\u4e8eIMU\u5b9a\u4f4d\u4e0e\u7ea2\u5916\u6570\u636e，\u5224\u65ad\u969c\u788d\u7c7b\u578b\u4e0e\u907f\u969c\u8def\u5f84
    \n\u5b9e\u65f6\u8c03\u6574\u884c\u9a76\u65b9\u5411，\u5b9e\u73b0\u7ed5\u969c\u3001\u8ddf\u5899\u7b49\u52a8\u6001\u907f\u969c<\/p>\n

    \/\/ \u7ea2\u5916+IMU\u878d\u5408SLAM+\u52a8\u6001\u907f\u969c\u76d1\u63a7\u7a0b\u5e8f<\/span>
    \n\/\/ \u786c\u4ef6：5\u8def\u7ea2\u5916\u6d4b\u8ddd（\u524d+\u5de6\u53f3\u524d+\u540e）+MPU6050+2\u8f6eBLDC<\/span>
    \n\/\/ \u529f\u80fd：\u5bb6\u5177\u5bc6\u96c6\u533a\u57df\u52a8\u6001\u907f\u969c，\u76d1\u63a7\u73af\u5883\u53d8\u5316，\u5b89\u5168\u5bfc\u822a<\/span><\/p>\n

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

    \/\/ ========== \u786c\u4ef6\u5b9a\u4e49 ==========<\/span>
    \nconst<\/span> byte IR_F =<\/span> A0,<\/span> IR_FL =<\/span> A1,<\/span> IR_FR =<\/span> A2,<\/span> IR_L =<\/span> A3,<\/span> IR_R =<\/span> A4;<\/span> \/\/ 5\u8def\u7ea2\u5916<\/span>
    \nconst<\/span> byte PWM_L =<\/span> 6<\/span>,<\/span> DIR_L =<\/span> 7<\/span>,<\/span> PWM_R =<\/span> 8<\/span>,<\/span> DIR_R =<\/span> 9<\/span>;<\/span><\/p>\n

    \/\/ ========== \u907f\u969c\u4e0eSLAM\u53c2\u6570 ==========<\/span>
    \nfloat<\/span> robotX =<\/span> 0.0<\/span>,<\/span> robotY =<\/span> 0.0<\/span>,<\/span> headingAngle =<\/span> 0.0<\/span>;<\/span>
    \nconst<\/span> float<\/span> SAFE_DIST =<\/span> 80.0<\/span>;<\/span> \/\/ \u5b89\u5168\u8ddd\u79bb（cm）<\/span>
    \nconst<\/span> float<\/span> MIN_DIST =<\/span> 30.0<\/span>;<\/span> \/\/ \u6700\u5c0f\u907f\u969c\u8ddd\u79bb（cm）<\/span>
    \nconst<\/span> float<\/span> BASE_SPEED =<\/span> 25.0<\/span>;<\/span> \/\/ \u57fa\u7840\u901f\u5ea6<\/span>
    \nfloat<\/span> obstacleInfo[<\/span>5<\/span>]<\/span> =<\/span> {<\/span>0<\/span>}<\/span>;<\/span> \/\/ \u5b58\u50a8\u5404\u7ea2\u5916\u68c0\u6d4b\u8ddd\u79bb<\/span><\/p>\n

    \/\/ ========== \u51fd\u6570\u58f0\u660e ==========<\/span>
    \nvoid<\/span> readIMU<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> scanAllInfrared<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span>\u5224\u65ad\u907f\u969cStrategy<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> updatePosition<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>float<\/span> l,<\/span> float<\/span> r)<\/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>"\u52a8\u6001\u907f\u969cSLAM\u7cfb\u7edf\u521d\u59cb\u5316\u5b8c\u6210"<\/span>)<\/span>;<\/span><\/p>\n

    Wire.<\/span>begin<\/span>(<\/span>)<\/span>;<\/span>
    \n mpu.<\/span>begin<\/span>(<\/span>MPU6050_SCALE_2000DPS)<\/span>;<\/span><\/p>\n

    for<\/span> (<\/span>byte i=<\/span>0<\/span>;<\/span> i<<\/span>5<\/span>;<\/span> i++<\/span>)<\/span> {<\/span>
    \n pinMode<\/span>(<\/span>IR_F+<\/span>i,<\/span> INPUT)<\/span>;<\/span>
    \n }<\/span>
    \n pinMode<\/span>(<\/span>PWM_L,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>DIR_L,<\/span> OUTPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>PWM_R,<\/span> OUTPUT)<\/span>;<\/span> pinMode<\/span>(<\/span>DIR_R,<\/span> OUTPUT)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
    \n readIMU<\/span>(<\/span>)<\/span>;<\/span> \/\/ 1. \u8bfb\u53d6\u59ff\u6001<\/span>
    \n scanAllInfrared<\/span>(<\/span>)<\/span>;<\/span> \/\/ 2. \u591a\u8def\u7ea2\u5916\u626b\u63cf<\/span>
    \n judge\u907f\u969cStrategy<\/span>(<\/span>)<\/span>;<\/span> \/\/ 3. \u5236\u5b9a\u907f\u969c\u7b56\u7565<\/span>
    \n updatePosition<\/span>(<\/span>)<\/span>;<\/span> \/\/ 4. \u66f4\u65b0\u5b9a\u4f4d<\/span>
    \n delay<\/span>(<\/span>15<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u8bfb\u53d6IMU，\u66f4\u65b0\u822a\u5411\u89d2<\/span>
    \nvoid<\/span> readIMU<\/span>(<\/span>)<\/span> {<\/span>
    \n mpu.<\/span>update<\/span>(<\/span>)<\/span>;<\/span>
    \n float<\/span> gyroZ =<\/span> mpu.<\/span>getGyroZ<\/span>(<\/span>)<\/span>;<\/span>
    \n headingAngle +=<\/span> gyroZ *<\/span> 0.01<\/span>;<\/span>
    \n headingAngle =<\/span> fmodf<\/span>(<\/span>headingAngle,<\/span> 360.0<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>headingAngle <<\/span> 0<\/span>)<\/span> headingAngle +=<\/span> 360.0<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u91c7\u96c65\u8def\u7ea2\u5916\u8ddd\u79bb<\/span>
    \nvoid<\/span> scanAllInfrared<\/span>(<\/span>)<\/span> {<\/span>
    \n obstacleInfo[<\/span>0<\/span>]<\/span> =<\/span> analogRead<\/span>(<\/span>IR_F)<\/span> *<\/span> 0.1<\/span>;<\/span> \/\/ \u524d\u65b9<\/span>
    \n obstacleInfo[<\/span>1<\/span>]<\/span> =<\/span> analogRead<\/span>(<\/span>IR_FL)<\/span> *<\/span> 0.1<\/span>;<\/span> \/\/ \u5de6\u524d<\/span>
    \n obstacleInfo[<\/span>2<\/span>]<\/span> =<\/span> analogRead<\/span>(<\/span>IR_FR)<\/span> *<\/span> 0.1<\/span>;<\/span> \/\/ \u53f3\u524d<\/span>
    \n obstacleInfo[<\/span>3<\/span>]<\/span> =<\/span> analogRead<\/span>(<\/span>IR_L)<\/span> *<\/span> 0.1<\/span>;<\/span> \/\/ \u5de6\u4fa7<\/span>
    \n obstacleInfo[<\/span>4<\/span>]<\/span> =<\/span> analogRead<\/span>(<\/span>IR_R)<\/span> *<\/span> 0.1<\/span>;<\/span> \/\/ \u53f3\u4fa7<\/span><\/p>\n

    \/\/ \u5f02\u5e38\u503c\u8fc7\u6ee4，\u907f\u514d\u5e72\u6270<\/span>
    \n for<\/span> (<\/span>byte i=<\/span>0<\/span>;<\/span> i<<\/span>5<\/span>;<\/span> i++<\/span>)<\/span> {<\/span>
    \n if<\/span> (<\/span>obstacleInfo[<\/span>i]<\/span> <<\/span> 10.0<\/span> ||<\/span> obstacleInfo[<\/span>i]<\/span> ><\/span> 300.0<\/span>)<\/span> {<\/span>
    \n obstacleInfo[<\/span>i]<\/span> =<\/span> 300.0<\/span>;<\/span> \/\/ \u8d85\u5927\u503c\u89c6\u4e3a\u65e0\u969c\u788d<\/span>
    \n }<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u5236\u5b9a\u907f\u969c\u7b56\u7565<\/span>
    \nvoid<\/span> judge\u907f\u969cStrategy<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> frontDist =<\/span> obstacleInfo[<\/span>0<\/span>]<\/span>;<\/span>
    \n float<\/span> leftDist =<\/span> obstacleInfo[<\/span>3<\/span>]<\/span>;<\/span>
    \n float<\/span> rightDist =<\/span> obstacleInfo[<\/span>4<\/span>]<\/span>;<\/span>
    \n float<\/span> leftFrontDist =<\/span> obstacleInfo[<\/span>1<\/span>]<\/span>;<\/span>
    \n float<\/span> rightFrontDist =<\/span> obstacleInfo[<\/span>2<\/span>]<\/span>;<\/span><\/p>\n

    \/\/ \u524d\u65b9\u65e0\u969c\u788d，\u6b63\u5e38\u524d\u8fdb<\/span>
    \n if<\/span> (<\/span>frontDist ><\/span> SAFE_DIST)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>BASE_SPEED,<\/span> BASE_SPEED)<\/span>;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u524d\u65b9\u969c\u788d\u8ddd\u79bb\u9002\u4e2d，\u5224\u65ad\u7ed5\u969c\u65b9\u5411<\/span>
    \n if<\/span> (<\/span>frontDist ><\/span> MIN_DIST)<\/span> {<\/span>
    \n \/\/ \u5de6\u4fa7\u7a7a\u95f4\u5145\u8db3，\u5de6\u4fa7\u7ed5\u969c<\/span>
    \n if<\/span> (<\/span>leftFrontDist ><\/span> frontDist +<\/span> 50.0<\/span> &&<\/span> leftDist ><\/span> SAFE_DIST)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>BASE_SPEED *<\/span> 0.6<\/span>,<\/span> BASE_SPEED)<\/span>;<\/span> \/\/ \u5de6\u8f6c\u7ed5\u969c<\/span>
    \n }<\/span>
    \n \/\/ \u53f3\u4fa7\u7a7a\u95f4\u5145\u8db3，\u53f3\u4fa7\u7ed5\u969c<\/span>
    \n else<\/span> if<\/span> (<\/span>rightFrontDist ><\/span> frontDist +<\/span> 50.0<\/span> &&<\/span> rightDist ><\/span> SAFE_DIST)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>BASE_SPEED,<\/span> BASE_SPEED *<\/span> 0.6<\/span>)<\/span>;<\/span> \/\/ \u53f3\u8f6c\u7ed5\u969c<\/span>
    \n }<\/span>
    \n \/\/ \u5de6\u53f3\u7a7a\u95f4\u5747\u4e0d\u8db3，\u51cf\u901f\u76f4\u884c\u5bfb\u627e\u907f\u969c\u7a7a\u95f4<\/span>
    \nelse<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>BASE_SPEED *<\/span> 0.3<\/span>,<\/span> BASE_SPEED *<\/span> 0.3<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u540e\u65b9\u65e0\u969c\u788d，\u5012\u8f66\u907f\u8ba9（\u6781\u7aef\u60c5\u51b5）<\/span>
    \n float<\/span> rearDist =<\/span> getRearDistance<\/span>(<\/span>)<\/span>;<\/span> \/\/ \u9700\u8865\u5145\u540e\u65b9\u7ea2\u5916\u6216\u4f30\u7b97<\/span>
    \n if<\/span> (<\/span>rearDist ><\/span> SAFE_DIST)<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>–<\/span>BASE_SPEED *<\/span> 0.8<\/span>,<\/span> –<\/span>BASE_SPEED *<\/span> 0.8<\/span>)<\/span>;<\/span> \/\/ \u5012\u8f66<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u5168\u5411\u7a7a\u95f4\u4e0d\u8db3，\u7d27\u6025\u505c\u6b62<\/span>
    \n driveMotor<\/span>(<\/span>0.0<\/span>,<\/span> 0.0<\/span>)<\/span>;<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u8b66\u544a：\u5168\u5411\u969c\u788d，\u7d27\u6025\u505c\u6b62"<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u66f4\u65b0\u5b9a\u4f4d（\u7b80\u5316\u903b\u8f91，\u540c\u6848\u4f8b1）<\/span>
    \nvoid<\/span> updatePosition<\/span>(<\/span>)<\/span> {<\/span>
    \n float<\/span> deltaHeading =<\/span> headingAngle –<\/span> lastHeading;<\/span>
    \n float<\/span> distance =<\/span> getEncoderDistance<\/span>(<\/span>)<\/span>;<\/span>
    \n robotX +=<\/span> distance *<\/span> cos<\/span>(<\/span>radians<\/span>(<\/span>headingAngle)<\/span>)<\/span>;<\/span>
    \n robotY +=<\/span> distance *<\/span> sin<\/span>(<\/span>radians<\/span>(<\/span>headingAngle)<\/span>)<\/span>;<\/span>
    \n lastHeading =<\/span> headingAngle;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7535\u673a\u9a71\u52a8<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>float<\/span> l,<\/span> float<\/span> r)<\/span> {<\/span>
    \n l =<\/span> constrain<\/span>(<\/span>l,<\/span> –<\/span>255.0<\/span>,<\/span> 255.0<\/span>)<\/span>;<\/span>
    \n r =<\/span> constrain<\/span>(<\/span>r,<\/span> –<\/span>255.0<\/span>,<\/span> 255.0<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>l >=<\/span>0<\/span>)<\/span> {<\/span> digitalWrite<\/span>(<\/span>DIR_L,<\/span> HIGH)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_L,<\/span> (<\/span>int<\/span>)<\/span>l)<\/span>;<\/span> }<\/span>
    \n else<\/span> {<\/span> digitalWrite<\/span>(<\/span>DIR_L,<\/span> LOW)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_L,<\/span> (<\/span>int<\/span>)<\/span>abs<\/span>(<\/span>l)<\/span>)<\/span>;<\/span>}<\/span>
    \n if<\/span> (<\/span>r >=<\/span>0<\/span>)<\/span> {<\/span> digitalWrite<\/span>(<\/span>DIR_R,<\/span> HIGH)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_R,<\/span> (<\/span>int<\/span>)<\/span>r)<\/span>;<\/span> }<\/span>
    \n else<\/span> {<\/span> digitalWrite<\/span>(<\/span>DIR_R,<\/span> LOW)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_R,<\/span> (<\/span>int<\/span>)<\/span>abs<\/span>(<\/span>r)<\/span>)<\/span>;<\/span> }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u83b7\u53d6\u540e\u65b9\u8ddd\u79bb（\u7b80\u5316，\u53ef\u8865\u5145\u7ea2\u5916\u6216\u4f30\u7b97）<\/span>
    \nfloat<\/span> getRearDistance<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u65e0\u540e\u65b9\u7ea2\u5916\u65f6，\u57fa\u4e8e\u5386\u53f2\u5b9a\u4f4d\u4f30\u7b97，\u6b64\u5904\u8fd4\u56de\u5b89\u5168\u8ddd\u79bb<\/span>
    \n return<\/span> 200.0<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7f16\u7801\u5668\u6d4b\u8ddd\u51fd\u6570（\u540c\u6848\u4f8b1，\u9700\u6309\u5b9e\u9645\u5b9e\u73b0）<\/span>
    \nfloat<\/span> getEncoderDistance<\/span>(<\/span>)<\/span> {<\/span>
    \n return<\/span> 2.0<\/span> \/<\/span> 10.0<\/span>;<\/span> \/\/ \u6309\u5b9e\u9645\u8f6e\u5f84\u4e0e\u91c7\u6837\u5468\u671f\u8ba1\u7b97<\/span>
    \n}<\/span><\/p>\n

    \u9002\u7528\u573a\u666f\u4f18\u5316
    \n\u52a0\u5165\u52a8\u6001\u969c\u788d\u8bc6\u522b，\u533a\u5206\u56fa\u5b9a\u5bb6\u5177\u4e0e\u79fb\u52a8\u7269\u4f53（\u5982\u5ba0\u7269\u3001\u4eba），\u63d0\u5347\u907f\u969c\u9488\u5bf9\u6027
    \n\u4f18\u5316\u907f\u969c\u8def\u5f84\u89c4\u5212，\u7ed3\u5408SLAM\u5730\u56fe\u9009\u62e9\u6700\u4f18\u7ed5\u969c\u8def\u7ebf，\u51cf\u5c11\u7ed5\u884c\u8ddd\u79bb
    \n\u589e\u52a0\u901f\u5ea6\u81ea\u9002\u5e94，\u969c\u788d\u8d8a\u8fd1\u901f\u5ea6\u8d8a\u4f4e，\u63d0\u5347\u5b89\u5168\u6027<\/p>\n

    6\u3001\u7ea2\u5916+IMU\u878d\u5408SLAM+\u5bfc\u822a\u76d1\u63a7\u4e0e\u5f02\u5e38\u4fdd\u62a4\u7a0b\u5e8f（\u6838\u5fc3：\u5b89\u5168\u8fd0\u884c\u4e0e\u72b6\u6001\u76d1\u63a7）
    \n\u9002\u7528\u573a\u666f：\u5bb6\u5ead\u5168\u573a\u666f\u670d\u52a1，\u673a\u5668\u4eba\u9700\u5b9e\u65f6\u76d1\u63a7\u81ea\u8eab\u72b6\u6001\u3001\u5b9a\u4f4d\u7cbe\u5ea6\u4e0e\u8fd0\u884c\u73af\u5883，\u5f02\u5e38\u65f6\u5feb\u901f\u54cd\u5e94，\u4fdd\u969c\u8bbe\u5907\u4e0e\u5c45\u5bb6\u5b89\u5168\u3002<\/p>\n

    \u6838\u5fc3\u903b\u8f91
    \n\u76d1\u63a7SLAM\u5b9a\u4f4d\u72b6\u6001\u3001\u4f20\u611f\u5668\u5065\u5eb7\u4e0e\u7535\u673a\u8fd0\u884c\u72b6\u6001
    \n\u68c0\u6d4b\u5b9a\u4f4d\u6f02\u79fb\u3001\u4f20\u611f\u5668\u6545\u969c\u3001\u7535\u673a\u5f02\u5e38\u7b49\u98ce\u9669
    \n\u5f02\u5e38\u65f6\u89e6\u53d1\u4fdd\u62a4\u673a\u5236，\u63a8\u9001\u544a\u8b66，\u786e\u4fdd\u5b89\u5168\u505c\u673a\u6216\u8c03\u6574\u8fd0\u884c<\/p>\n

    \/\/ \u7ea2\u5916+IMU\u878d\u5408SLAM+\u5bfc\u822a\u76d1\u63a7\u4e0e\u5f02\u5e38\u4fdd\u62a4\u7a0b\u5e8f<\/span>
    \n\/\/ \u786c\u4ef6：\u7ea2\u5916+IMU+BLDC+\u8702\u9e23\u5668+\u72b6\u6001\u6307\u793a\u706f<\/span>
    \n\/\/ \u529f\u80fd：\u5168\u573a\u666f\u8fd0\u884c\u76d1\u63a7，\u5f02\u5e38\u4fdd\u62a4，\u4fdd\u969c\u670d\u52a1\u5b89\u5168<\/span><\/p>\n

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

    \/\/ ========== \u76d1\u63a7\u4e0e\u4fdd\u62a4\u53c2\u6570 ==========<\/span>
    \nconst<\/span> byte BUZZER =<\/span> 10<\/span>;<\/span> \/\/ \u5f02\u5e38\u8702\u9e23\u5668<\/span>
    \nconst<\/span> byte LED_OK =<\/span> 11<\/span>;<\/span> \/\/ \u6b63\u5e38\u8fd0\u884c\u6307\u793a\u706f<\/span>
    \nconst<\/span> byte LED_ERR =<\/span> 12<\/span>;<\/span> \/\/ \u5f02\u5e38\u6307\u793a\u706f<\/span><\/p>\n

    float<\/span> robotX =<\/span> 0.0<\/span>,<\/span> robotY =<\/span> 0.0<\/span>,<\/span> headingAngle =<\/span> 0.0<\/span>;<\/span>
    \nfloat<\/span> lastX =<\/span> 0.0<\/span>,<\/span> lastY =<\/span> 0.0<\/span>;<\/span>\/\/ \u8f85\u52a9\u5224\u65ad\u5b9a\u4f4d\u5f02\u5e38<\/span>
    \nbool<\/span> imuError =<\/span> false<\/span>,<\/span> irError =<\/span> false<\/span>,<\/span> motorError =<\/span> false<\/span>;<\/span>
    \nbool<\/span> shutdown =<\/span> false<\/span>;<\/span><\/p>\n

    \/\/ \u76d1\u63a7\u9608\u503c<\/span>
    \nconst<\/span> float<\/span> POSITION_DRIFT =<\/span> 50.0<\/span>;<\/span> \/\/ \u5b9a\u4f4d\u6f02\u79fb\u9608\u503c（cm）<\/span>
    \nconst<\/span> float<\/span> BATTERY_LOW =<\/span> 20.0<\/span>;<\/span> \/\/ \u4f4e\u7535\u91cf\u9608\u503c（%）（\u9700\u63a5\u5165\u7535\u538b\u91c7\u96c6）<\/span>
    \nunsigned<\/span> long<\/span> errorStartTime =<\/span> 0<\/span>;<\/span><\/p>\n

    \/\/ ========== \u51fd\u6570\u58f0\u660e ==========<\/span>
    \nvoid<\/span> monitorIMU<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> monitorInfrared<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> monitorMotor<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> checkProtect<\/span>(<\/span>)<\/span>;<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>float<\/span> l,<\/span> float<\/span> r)<\/span>;<\/span>
    \nvoid<\/span> triggerFault<\/span>(<\/span>)<\/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>"\u5bfc\u822a\u76d1\u63a7\u4e0e\u5f02\u5e38\u4fdd\u62a4\u7cfb\u7edf\u521d\u59cb\u5316\u5b8c\u6210"<\/span>)<\/span>;<\/span><\/p>\n

    pinMode<\/span>(<\/span>BUZZER,<\/span> OUTPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>LED_OK,<\/span> OUTPUT)<\/span>;<\/span>
    \n pinMode<\/span>(<\/span>LED_ERR,<\/span> OUTPUT)<\/span>;<\/span><\/p>\n

    Wire.<\/span>begin<\/span>(<\/span>)<\/span>;<\/span>
    \n mpu.<\/span>begin<\/span>(<\/span>MPU6050_SCALE_2000DPS)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    void<\/span> loop<\/span>(<\/span>)<\/span> {<\/span>
    \n monitorIMU<\/span>(<\/span>)<\/span>;<\/span> \/\/ 1. \u76d1\u63a7IMU\u72b6\u6001<\/span>
    \n monitorInfrared<\/span>(<\/span>)<\/span>;<\/span> \/\/ 2. \u76d1\u63a7\u7ea2\u5916\u4f20\u611f\u5668<\/span>
    \n monitorMotor<\/span>(<\/span>)<\/span>;<\/span> \/\/ 3. \u76d1\u63a7\u7535\u673a\u72b6\u6001<\/span>
    \n checkProtect<\/span>(<\/span>)<\/span>;<\/span> \/\/ 4. \u5f02\u5e38\u4fdd\u62a4\u5224\u65ad<\/span>
    \n \/\/ \u6b63\u5e38\u65f6\u6267\u884c\u57fa\u7840\u5bfc\u822a<\/span>
    \n if<\/span> (<\/span>!<\/span>shutdown)<\/span> {<\/span>
    \n basicNavigate<\/span>(<\/span>)<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n driveMotor<\/span>(<\/span>0.0<\/span>,<\/span> 0.0<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n delay<\/span>(<\/span>20<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u76d1\u63a7IMU\u5065\u5eb7\u72b6\u6001<\/span>
    \nvoid<\/span> monitorIMU<\/span>(<\/span>)<\/span> {<\/span>
    \n bool<\/span> currentImuOk =<\/span> mpu.<\/span>update<\/span>(<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>!<\/span>currentImuOk)<\/span> {<\/span>
    \n imuError =<\/span> true<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n \/\/ \u68c0\u6d4b\u822a\u5411\u89d2\u8df3\u53d8，\u5224\u65adIMU\u6570\u636e\u5f02\u5e38<\/span>
    \n float<\/span> currentHeading =<\/span> mpu.<\/span>getGyroZ<\/span>(<\/span>)<\/span> *<\/span> 0.01<\/span>;<\/span>
    \n if<\/span> (<\/span>abs<\/span>(<\/span>currentHeading –<\/span> headingAngle)<\/span> ><\/span> 180.0<\/span>)<\/span> {<\/span>
    \n imuError =<\/span> true<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n imuError =<\/span> false<\/span>;<\/span>
    \n }<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u76d1\u63a7\u7ea2\u5916\u4f20\u611f\u5668\u72b6\u6001<\/span>
    \nvoid<\/span> monitorInfrared<\/span>(<\/span>)<\/span> {<\/span>
    \n int<\/span> irFront =<\/span> analogRead<\/span>(<\/span>IR_F)<\/span>;<\/span>
    \n \/\/ \u4f20\u611f\u5668\u5f02\u5e38（\u8f93\u51fa\u56fa\u5b9a\u503c\u6216\u65ad\u7ebf）\u5224\u65ad<\/span>
    \n if<\/span> (<\/span>irFront ==<\/span> 0<\/span> ||<\/span> irFront ==<\/span> 1023<\/span>)<\/span> {<\/span>
    \n irError =<\/span> true<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n irError =<\/span> false<\/span>;<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u76d1\u63a7\u7535\u673a\u8fd0\u884c\u72b6\u6001<\/span>
    \nvoid<\/span> monitorMotor<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u68c0\u6d4b\u7535\u673a\u5835\u8f6c：\u7535\u6d41\u8fc7\u5927\u4e14\u8f6c\u901f\u4e3a\u96f6（\u9700\u63a5\u5165\u7535\u6d41\u91c7\u96c6，\u6b64\u5904\u7b80\u5316）<\/span>
    \n if<\/span> (<\/span>getMotorCurrent<\/span>(<\/span>)<\/span> ><\/span> 800<\/span>)<\/span> {<\/span>
    \n motorError =<\/span> true<\/span>;<\/span>
    \n }<\/span> else<\/span> {<\/span>
    \n motorError =<\/span> false<\/span>;<\/span>}<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u5f02\u5e38\u4fdd\u62a4\u5224\u65ad\u4e0e\u6267\u884c<\/span>
    \nvoid<\/span> checkProtect<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u5b9a\u4f4d\u6f02\u79fb\u68c0\u6d4b：\u65e0\u4f4d\u79fb\u4f46\u822a\u5411\u53d8\u5316\u8fc7\u5927，\u5224\u65ad\u4e3a\u5b9a\u4f4d\u5f02\u5e38<\/span>
    \n float<\/span> dist =<\/span> sqrtf<\/span>(<\/span>(<\/span>robotX–<\/span>lastX)<\/span>*<\/span>(<\/span>robotX–<\/span>lastX)<\/span> +<\/span> (<\/span>robotY–<\/span>lastY)<\/span>*<\/span>(<\/span>robotY–<\/span>lastY)<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>dist <<\/span> 1.0<\/span> &&<\/span> getHeadingChange<\/span>(<\/span>)<\/span> ><\/span> 20.0<\/span>)<\/span> {<\/span>
    \n errorStartTime =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
    \n }<\/span><\/p>\n

    float<\/span> driftTime =<\/span> millis<\/span>(<\/span>)<\/span> –<\/span> errorStartTime;<\/span><\/p>\n

    \/\/ \u4efb\u4e00\u5f02\u5e38\u89e6\u53d1\u7acb\u5373\u4fdd\u62a4<\/span>
    \n if<\/span> (<\/span>imuError ||<\/span> irError ||<\/span> motorError)<\/span> {<\/span>
    \n triggerFault<\/span>(<\/span>)<\/span>;<\/span>
    \n shutdown =<\/span> true<\/span>;<\/span>
    \n return<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u5b9a\u4f4d\u6f02\u79fb\u8d85\u65f6，\u89e6\u53d1\u544a\u8b66\u5e76\u6682\u505c<\/span>
    \n if<\/span> (<\/span>driftTime ><\/span> 1000.0<\/span>)<\/span> {<\/span>
    \n triggerFault<\/span>(<\/span>)<\/span>;<\/span>
    \n shutdown =<\/span> true<\/span>;<\/span>
    \n }<\/span><\/p>\n

    \/\/ \u4f4e\u7535\u91cf\u4fdd\u62a4<\/span>
    \n float<\/span> batteryVoltage =<\/span> getBatteryVoltage<\/span>(<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>batteryVoltage <<\/span> BATTERY_LOW)<\/span> {<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u8b66\u544a：\u4f4e\u7535\u91cf，\u9700\u5c3d\u5feb\u8fd4\u822a"<\/span>)<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>–<\/span>BASE_SPEED *<\/span> 0.5<\/span>,<\/span> –<\/span>BASE_SPEED *<\/span> 0.5<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u89e6\u53d1\u5f02\u5e38\u544a\u8b66<\/span>
    \nvoid<\/span> triggerFault<\/span>(<\/span>)<\/span> {<\/span>
    \n digitalWrite<\/span>(<\/span>LED_ERR,<\/span> HIGH)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>LED_OK,<\/span> LOW)<\/span>;<\/span>
    \n \/\/ \u8702\u9e23\u5668\u95f4\u6b47\u9e23\u53eb<\/span>
    \n for<\/span> (<\/span>byte i=<\/span>0<\/span>;<\/span> i<<\/span>3<\/span>;<\/span> i++<\/span>)<\/span> {<\/span>
    \n digitalWrite<\/span>(<\/span>BUZZER,<\/span> HIGH)<\/span>;<\/span>
    \n delay<\/span>(<\/span>150<\/span>)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>BUZZER,<\/span> LOW)<\/span>;<\/span>
    \n delay<\/span>(<\/span>150<\/span>)<\/span>;<\/span>
    \n }<\/span>
    \n Serial.<\/span>println<\/span>(<\/span>"\u7cfb\u7edf\u5f02\u5e38，\u89e6\u53d1\u4fdd\u62a4！"<\/span>)<\/span>;<\/span>
    \n errorStartTime =<\/span> millis<\/span>(<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u57fa\u7840\u5bfc\u822a（\u7b80\u5316，\u540c\u6848\u4f8b1）<\/span>
    \nvoid<\/span> basicNavigate<\/span>(<\/span>)<\/span> {<\/span>
    \n digitalWrite<\/span>(<\/span>LED_OK,<\/span> HIGH)<\/span>;<\/span>
    \n digitalWrite<\/span>(<\/span>LED_ERR,<\/span> LOW)<\/span>;<\/span>
    \n driveMotor<\/span>(<\/span>25.0<\/span>,<\/span> 25.0<\/span>)<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u7535\u673a\u9a71\u52a8<\/span>
    \nvoid<\/span> driveMotor<\/span>(<\/span>float<\/span> l,<\/span> float<\/span> r)<\/span> {<\/span>
    \n l =<\/span> constrain<\/span>(<\/span>l,<\/span> –<\/span>255.0<\/span>,<\/span> 255.0<\/span>)<\/span>;<\/span>
    \n r =<\/span> constrain<\/span>(<\/span>r,<\/span> –<\/span>255.0<\/span>,<\/span> 255.0<\/span>)<\/span>;<\/span>
    \n if<\/span> (<\/span>l >=<\/span>0<\/span>)<\/span> {<\/span> digitalWrite<\/span>(<\/span>PWM_L,<\/span> HIGH)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_L,<\/span> (<\/span>int<\/span>)<\/span>l)<\/span>;<\/span>}<\/span>
    \n else<\/span> {<\/span> digitalWrite<\/span>(<\/span>PWM_L,<\/span> LOW)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_L,<\/span> (<\/span>int<\/span>)<\/span>abs<\/span>(<\/span>l)<\/span>)<\/span>;<\/span> }<\/span>
    \n if<\/span> (<\/span>r >=<\/span>0<\/span>)<\/span> {<\/span> digitalWrite<\/span>(<\/span>PWM_R,<\/span> HIGH)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_R,<\/span> (<\/span>int<\/span>)<\/span>r)<\/span>;<\/span>}<\/span>
    \n else<\/span> {<\/span> digitalWrite<\/span>(<\/span>PWM_R,<\/span> LOW)<\/span>;<\/span> analogWrite<\/span>(<\/span>PWM_R,<\/span> (<\/span>int<\/span>)<\/span>abs<\/span>(<\/span>r)<\/span>)<\/span>;<\/span> }<\/span>
    \n}<\/span><\/p>\n

    \/\/ \u8f85\u52a9\u51fd\u6570：\u83b7\u53d6\u822a\u5411\u53d8\u5316\u91cf<\/span>
    \nfloat<\/span> getHeadingChange<\/span>(<\/span>)<\/span> {<\/span>
    \n return<\/span> headingAngle –<\/span> lastHeadingAngle;<\/span>
    \n}<\/span>
    \nfloat<\/span> getMotorCurrent<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u9700\u63a5\u5165\u7535\u6d41\u91c7\u6837\u7535\u8def，\u6b64\u5904\u8fd4\u56de\u6a21\u62df\u503c<\/span>
    \n return<\/span> analogRead<\/span>(<\/span>A5)<\/span>;<\/span>
    \n}<\/span>
    \nfloat<\/span> getBatteryVoltage<\/span>(<\/span>)<\/span> {<\/span>
    \n \/\/ \u9700\u63a5\u5165\u7535\u538b\u91c7\u96c6，\u6b64\u5904\u8fd4\u56de\u6a21\u62df\u503c<\/span>
    \n return<\/span> analogRead<\/span>(<\/span>A6)<\/span> *<\/span> 0.03<\/span>;<\/span>
    \n}<\/span><\/p>\n

    \u9002\u7528\u573a\u666f\u4f18\u5316
    \n\u589e\u52a0\u8fdc\u7a0b\u72b6\u6001\u4e0a\u62a5\u529f\u80fd，\u5f02\u5e38\u65f6\u5411\u7528\u6237\u63a8\u9001\u544a\u8b66，\u63d0\u5347\u8fd0\u7ef4\u6548\u7387
    \n\u4f18\u5316\u5f02\u5e38\u6062\u590d\u903b\u8f91，\u4f20\u611f\u5668\u77ed\u6682\u5f02\u5e38\u540e\u81ea\u52a8\u590d\u4f4d，\u907f\u514d\u9891\u7e41\u505c\u673a
    \n\u52a0\u5165\u673a\u5668\u4eba\u59ff\u6001\u4fdd\u62a4，\u68c0\u6d4b\u5230\u503e\u7ffb\u98ce\u9669\u65f6\u81ea\u52a8\u8c03\u6574\u8f6e\u7aef\u529b\u77e9，\u9632\u6b62\u6454\u5012<\/p>\n

    \u8981\u70b9\u89e3\u8bfb
    \n\u8981\u70b91：\u7ea2\u5916+IMU\u878d\u5408\u662f\u8f7b\u91cfSLAM\u7684\u6838\u5fc3，\u4e92\u8865\u4f18\u52bf\u63d0\u5347\u7cbe\u5ea6
    \n\u7ea2\u5916\u4e0eIMU\u7684\u7279\u6027\u4e92\u8865，\u662f\u4f4e\u6210\u672c\u5b9e\u73b0SLAM\u7684\u5173\u952e\u3002
    \n\u529f\u80fd\u4e92\u8865：\u7ea2\u5916\u6d4b\u8ddd\u64c5\u957f\u68c0\u6d4b\u8fd1\u8ddd\u79bb\u969c\u788d\u3001\u73af\u5883\u8fb9\u754c，\u6210\u672c\u4f4e\u3001\u54cd\u5e94\u5feb，\u4f46\u5b58\u5728\u68c0\u6d4b\u76f2\u533a；IMU\u53ef\u83b7\u53d6\u822a\u5411\u3001\u59ff\u6001\u6570\u636e，\u5b9e\u73b0\u9640\u87ba\u4eea\u6d4b\u89d2\u4e0e\u52a0\u901f\u5ea6\u8ba1\u6d4b\u901f，\u5f25\u8865\u7ea2\u5916\u65e0\u6cd5\u68c0\u6d4b\u65b9\u5411\u3001\u52a8\u6001\u89d2\u901f\u5ea6\u7684\u4e0d\u8db3
    \n\u6f02\u79fb\u8865\u507f：\u5355\u72ec\u4f7f\u7528IMU\u5b58\u5728\u7d2f\u79ef\u6f02\u79fb\u95ee\u9898，\u7ed3\u5408\u7ea2\u5916\u68c0\u6d4b\u7684\u8fb9\u754c\u3001\u969c\u788d\u7279\u5f81，\u53ef\u5b9a\u671f\u6821\u51c6\u5b9a\u4f4d，\u4fee\u6b63\u957f\u8ddd\u79bb\u884c\u9a76\u7684\u7d2f\u79ef\u8bef\u5dee，\u63d0\u5347\u957f\u671f\u5b9a\u4f4d\u7cbe\u5ea6
    \n\u4f4e\u6210\u672c\u9002\u914d：\u4e24\u7c7b\u5668\u4ef6\u4f53\u79ef\u5c0f\u3001\u529f\u8017\u4f4e\u3001\u4ef7\u683c\u4eb2\u6c11，\u9002\u914d\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u7684\u8f7b\u91cf\u5316\u3001\u4f4e\u6210\u672c\u9700\u6c42，\u65e0\u9700\u4f9d\u8d56\u6fc0\u5149\u96f7\u8fbe\u7b49\u9ad8\u6210\u672c\u4f20\u611f\u5668<\/p>\n

    \u8981\u70b92：\u4f4e\u6f02\u79fb\u5b9a\u4f4d\u9700\u7ed3\u5408\u591a\u6e90\u6821\u51c6，\u4fdd\u969c\u5bfc\u822a\u53ef\u9760\u6027
    \n\u8f7b\u91cfSLAM\u7684\u5b9a\u4f4d\u7cbe\u5ea6\u4f9d\u8d56\u6301\u7eed\u6821\u51c6，\u907f\u514d\u8bef\u5dee\u7d2f\u79ef\u3002
    \nIMU\u6807\u5b9a\u4e0e\u6ee4\u6ce2：\u9700\u5728\u5f00\u673a\u65f6\u5b8c\u6210IMU\u7684\u96f6\u70b9\u6821\u51c6\u3001\u6e29\u5ea6\u8865\u507f，\u8fd0\u884c\u4e2d\u91c7\u7528\u5361\u5c14\u66fc\u6ee4\u6ce2\u7b49\u7b97\u6cd5\u5904\u7406\u9640\u87ba\u4eea\u4e0e\u52a0\u901f\u5ea6\u8ba1\u6570\u636e，\u6291\u5236\u566a\u58f0\u5e72\u6270，\u51cf\u5c11\u59ff\u6001\u6d4b\u91cf\u8bef\u5dee
    \n\u8fb9\u754c\u7279\u5f81\u6821\u51c6：\u5229\u7528\u7ea2\u5916\u68c0\u6d4b\u5230\u7684\u5899\u58c1\u3001\u5bb6\u5177\u7b49\u56fa\u5b9a\u8fb9\u754c，\u7ed3\u5408\u822a\u5411\u89d2\u6362\u7b97\u5b9e\u9645\u5750\u6807，\u4f5c\u4e3a\u5b9a\u4f4d\u57fa\u51c6，\u5b9a\u671f\u4fee\u6b63\u91cc\u7a0b\u8ba1\u6f02\u79fb，\u786e\u4fdd\u5b9a\u4f4d\u957f\u671f\u51c6\u786e
    \n\u91cc\u7a0b\u8ba1\u6807\u5b9a：\u51c6\u786e\u6807\u5b9a\u7f16\u7801\u5668\u4e0e\u8f6e\u5f84\u3001\u884c\u9a76\u8ddd\u79bb\u7684\u5bf9\u5e94\u5173\u7cfb，\u51cf\u5c11\u79fb\u52a8\u8ddd\u79bb\u8ba1\u7b97\u8bef\u5dee，\u4e3a\u5b9a\u4f4d\u63d0\u4f9b\u53ef\u9760\u7684\u4f4d\u79fb\u8f93\u5165<\/p>\n

    \u8981\u70b93：\u591a\u7ea2\u5916\u534f\u540c\u5b9e\u73b0\u5168\u65b9\u4f4d\u907f\u969c，\u9002\u914d\u5bb6\u5ead\u590d\u6742\u73af\u5883
    \n\u5bb6\u5ead\u73af\u5883\u969c\u788d\u7269\u5bc6\u96c6\u3001\u5f62\u72b6\u591a\u6837，\u9700\u901a\u8fc7\u591a\u7ea2\u5916\u534f\u540c\u5b9e\u73b0\u53ef\u9760\u907f\u969c\u3002
    \n\u591a\u8def\u7ea2\u5916\u5e03\u5c40：\u91c7\u7528\u524d\u3001\u5de6\u524d\u3001\u53f3\u524d\u3001\u5de6\u3001\u53f3\u7684\u5e03\u5c40，\u5b9e\u73b0\u524d\u5411\u4e0e\u4fa7\u5411\u5168\u8986\u76d6，\u51cf\u5c11\u68c0\u6d4b\u76f2\u533a，\u7cbe\u51c6\u5224\u65ad\u969c\u788d\u4f4d\u7f6e\u4e0e\u8ddd\u79bb
    \n\u52a8\u6001\u907f\u969c\u7b56\u7565：\u6839\u636e\u969c\u788d\u8ddd\u79bb\u3001\u5468\u8fb9\u7a7a\u95f4，\u7075\u6d3b\u9009\u62e9\u7ed5\u969c\u3001\u51cf\u901f\u3001\u8ddf\u968f\u7b49\u7b56\u7565，\u4f8b\u5982\u5bb6\u5177\u95f4\u9699\u72ed\u7a84\u65f6\u8d34\u8fb9\u884c\u9a76，\u79fb\u52a8\u969c\u788d\u65f6\u5feb\u901f\u907f\u8ba9，\u9002\u914d\u4e0d\u540c\u969c\u788d\u7c7b\u578b
    \n\u73af\u5883\u81ea\u9002\u5e94：\u9488\u5bf9\u4e0d\u540c\u5bb6\u5c45\u73af\u5883（\u7a7a\u65f7\u5ba2\u5385\u3001\u72ed\u7a84\u8d70\u5eca\u3001\u5bb6\u5177\u5bc6\u96c6\u5367\u5ba4），\u8c03\u6574\u7ea2\u5916\u68c0\u6d4b\u9608\u503c\u4e0e\u907f\u969c\u901f\u5ea6，\u63d0\u5347\u73af\u5883\u9002\u5e94\u80fd\u529b<\/p>\n

    \u8981\u70b94：SLAM\u5730\u56fe\u9700\u8f7b\u91cf\u5316\u5b58\u50a8\u4e0e\u9ad8\u6548\u5e94\u7528，\u5e73\u8861\u6027\u80fd\u4e0e\u7cbe\u5ea6
    \n\u8f7b\u91cfSLAM\u7684\u5730\u56fe\u9700\u517c\u987e\u8ba1\u7b97\u6548\u7387\u4e0e\u5bfc\u822a\u9700\u6c42，\u9002\u914dArduino\u7b49\u4f4e\u7b97\u529b\u5e73\u53f0\u3002
    \n\u5730\u56fe\u7b80\u5316\u8bbe\u8ba1：\u91c7\u7528\u6805\u683c\u5730\u56fe\u6216\u7279\u5f81\u70b9\u5730\u56fe，\u66ff\u4ee3\u9ad8\u7cbe\u5ea6\u5168\u91cf\u5730\u56fe，\u8bb0\u5f55\u969c\u788d\u4f4d\u7f6e\u3001\u53ef\u901a\u884c\u533a\u57df\u7b49\u5173\u952e\u4fe1\u606f，\u51cf\u5c11\u5b58\u50a8\u4e0e\u8ba1\u7b97\u538b\u529b，\u9002\u914d\u8f7b\u91cf\u7ea7\u786c\u4ef6
    \n\u589e\u91cf\u6784\u5efa：\u673a\u5668\u4eba\u884c\u9a76\u8fc7\u7a0b\u4e2d\u52a8\u6001\u66f4\u65b0\u5730\u56fe，\u4ec5\u5b58\u50a8\u5df2\u63a2\u7d22\u533a\u57df\u4e0e\u969c\u788d\u4fe1\u606f，\u907f\u514d\u5197\u4f59，\u63d0\u5347\u5730\u56fe\u6784\u5efa\u6548\u7387
    \n\u5730\u56fe\u590d\u7528：\u5c06\u6784\u5efa\u7684\u5730\u56fe\u4fdd\u5b58\u81f3\u5b58\u50a8\u6a21\u5757，\u652f\u6301\u5f00\u673a\u52a0\u8f7d，\u907f\u514d\u91cd\u590d\u626b\u63cf，\u63d0\u5347\u673a\u5668\u4eba\u591a\u6b21\u5de1\u67e5\u7684\u6548\u7387<\/p>\n

    \u8981\u70b95：\u8fd0\u884c\u76d1\u63a7\u4e0e\u5f02\u5e38\u4fdd\u62a4\u662f\u5b89\u5168\u5e95\u7ebf，\u4fdd\u969c\u5c45\u5bb6\u573a\u666f\u53ef\u9760\u8fd0\u884c
    \n\u5bb6\u5ead\u573a\u666f\u4eba\u5458\u6d3b\u52a8\u9891\u7e41\u3001\u73af\u5883\u591a\u53d8，\u5fc5\u987b\u5b8c\u5584\u76d1\u63a7\u4e0e\u4fdd\u62a4\u673a\u5236\u3002
    \n\u72b6\u6001\u5168\u666f\u76d1\u63a7：\u5b9e\u65f6\u76d1\u63a7\u4f20\u611f\u5668（\u7ea2\u5916\u3001IMU）\u3001\u5b9a\u4f4d\u3001\u7535\u673a\u3001\u7535\u91cf\u7b49\u6838\u5fc3\u72b6\u6001，\u5168\u65b9\u4f4d\u638c\u63e1\u673a\u5668\u4eba\u8fd0\u884c\u60c5\u51b5，\u53ca\u65f6\u53d1\u73b0\u5f02\u5e38\u9690\u60a3
    \n\u5f02\u5e38\u5206\u7ea7\u54cd\u5e94：\u5c06\u5f02\u5e38\u5206\u4e3a\u8f7b\u3001\u4e2d\u3001\u91cd\u4e09\u7ea7，\u8f7b\u5fae\u5f02\u5e38\u51cf\u901f\u9884\u8b66，\u4e2d\u5ea6\u5f02\u5e38\u6682\u505c\u6821\u51c6，\u4e25\u91cd\u5f02\u5e38\u7d27\u6025\u505c\u673a，\u907f\u514d\u6216\u51cf\u5c11\u78b0\u649e\u3001\u6545\u969c\u7b49\u98ce\u9669
    \n\u5b89\u5168\u9632\u62a4\u8bbe\u8ba1：\u52a0\u5165\u6025\u505c\u63a7\u5236\u3001\u5012\u8f66\u907f\u9669\u3001\u4f4e\u7535\u91cf\u8fd4\u822a\u7b49\u4fdd\u62a4\u903b\u8f91，\u5f02\u5e38\u65f6\u4f18\u5148\u4fdd\u969c\u5bb6\u5c45\u4e0e\u4eba\u7684\u5b89\u5168，\u9002\u914d\u5bb6\u5ead\u4f7f\u7528\u7684\u9ad8\u5b89\u5168\u8981\u6c42<\/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":"

    \u201cArduino BLDC\u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7\u201d\u662f\u4e00\u4e2a\u5178\u578b\u7684\u4f4e\u6210\u672c\u3001\u9ad8\u96c6\u6210\u5ea6\u7684\u5d4c\u5165\u5f0f\u673a\u5668\u4eba\u7cfb\u7edf\u3002\u5b83\u5229\u7528Arduino\u4f5c\u4e3a\u6838\u5fc3\u63a7\u5236\u5668，\u7ed3\u5408\u65e0\u5237\u76f4\u6d41\u7535\u673a（BLDC）\u7684\u9ad8\u52a8\u6001\u6027\u80fd，\u901a\u8fc7\u7ea2\u5916\u4f20\u611f\u5668\u4e0e\u60ef\u6027\u6d4b\u91cf\u5355\u5143（IMU）\u7684\u6570\u636e\u878d\u5408，\u5b9e\u73b0\u8f7b\u91cf\u7ea7\u7684\u540c\u6b65\u5b9a\u4f4d\u4e0e\u5730\u56fe\u6784\u5efa（SLAM）\u4ee5\u53ca\u52a8\u6001\u907f\u969c\u529f\u80fd\u3002\u4ee5\u4e0b\u4ece\u4e3b\u8981\u7279\u70b9\u3001\u5e94\u7528\u573a\u666f\u53ca\u6ce8<\/p>\n","protected":false},"author":2,"featured_media":109888,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[12538,12537,55,270,269],"topic":[],"class_list":["post-109892","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-server","tag-12538","tag-slam","tag-c","tag-270","tag-269"],"yoast_head":"\n\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7 - \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\/109892.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\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"og:description\" content=\"\u201cArduino BLDC\u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7\u201d\u662f\u4e00\u4e2a\u5178\u578b\u7684\u4f4e\u6210\u672c\u3001\u9ad8\u96c6\u6210\u5ea6\u7684\u5d4c\u5165\u5f0f\u673a\u5668\u4eba\u7cfb\u7edf\u3002\u5b83\u5229\u7528Arduino\u4f5c\u4e3a\u6838\u5fc3\u63a7\u5236\u5668，\u7ed3\u5408\u65e0\u5237\u76f4\u6d41\u7535\u673a（BLDC）\u7684\u9ad8\u52a8\u6001\u6027\u80fd，\u901a\u8fc7\u7ea2\u5916\u4f20\u611f\u5668\u4e0e\u60ef\u6027\u6d4b\u91cf\u5355\u5143（IMU）\u7684\u6570\u636e\u878d\u5408，\u5b9e\u73b0\u8f7b\u91cf\u7ea7\u7684\u540c\u6b65\u5b9a\u4f4d\u4e0e\u5730\u56fe\u6784\u5efa（SLAM）\u4ee5\u53ca\u52a8\u6001\u907f\u969c\u529f\u80fd\u3002\u4ee5\u4e0b\u4ece\u4e3b\u8981\u7279\u70b9\u3001\u5e94\u7528\u573a\u666f\u53ca\u6ce8\" \/>\n<meta property=\"og:url\" content=\"https:\/\/www.wsisp.com\/helps\/109892.html\" \/>\n<meta property=\"og:site_name\" content=\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"article:published_time\" content=\"2026-09-24T21:17:13+00:00\" \/>\n<meta property=\"og:image\" content=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/09\/20260924211710-6ab5935649f71.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=\"17 \u5206\" \/>\n<script type=\"application\/ld+json\" class=\"yoast-schema-graph\">{\"@context\":\"https:\/\/schema.org\",\"@graph\":[{\"@type\":\"WebPage\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/109892.html\",\"url\":\"https:\/\/www.wsisp.com\/helps\/109892.html\",\"name\":\"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"isPartOf\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\"},\"datePublished\":\"2026-09-24T21:17:13+00:00\",\"dateModified\":\"2026-09-24T21:17:13+00:00\",\"author\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\"},\"breadcrumb\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/109892.html#breadcrumb\"},\"inLanguage\":\"zh-Hans\",\"potentialAction\":[{\"@type\":\"ReadAction\",\"target\":[\"https:\/\/www.wsisp.com\/helps\/109892.html\"]}]},{\"@type\":\"BreadcrumbList\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/109892.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\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7\"}]},{\"@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\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7 - \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\/109892.html","og_locale":"zh_CN","og_type":"article","og_title":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","og_description":"\u201cArduino BLDC\u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7\u201d\u662f\u4e00\u4e2a\u5178\u578b\u7684\u4f4e\u6210\u672c\u3001\u9ad8\u96c6\u6210\u5ea6\u7684\u5d4c\u5165\u5f0f\u673a\u5668\u4eba\u7cfb\u7edf\u3002\u5b83\u5229\u7528Arduino\u4f5c\u4e3a\u6838\u5fc3\u63a7\u5236\u5668，\u7ed3\u5408\u65e0\u5237\u76f4\u6d41\u7535\u673a（BLDC）\u7684\u9ad8\u52a8\u6001\u6027\u80fd，\u901a\u8fc7\u7ea2\u5916\u4f20\u611f\u5668\u4e0e\u60ef\u6027\u6d4b\u91cf\u5355\u5143（IMU）\u7684\u6570\u636e\u878d\u5408，\u5b9e\u73b0\u8f7b\u91cf\u7ea7\u7684\u540c\u6b65\u5b9a\u4f4d\u4e0e\u5730\u56fe\u6784\u5efa（SLAM）\u4ee5\u53ca\u52a8\u6001\u907f\u969c\u529f\u80fd\u3002\u4ee5\u4e0b\u4ece\u4e3b\u8981\u7279\u70b9\u3001\u5e94\u7528\u573a\u666f\u53ca\u6ce8","og_url":"https:\/\/www.wsisp.com\/helps\/109892.html","og_site_name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","article_published_time":"2026-09-24T21:17:13+00:00","og_image":[{"url":"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/09\/20260924211710-6ab5935649f71.gif"}],"author":"admin","twitter_card":"summary_large_image","twitter_misc":{"\u4f5c\u8005":"admin","\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4":"17 \u5206"},"schema":{"@context":"https:\/\/schema.org","@graph":[{"@type":"WebPage","@id":"https:\/\/www.wsisp.com\/helps\/109892.html","url":"https:\/\/www.wsisp.com\/helps\/109892.html","name":"\u3010\u82b1\u96d5\u5b66\u7f16\u7a0b\u3011Arduino BLDC \u4e4b\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7 - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","isPartOf":{"@id":"https:\/\/www.wsisp.com\/helps\/#website"},"datePublished":"2026-09-24T21:17:13+00:00","dateModified":"2026-09-24T21:17:13+00:00","author":{"@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41"},"breadcrumb":{"@id":"https:\/\/www.wsisp.com\/helps\/109892.html#breadcrumb"},"inLanguage":"zh-Hans","potentialAction":[{"@type":"ReadAction","target":["https:\/\/www.wsisp.com\/helps\/109892.html"]}]},{"@type":"BreadcrumbList","@id":"https:\/\/www.wsisp.com\/helps\/109892.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\u5bb6\u5ead\u670d\u52a1\u673a\u5668\u4eba\u2014\u2014\u7ea2\u5916+IMU\u878d\u5408\u7684\u8f7b\u91cfSLAM\u5bfc\u822a\u4e0e\u907f\u969c\u76d1\u63a7"}]},{"@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\/109892","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=109892"}],"version-history":[{"count":0,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/109892\/revisions"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media\/109888"}],"wp:attachment":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media?parent=109892"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/categories?post=109892"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/tags?post=109892"},{"taxonomy":"topic","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/topic?post=109892"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}