diff --git a/.gitee/hexapod-robot.png b/.gitee/hexapod-robot.png new file mode 100644 index 0000000000000000000000000000000000000000..652c4f7f14fa5c20d69b1c59896dcd56c2310cd8 Binary files /dev/null and b/.gitee/hexapod-robot.png differ diff --git "a/.gitee/\345\276\256\344\277\241\345\233\276\347\211\207_20230709205151.jpg" "b/.gitee/\345\276\256\344\277\241\345\233\276\347\211\207_20230709205151.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..3631eedea35660c36267c26175aff3695c8130e2 Binary files /dev/null and "b/.gitee/\345\276\256\344\277\241\345\233\276\347\211\207_20230709205151.jpg" differ diff --git "a/.gitee/\346\225\264\344\275\223\347\273\204\350\243\205\347\244\272\346\204\217.jpg" "b/.gitee/\346\225\264\344\275\223\347\273\204\350\243\205\347\244\272\346\204\217.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..6e4a99141b89f762aaf16baaf9c79b11ce4ce311 Binary files /dev/null and "b/.gitee/\346\225\264\344\275\223\347\273\204\350\243\205\347\244\272\346\204\217.jpg" differ diff --git "a/.gitee/\351\223\276\346\216\245\347\224\265\350\267\257\345\233\276.jpg" "b/.gitee/\351\223\276\346\216\245\347\224\265\350\267\257\345\233\276.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..214e2ea23e26474856820f901cbc6963c1173e7e Binary files /dev/null and "b/.gitee/\351\223\276\346\216\245\347\224\265\350\267\257\345\233\276.jpg" differ diff --git "a/3D \346\211\223\345\215\260/606662a35995d_wifi-hexapod-spider-robot.zip" "b/3D \346\211\223\345\215\260/606662a35995d_wifi-hexapod-spider-robot.zip" new file mode 100644 index 0000000000000000000000000000000000000000..c0081c3442469f3d5129ab2e740cbab59c87dfb6 Binary files /dev/null and "b/3D \346\211\223\345\215\260/606662a35995d_wifi-hexapod-spider-robot.zip" differ diff --git "a/3D \346\211\223\345\215\260/coxa-a.SLDPRT" "b/3D \346\211\223\345\215\260/coxa-a.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..e347774a130e1351fbb38c75fa524a0423200dcb Binary files /dev/null and "b/3D \346\211\223\345\215\260/coxa-a.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/coxa-b.SLDPRT" "b/3D \346\211\223\345\215\260/coxa-b.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..1dc2437dbae6ac275272c8f19b8ee73d290d80ff Binary files /dev/null and "b/3D \346\211\223\345\215\260/coxa-b.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/femur-a.SLDPRT" "b/3D \346\211\223\345\215\260/femur-a.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..039a59f2cff8b267f919336abbac6128a441bb31 Binary files /dev/null and "b/3D \346\211\223\345\215\260/femur-a.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/femur-b.SLDPRT" "b/3D \346\211\223\345\215\260/femur-b.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..b9665eae0ed0806a464f0b1eadc8eefeeef75bd1 Binary files /dev/null and "b/3D \346\211\223\345\215\260/femur-b.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/hex-body-short.SLDPRT" "b/3D \346\211\223\345\215\260/hex-body-short.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..b896480278ed626e4fffd0feca4d18619ce99092 Binary files /dev/null and "b/3D \346\211\223\345\215\260/hex-body-short.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/hex-cover-short.SLDPRT" "b/3D \346\211\223\345\215\260/hex-cover-short.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..27d0a67a85d87e93ee6147dfd524d4dfd9885f26 Binary files /dev/null and "b/3D \346\211\223\345\215\260/hex-cover-short.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/shield.SLDPRT" "b/3D \346\211\223\345\215\260/shield.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..ff147e2d9941359dadca4b3f8b731d801c9bb0d4 Binary files /dev/null and "b/3D \346\211\223\345\215\260/shield.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/tibia-a.SLDPRT" "b/3D \346\211\223\345\215\260/tibia-a.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..2a8efb9972efdfb3babb6eab32d482fb6eef4ba1 Binary files /dev/null and "b/3D \346\211\223\345\215\260/tibia-a.SLDPRT" differ diff --git "a/3D \346\211\223\345\215\260/tibia-b.SLDPRT" "b/3D \346\211\223\345\215\260/tibia-b.SLDPRT" new file mode 100644 index 0000000000000000000000000000000000000000..f34dc9f07fd2e1162895021f5ab71e8ffd780aae Binary files /dev/null and "b/3D \346\211\223\345\215\260/tibia-b.SLDPRT" differ diff --git a/README.en.md b/README.en.md deleted file mode 100644 index 71cdb445f048b2238c0e92dd96b53b161b17a3f4..0000000000000000000000000000000000000000 --- a/README.en.md +++ /dev/null @@ -1,36 +0,0 @@ -# 六足机器人 - -#### Description -制作仿生六足机器人的保姆级教程 - -#### Software Architecture -Software architecture description - -#### Installation - -1. xxxx -2. xxxx -3. xxxx - -#### Instructions - -1. xxxx -2. xxxx -3. xxxx - -#### Contribution - -1. Fork the repository -2. Create Feat_xxx branch -3. Commit your code -4. Create Pull Request - - -#### Gitee Feature - -1. You can use Readme\_XXX.md to support different languages, such as Readme\_en.md, Readme\_zh.md -2. Gitee blog [blog.gitee.com](https://blog.gitee.com) -3. Explore open source project [https://gitee.com/explore](https://gitee.com/explore) -4. The most valuable open source project [GVP](https://gitee.com/gvp) -5. The manual of Gitee [https://gitee.com/help](https://gitee.com/help) -6. The most popular members [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/) diff --git a/README.md b/README.md index caeeaf7ec5d2d20708a8a47caad2b5168fb194b7..fadb9454f1b846ac79034915ef340a6fa131a2a3 100644 --- a/README.md +++ b/README.md @@ -1,37 +1,151 @@ # 六足机器人 -#### 介绍 -制作仿生六足机器人的保姆级教程 +#### 介绍。 +制作仿生六足机器人的完整制作教程，该教程包含3D打印文件和制作全流程。 -#### 软件架构 -软件架构说明 +欢迎关注Star :kissing_heart: :kissing_heart: :kissing_heart: +![输入图片说明](.gitee/%E5%BE%AE%E4%BF%A1%E5%9B%BE%E7%89%87_20230709205151.jpg) + +B站视频：https://www.bilibili.com/video/BV15x4y1N7id/?pop_share=1&vd_source=1608f0247af63a025b4aed15bc542838 + +六足机器人是一种用六条腿行走的机械人。由于机器人可以在三条或更多条腿上保持静态稳定，因此六足机器人机器人在移动方式方面具有很大的灵活性。 + +许多六足机器人机器人的生物学灵感来自六足机器人运动。六足机器人可用于测试有关昆虫运动、运动控制和神经生物学的生物学理论。 + +#### 硬件架构 + + 硬件件架构说明 + + 需要用到的电子元器件(图片在电子元器件文件夹） + + 1.32路舵机控制板 + + 2.mini D1 wifi ESP8266开发板ModeMcu + + 3.SG90 9g舵机 + + 4.HENGE 恒吉4A UBEC + + 5.电源降压模块 + + 6.杜邦线若干+电源 #### 安装教程 -1. xxxx -2. xxxx -3. xxxx +1. 3D打印清单(文件存放于3D打印文件夹中）： + + 3xcoxa-a.SLDPRT + + 3xcoxa-b.SLDPRT + + 3xfemur-a.SLDPRT + + 3xfemur-b.SLDPRT + + 1xhex-body-short.SLDPRT + + 1xhex-cover-short.SLDPRT + + 6xshield.SLDPRT + + 3xtibia-a.SLDPRT + + 3xtibia-b.SLDPRT + +2. 六足机器人的组装教程 + + 整体组装示意图 + +![输入图片说明](.gitee/%E6%95%B4%E4%BD%93%E7%BB%84%E8%A3%85%E7%A4%BA%E6%84%8F.jpg) + + 具体组装流程:(因涉及图片较多 **具体流程图片放在零件到整体文件夹** 里 :blush: ） + +搭建视频教程：https://www.bilibili.com/video/BV1oY4y1X7rZ/?spm_id_from=333.1007.top_right_bar_window_dynamic.content.click&vd_source=1608f0247af63a025b4aed15bc542838 + +3. 线路组装教程 + + 整体线路连接示意图 + +![输入图片说明](%E7%BA%BF%E8%B7%AF%E7%BB%84%E8%A3%85%E5%9B%BE/%E9%93%BE%E6%8E%A5%E7%94%B5%E8%B7%AF%E5%9B%BE.jpg) + + 舵机连接说明 + +![输入图片说明](%E7%BA%BF%E8%B7%AF%E7%BB%84%E8%A3%85%E5%9B%BE/%E8%88%B5%E6%9C%BA%E9%93%BE%E6%8E%A5%E7%A4%BA%E6%84%8F%E5%9B%BE.jpg) + + 1.来自 UBEC 的 5v 电源输出 （+） + + 2.来自UBEC和迷你降压的地面 + + 3.5V 电源输出 （+） 从迷你降压 + + 4.至 Wemos D1 迷你 G 引脚 + + 5.至 Wemos D1 迷你 5v 引脚 + + 6.至 Wemos 迷你 RX 引脚 + + 7.至 Wemos 迷你 TX 引脚 + + 关节说明 + +![输入图片说明](%E7%BA%BF%E8%B7%AF%E7%BB%84%E8%A3%85%E5%9B%BE/%E5%9B%BE%E7%89%871.png) + + 8.（将引脚颜色与舵机电缆颜色匹配）到右前腿（引脚 1 到髋关节，引脚 2 到膝关节，引脚 3 踝关节舵机） + + 9.（引脚颜色与伺服电缆颜色匹配）到左前腿（引脚 5 到髋关节，引脚 6 到膝关节，引脚 7 踝关节舵机） + + 11.（引脚颜色与伺服电缆颜色匹配）到左中腿（引脚 13 连接到髋关节，引脚 14 连接到膝关节，引脚 15 踝关节舵机） + + 12.（引脚颜色与伺服电缆颜色匹配）到右中间腿（引脚 17 连接到髋关节，引脚 18 连接到膝关节，引脚 19 踝关节舵机） + + 13.（引脚颜色与伺服电缆颜色匹配）到左后腿（引脚 25 连接到髋关节，引脚 26 到膝关节，引脚 27 踝关节舵机） + + 14.（引脚颜色与伺服电缆颜色匹配）到右后腿（引脚 29 连接到髋关节，引脚 30 连接到膝关节，引脚 31 踝关节舵机） #### 使用说明 -1. xxxx -2. xxxx -3. xxxx +1. mini D1 ESP8266 WiFi板电路连接示意图 + +![输入图片说明](%E7%BA%BF%E8%B7%AF%E7%BB%84%E8%A3%85%E5%9B%BE/mini%20D1%20%E9%93%BE%E6%8E%A5%E5%9B%BE.jpg) + + 电路连接说明： + + +WeMos D1 mini是最小的ESP8266 wifi板 + + 使用wemos D1 mini作为WIFI AP，我们可以向32路舵机控制器发送串行命令， + + 并独立成为Web服务器AP来控制机器人。 + + 引脚连接说明： + + 1.到32路舵机控制器RX引脚 + + 2.到32路舵机控制器TX引脚(注意不要把RX、RT引脚接反了，在开发板下载程序时，不要连接引脚） + + 3.来自32路舵机控制器G针 + + 4.来自32路舵机控制器5v引脚 + +2. 程序(文件储存在程序文件夹) + + +3. 开始使用 + + 我们可以将智能手机连接到nodeMCU AP，并使用网络浏览器打开http://192.168.4.1，我们可以看到所有的命令。(网址一定要使用手机打开） + +#### 特别说明 + +1. 本项目有参考网上的一些开源项目 + +https://acktomas.blog.csdn.net/article/details/125620291?spm=1001.2101.3001.6650.18 -#### 参与贡献 +https://blog.csdn.net/zyc121561/article/details/77679278?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166873748816782414912961%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=166873748816782414912961&biz_id=0&spm=1018.2226.3001.4187 -1. Fork 本仓库 -2. 新建 Feat_xxx 分支 -3. 提交代码 -4. 新建 Pull Request +2. 编程软件为 Arduino IDE ，舵机调试软件为 ServoController。 #### 特技 -1. 使用 Readme\_XXX.md 来支持不同的语言，例如 Readme\_en.md, Readme\_zh.md -2. Gitee 官方博客 [blog.gitee.com](https://blog.gitee.com) -3. 你可以 [https://gitee.com/explore](https://gitee.com/explore) 这个地址来了解 Gitee 上的优秀开源项目 -4. [GVP](https://gitee.com/gvp) 全称是 Gitee 最有价值开源项目，是综合评定出的优秀开源项目 -5. Gitee 官方提供的使用手册 [https://gitee.com/help](https://gitee.com/help) -6. Gitee 封面人物是一档用来展示 Gitee 会员风采的栏目 [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/) +1. 提供关于六足机器人的电路、3D打印、程序的一切支持 diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/32\350\267\257\350\210\265\346\234\272\346\216\247\345\210\266\346\235\277.jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/32\350\267\257\350\210\265\346\234\272\346\216\247\345\210\266\346\235\277.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..4196a364800603f549afb534a3ecbe2f31542392 Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/32\350\267\257\350\210\265\346\234\272\346\216\247\345\210\266\346\235\277.jpg" differ diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/HENGE \346\201\222\345\220\2114A UBEC.jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/HENGE \346\201\222\345\220\2114A UBEC.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..5015b8fb153375c704c11b0cd118e264eaeafe66 Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/HENGE \346\201\222\345\220\2114A UBEC.jpg" differ diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/SG90 9g\350\210\265\346\234\272.jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/SG90 9g\350\210\265\346\234\272.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..d62fc8c0beed0ce710e3184c088894b83a0b798e Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/SG90 9g\350\210\265\346\234\272.jpg" differ diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/mini D1 wifi ESP8266\345\274\200\345\217\221\346\235\277ModeMcu .jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/mini D1 wifi ESP8266\345\274\200\345\217\221\346\235\277ModeMcu .jpg" new file mode 100644 index 0000000000000000000000000000000000000000..bf8d86d5e313ea4c9f1d243b0894a245a3c5c1e7 Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/mini D1 wifi ESP8266\345\274\200\345\217\221\346\235\277ModeMcu .jpg" differ diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\346\235\234\351\202\246\347\272\277.jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\346\235\234\351\202\246\347\272\277.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..ec75e15a8860376aba31dcc586b674e7b7a4fc18 Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\346\235\234\351\202\246\347\272\277.jpg" differ diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\347\224\265\346\272\220.jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\347\224\265\346\272\220.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..7d494c638f39aeedc715b1f4381f80743f6bea8a Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\347\224\265\346\272\220.jpg" differ diff --git "a/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\347\224\265\346\272\220\351\231\215\345\216\213\346\250\241\345\235\227.jpg" "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\347\224\265\346\272\220\351\231\215\345\216\213\346\250\241\345\235\227.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..214078b6a1b2852d64ab9170c6c8fc24dcc06e31 Binary files /dev/null and "b/\347\224\265\345\255\220\345\205\203\345\231\250\344\273\266/\347\224\265\346\272\220\351\231\215\345\216\213\346\250\241\345\235\227.jpg" differ diff --git "a/\347\250\213\345\272\217/F27D9DGJTT5USBD.ino" "b/\347\250\213\345\272\217/F27D9DGJTT5USBD.ino" new file mode 100644 index 0000000000000000000000000000000000000000..9071a603170c30c122f4961317ac7caf1f69f9dc --- /dev/null +++ "b/\347\250\213\345\272\217/F27D9DGJTT5USBD.ino" @@ -0,0 +1,353 @@ +#include +#include +#include +#include +#include + +String CommOut=""; String CommIn=""; String lastComm = ""; +int StepSpeed = 50; int lastSpeed = 50; +int SMov[32]={1440,1440,1440,0,1440,1440,1440,0,0,0,0,0,1440,1440,1440,0,1440,1440,1440,0,0,0,0,0,1440,1440,1440,0,1440,1440,1440,0}; +int SAdj[32]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; +int StaBlink=0; int IntBlink=0; +int ClawPos=1500; + +//SSID and Password to your ESP Access Point +const char* ssid = "HexapodWifi"; +const char* password = "12345678"; + +static const char PROGMEM INDEX_HTML[] = R"rawliteral( +ESP8266 Spider Hexapod + + 
Quadruped Controller
+ + + + + + + + + +
 
 
 

this application requires Mwilmar Quadruped platform.

+)rawliteral"; + + +WebSocketsServer webSocket = WebSocketsServer(81); +ESP8266WebServer server(80); + +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + Serial.begin(9600); + Serial.println (">> Setup"); + for(uint8_t t = 4;t > 0;t--) { + Serial.flush(); + delay(1000); + } + delay(1000); + WiFi.mode(WIFI_AP); //Only Access point + WiFi.softAP(ssid, password); //Start HOTspot removing password will disable security + + IPAddress myIP = WiFi.softAPIP(); //Get IP address + Serial.println(""); + Serial.print("Connected to "); + Serial.println(ssid); + Serial.print("IP address: "); + Serial.println(myIP); + + server.on("/", [](){ + server.send(200, "text/html", INDEX_HTML); + }); + + server.begin(); + + webSocket.begin(); + webSocket.onEvent(webSocketEvent); + +} + +void loop() { + if (Serial.available() > 0){ + char c[] = {(char)Serial.read()}; + webSocket.broadcastTXT(c, sizeof(c)); + } + + IntBlink +=1; + if (IntBlink >=5000){ + if (StaBlink ==0){ + digitalWrite(LED_BUILTIN, HIGH); + StaBlink=1; + } else { + digitalWrite(LED_BUILTIN, LOW); + StaBlink=0; + } + IntBlink =0; + } + webSocket.loop(); + server.handleClient(); + if (CommOut == "w 0 1") Move_STP(); //~~~~~~~~ Stop + if (CommOut == "w 1 1") Move_FWD(); //~~~~~~~~ Forward + if (CommOut == "w 2 1") Move_BWD(); //~~~~~~~~ backward + if (CommOut == "w 3 1") Move_LFT(); //~~~~~~~~ turn left + if (CommOut == "w 4 1") Move_RGT(); //~~~~~~~~ turn right + //~~~~~~~~ shake hand + if (CommOut == "w 5 3"){ + lastSpeed = StepSpeed;StepSpeed = 300; + Move_SHK();StepSpeed = lastSpeed;CommOut=lastComm; + } + //~~~~~~~~ waving + if (CommOut == "w 6 3"){ + lastSpeed = StepSpeed;StepSpeed = 300; + Move_WAV();StepSpeed = lastSpeed;CommOut=lastComm; + } + //~~~~~~~~ pos initial + if (CommOut == "w 15") { + Pos_INT();CommOut=lastComm; + } + //~~~~~~~~ pos service + if (CommOut == "w 12") { + Pos_SRV();CommOut=lastComm; + } + //~~~~~~~~ adjust body left + if (CommOut == "w 11 5") { + Adj_LF();CommOut=lastComm; + } + //~~~~~~~~ adjust body right + if (CommOut == "w 10 5") { + Adj_RG();CommOut=lastComm; + } + //~~~~~~~~ adjust head up + if (CommOut == "w 8 5") { + Adj_HU();CommOut=lastComm; + } + //~~~~~~~~ adjust head down + if (CommOut == "w 9 5") { + Adj_HD();CommOut=lastComm; + } + //~~~~~~~~ adjust body higher + if (CommOut == "w 13") { + Adj_HG();CommOut=lastComm; + } + //~~~~~~~~ adjust body lower + if (CommOut == "w 14") { + Adj_LW();CommOut=lastComm; + } + //~~~~~~~~ adjust twist left + if (CommOut == "w 16") { + Adj_TL();CommOut=lastComm; + } + //~~~~~~~~ adjust twist right + if (CommOut == "w 17") { + Adj_TR();CommOut=lastComm; + } + //~~~~~~~~ adjust speed walk + if (CommOut == "w 0 0") { + StepSpeed = 300;CommOut=lastComm; + } + //~~~~~~~~ adjust speed run + if (CommOut == "w 7 1") { + StepSpeed = 50;CommOut=lastComm; + } + //~~~~~~~ claw close + if (CommOut == "w 20") { + ClwCls();CommOut=lastComm; + } + //~~~~~~~~~~~ claw open + if (CommOut == "w 21") { + ClwOpn();CommOut=lastComm; + } +} + +void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) +{ + switch(type) { + case WStype_DISCONNECTED: + break; + case WStype_CONNECTED: + { + IPAddress ip = webSocket.remoteIP(num); + } + break; + case WStype_TEXT: + lastComm = CommOut; CommOut = ""; + for(int i = 0;i < length;i++) CommOut += ((char) payload[i]); + // send data to all connected clients + webSocket.broadcastTXT(payload, length); + break; + case WStype_BIN: + hexdump(payload, length); + // echo data back to browser + webSocket.sendBIN(num, payload, length); + break;default:break; + } +} + +void Send_Comm() { + String SendString;int i; + for (i = 1;i < 32;i += 1) { + if (SMov[i]>=600 and SMov[i]<=2280){ + SendString = SendString +"#" +i +"P" +String(SMov[i]); + } + } + Serial.print (SendString +"T"+String(StepSpeed)+"D0\r\n");wait_serial_return_ok(); +} + +void wait_serial_return_ok() +{ + // int TimeOut=0; + int num=0; char c[16]; while(1) + { + // TimeOut +=1; + while(Serial.available() > 0) + { + webSocket.loop();server.handleClient(); + c[num]=Serial.read();num++; + if(num>=15) num=0; + } + if(c[num-2]=='O'&&c[num-1]=='K') break; + // if(TimeOut > 800000) break; + } +} + + +//================================================================================= Servo Move ======================================================================= +//~~~~~~~~ claw open +void ClwOpn(){ + ClawPos=1000; + if (ClawPos <= 1000) ClawPos=1000; + Serial.println("#9P" + String(ClawPos) + "T50D0"); + wait_serial_return_ok(); +} +//~~~~~~~~~~claw close +void ClwCls(){ + ClawPos +=100; + if (ClawPos >= 1600) ClawPos=1600; + Serial.println("#9P" + String(ClawPos) + "T50D0"); + wait_serial_return_ok(); +} +//~~~~~~~~ Service position +void Pos_SRV(){ + lastSpeed = StepSpeed;StepSpeed = 50; + SMov[29]=SAdj[29]+1440;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1440;SMov[17]=SAdj[17]+1440;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1440;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1440;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1440;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1440;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1440;Send_Comm(); + StepSpeed = lastSpeed; +} +//~~~~~~~~ Initial position (adjust all init servo here) +void Pos_INT(){ + SAdj[29]=0;SAdj[30]=0;SAdj[31]=0;SAdj[17]=0;SAdj[18]=0;SAdj[19]=0;SAdj[1]=0;SAdj[2]=0;SAdj[3]=0;SAdj[5]=0;SAdj[6]=0;SAdj[7]=0;SAdj[13]=0;SAdj[14]=0;SAdj[15]=0;SAdj[25]=0;SAdj[26]=0;SAdj[27]=0;Send_Comm(); + Pos_SRV(); +} +//~~~~~~~~ Stop motion +void Move_STP(){ + SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1440;Send_Comm(); +} +//~~~~~~~~ Shake hand +void Move_SHK(){ + SMov[5]=SAdj[5]+1117;SMov[6]=SAdj[6]+2218;SMov[7]=SAdj[7]+1828;Send_Comm(); + SMov[7]=SAdj[7]+1246;Send_Comm(); + SMov[7]=SAdj[7]+1795;Send_Comm(); + SMov[7]=SAdj[7]+1182;Send_Comm(); + SMov[7]=SAdj[7]+1763;Send_Comm(); + SMov[7]=SAdj[7]+1117;Send_Comm(); + SMov[5]=SAdj[5]+1440;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;Send_Comm(); +} +//~~~~~~~~ Waving hand +void Move_WAV(){ + SMov[5]=SAdj[5]+1058;SMov[6]=SAdj[6]+1975;SMov[7]=SAdj[7]+2280;Send_Comm(); + SMov[5]=SAdj[5]+1478;Send_Comm(); + SMov[5]=SAdj[5]+1096;Send_Comm(); + SMov[5]=SAdj[5]+1478;Send_Comm(); + SMov[5]=SAdj[5]+1096;Send_Comm(); + SMov[5]=SAdj[5]+1478;Send_Comm(); + SMov[5]=SAdj[5]+1440;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;Send_Comm(); +} +//~~~~~~~~ adjust body higher +void Adj_HG(){ + SAdj[6]-=50;SAdj[7]+=25;SAdj[14]-=50;SAdj[15]+=25;SAdj[26]-=50;SAdj[27]+=25;SAdj[2]+=50;SAdj[3]-=25;SAdj[18]+=50;SAdj[19]-=25;SAdj[30]+=50;SAdj[31]-=25; + Pos_SRV(); +} +//~~~~~~~~ adjust body lower +void Adj_LW(){ + SAdj[6]+=50;SAdj[7]-=25;SAdj[14]+=50;SAdj[15]-=25;SAdj[26]+=50;SAdj[27]-=25;SAdj[2]-=50;SAdj[3]+=25;SAdj[18]-=50;SAdj[19]+=25;SAdj[30]-=50;SAdj[31]+=25; + Pos_SRV(); +} +//~~~~~~~~ adjust head up +void Adj_HU(){ + SAdj[6]-=50;SAdj[7]+=25;SAdj[26]+=50;SAdj[27]-=25;SAdj[2]+=50;SAdj[3]-=25;SAdj[30]-=50;SAdj[31]+=25; + Pos_SRV(); +} +//~~~~~~~~ adjust head down +void Adj_HD(){ + SAdj[6]+=50;SAdj[7]-=25;SAdj[26]-=50;SAdj[27]+=25;SAdj[2]-=50;SAdj[3]+=25;SAdj[30]+=50;SAdj[31]-=25; + Pos_SRV(); +} +//~~~~~~~~ adjust body left +void Adj_LF(){ + SAdj[6]+=50;SAdj[7]-=25;SAdj[14]+=50;SAdj[15]-=25;SAdj[26]+=50;SAdj[27]-=25;SAdj[2]+=50;SAdj[3]-=25;SAdj[18]+=50;SAdj[19]-=25;SAdj[30]+=50;SAdj[31]-=25; + Pos_SRV(); +} +//~~~~~~~~ adjust body right +void Adj_RG(){ + SAdj[6]-=50;SAdj[7]+=25;SAdj[14]-=50;SAdj[15]+=25;SAdj[26]-=50;SAdj[27]+=25;SAdj[2]-=50;SAdj[3]+=25;SAdj[18]-=50;SAdj[19]+=25;SAdj[30]-=50;SAdj[31]+=25; + Pos_SRV(); +} +//~~~~~~~~ adjust twist left +void Adj_TL(){ + SAdj[5]-=50;SAdj[13]-=50;SAdj[25]-=50;SAdj[1]-=50;SAdj[17]-=50;SAdj[29]-=50; + Pos_SRV(); +} +//~~~~~~~~ adjust twist right +void Adj_TR(){ + SAdj[5]+=50;SAdj[13]+=50;SAdj[25]+=50;SAdj[1]+=50;SAdj[17]+=50;SAdj[29]+=50; + Pos_SRV(); +} +//~~~~~~~~ move forward +void Move_FWD(){ + SMov[29]=SAdj[29]+1490;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1625;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1480;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1830;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1415;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1690;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1225;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1555;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1490;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1890;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1475;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1150;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1565;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1815;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1700;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1380;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1640;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1740;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1325;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1455;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1590;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1665;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1250;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1600;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1855;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1390;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1590;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1525;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1930;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1605;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1190;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1515;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1265;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1440;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1700;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1380;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1755;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1340;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); +} +//~~~~~~~~ move backward +void Move_BWD(){ + SMov[29]=SAdj[29]+1440;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1700;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1380;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1755;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1340;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1515;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1265;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1590;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1525;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1930;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1605;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1190;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1665;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1250;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1600;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1855;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1390;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1740;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1325;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1455;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1590;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1815;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1700;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1380;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1640;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1890;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1475;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1150;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1565;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1690;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1225;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1555;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1490;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1490;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1625;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1480;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1830;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1415;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); +} +//~~~~~~~~ turn left +void Move_LFT(){ + SMov[29]=SAdj[29]+1490;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1625;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1755;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1340;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1690;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1225;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1265;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1890;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1475;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1150;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1930;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1605;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1190;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1815;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1855;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1390;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1740;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1325;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1455;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1590;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1665;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1250;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1600;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1700;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1380;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1640;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1590;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1525;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1565;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1515;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1555;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1490;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1440;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1700;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1380;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1480;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1830;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1415;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + } +//~~~~~~~~ turn right +void Move_RGT(){ + SMov[29]=SAdj[29]+1440;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1700;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1380;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1480;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1830;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1415;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1515;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1555;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1490;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1590;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+790;SMov[19]=SAdj[19]+1890;SMov[1]=SAdj[1]+1525;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+2090;SMov[15]=SAdj[15]+990;SMov[25]=SAdj[25]+1565;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1665;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1250;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1600;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1700;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1380;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1640;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1740;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1325;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1450;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1455;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1590;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1815;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1400;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+790;SMov[3]=SAdj[3]+1890;SMov[5]=SAdj[5]+1855;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1530;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1390;SMov[26]=SAdj[26]+2090;SMov[27]=SAdj[27]+890;Send_Comm(); + SMov[29]=SAdj[29]+1890;SMov[30]=SAdj[30]+1440;SMov[31]=SAdj[31]+1490;SMov[17]=SAdj[17]+1475;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1150;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1930;SMov[6]=SAdj[6]+1440;SMov[7]=SAdj[7]+1440;SMov[13]=SAdj[13]+1605;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1190;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1690;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1550;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1225;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1780;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1680;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1265;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); + SMov[29]=SAdj[29]+1490;SMov[30]=SAdj[30]+790;SMov[31]=SAdj[31]+1990;SMov[17]=SAdj[17]+1625;SMov[18]=SAdj[18]+1440;SMov[19]=SAdj[19]+1440;SMov[1]=SAdj[1]+1300;SMov[2]=SAdj[2]+1440;SMov[3]=SAdj[3]+1440;SMov[5]=SAdj[5]+1630;SMov[6]=SAdj[6]+2090;SMov[7]=SAdj[7]+990;SMov[13]=SAdj[13]+1755;SMov[14]=SAdj[14]+1440;SMov[15]=SAdj[15]+1440;SMov[25]=SAdj[25]+1340;SMov[26]=SAdj[26]+1440;SMov[27]=SAdj[27]+1390;Send_Comm(); +} +//================================================================================= Servo Move end ======================================================================= diff --git "a/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/.keep" "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/.keep" new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git "a/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/mini D1 \351\223\276\346\216\245\345\233\276.jpg" "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/mini D1 \351\223\276\346\216\245\345\233\276.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..2689f7069d8c296c5f93869b02672e4be8ed6151 Binary files /dev/null and "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/mini D1 \351\223\276\346\216\245\345\233\276.jpg" differ diff --git "a/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\345\233\276\347\211\2071.png" "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\345\233\276\347\211\2071.png" new file mode 100644 index 0000000000000000000000000000000000000000..c68243261b95296b3deb2b7828efbcce5609808b Binary files /dev/null and "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\345\233\276\347\211\2071.png" differ diff --git "a/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\350\210\265\346\234\272\351\223\276\346\216\245\347\244\272\346\204\217\345\233\276.jpg" "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\350\210\265\346\234\272\351\223\276\346\216\245\347\244\272\346\204\217\345\233\276.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..92466480ff9194300d4ac3dcd66f69ac459f6dad Binary files /dev/null and "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\350\210\265\346\234\272\351\223\276\346\216\245\347\244\272\346\204\217\345\233\276.jpg" differ diff --git "a/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\351\223\276\346\216\245\347\224\265\350\267\257\345\233\276.jpg" "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\351\223\276\346\216\245\347\224\265\350\267\257\345\233\276.jpg" new file mode 100644 index 0000000000000000000000000000000000000000..214e2ea23e26474856820f901cbc6963c1173e7e Binary files /dev/null and "b/\347\272\277\350\267\257\347\273\204\350\243\205\345\233\276/\351\223\276\346\216\245\347\224\265\350\267\257\345\233\276.jpg" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/00.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/00.png" new file mode 100644 index 0000000000000000000000000000000000000000..2ea77c117f644e21ccadd23ad284475756faf7ef Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/00.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/01.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/01.png" new file mode 100644 index 0000000000000000000000000000000000000000..f95916934879f89a7c68ee93037030d5f768c9f8 Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/01.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/02.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/02.png" new file mode 100644 index 0000000000000000000000000000000000000000..d4c936b3a65717b97249c70dc9d94327d918ce6a Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/02.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/03.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/03.png" new file mode 100644 index 0000000000000000000000000000000000000000..1e619c2e9efc984e0885b25b2b09c10b522ecade Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/03.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/04.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/04.png" new file mode 100644 index 0000000000000000000000000000000000000000..5d5fa1f6fe98211695000af4b9cf4914fbd12eb3 Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/04.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/05.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/05.png" new file mode 100644 index 0000000000000000000000000000000000000000..0b6fb0865a7ffac280562a54d20eeeb42bbb4200 Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/05.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/06.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/06.png" new file mode 100644 index 0000000000000000000000000000000000000000..b9681802555155bde65a7e543daa7d7bc47f53b7 Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/06.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/07.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/07.png" new file mode 100644 index 0000000000000000000000000000000000000000..0c3a7d6ed08ebb1fa0695a94e399b631365fddae Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/07.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/08.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/08.png" new file mode 100644 index 0000000000000000000000000000000000000000..fd3d0c078d0a92c44b0e82b23841d184e7551248 Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/08.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/09.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/09.png" new file mode 100644 index 0000000000000000000000000000000000000000..2e84845c6d6b58725fb707c10605b378ba0614a5 Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/09.png" differ diff --git "a/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/10.png" "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/10.png" new file mode 100644 index 0000000000000000000000000000000000000000..f60a81807605d3ecd5587abd8b4a3f3c4f19f19b Binary files /dev/null and "b/\351\233\266\344\273\266\345\210\260\346\225\264\344\275\223/10.png" differ