Arduino Spider 机器人（四足）

关于这个项目

大家好！这里有一个新的教程来指导你一步一步地制作这种超级神奇的电子项目，这就是“爬行机器人”，也被称为“蜘蛛机器人”或“四足机器人”。

由于每个人都注意到机器人技术的高速发展，因此我们决定将你们带到机器人技术和机器人制造的更高水平。我们前段时间开始做一些基本的电子项目和基本的机器人，比如 PICTO92 跟随机器人，目的是让你对电子产品有点熟悉，并发现自己能够发明自己的项目。

进入另一个层次，我们从这个机器人开始，它是概念中的一个基本机器人，但如果你深入了解它的程序，它会变得有点复杂。由于这些小工具在网上商店中非常昂贵，我们提供了此分步指南 指导你们制作自己的Spiderbot .

这个项目非常好用，在得到我们从JLCPCB订购的定制PCB以改善我们机器人的外观之后，并且本指南中有足够的文档和代码可以让您轻松创建爬虫。

我们只用了 7 天就完成了这个项目，只用了两天时间完成硬件制作和组装，然后用五天时间准备代码和 android 应用程序。以便通过它控制机器人。开始之前先来看看

您将学到什么：

• 根据您的项目功能选择正确的组件

• 制作电路以连接所有选定的组件

• 组装所有项目部件

• 机器人平衡的缩放

• 使用 Android 应用。通过蓝牙连接并开始操纵系统

第 1 步：什么是“蜘蛛机器人”？

正如它的名字所定义的那样，我们的机器人是吸管运动的基本表示，但它不会执行完全相同的身体运动，因为我们只使用四条腿而不是八条腿。

也被称为 四足动物 机器人有四条腿，利用这四条腿进行运动，因此每条腿的运动都与其他腿有关，以识别机器人的身体位置，并控制机器人的身体平衡。

腿式机器人比轮式机器人更好地处理地形，并且以各种动物性的方式移动。然而，这使得腿式机器人变得更加复杂，并且对于许多制造商来说更难使用。还有制造成本和制造商为了制造全身四足动物应该花费的高昂费用，因为它基于伺服电机或步进电机，两者都比可用于轮式机器人的直流电机贵。

优势

您会发现自然界中有很多四足动物，因为四足可以提供被动稳定性，或者无需主动调整位置即可保持站立的能力。机器人也是如此。四腿机器人比多腿机器人更便宜、更简单，但仍然可以达到稳定性。

第 2 步：伺服电机是主要执行器

维基百科中定义的伺服电机是一种旋转执行器或线性执行器，可以精确控制角或线性位置、速度和加速度。[1]它由一个合适的电机组成，该电机与传感器耦合以提供位置反馈。它还需要一个相对复杂的控制器，通常是专门为伺服电机设计的专用模块。

伺服电机不是特定类别的电机，尽管术语伺服电机通常用于指适用于闭环控制系统的电机。

一般来说，控制信号是方波脉冲串。控制信号的常用频率为 44Hz、50Hz 和 400Hz。正脉冲宽度决定了伺服位置。大约 0.5ms 的正脉冲宽度将导致伺服喇叭尽可能向左偏转（通常大约 45 到 90 度，具体取决于所讨论的伺服）。大约 2.5ms 到 3.0ms 的正脉冲宽度将导致舵机尽可能向右偏转。大约 1.5ms 的脉冲宽度将导致伺服将中性位置保持在 0 度。输出高电压通常在 2.5 伏到 10 伏之间（典型值为 3V）。输出低电压范围为-40mV至0V。

第 3 步：PCB 制作（由 JLCPCB 生产）

关于JLCPCB

JLCPCB（深圳市嘉力创电子科技发展有限公司）是中国最大的PCB样机企业，是一家专业从事PCB快速样机和小批量PCB生产的高新技术制造商。

凭借10多年PCB制造经验，JLCPCB拥有超过200,000家国内外客户，每天在线PCB原型制作和小批量PCB生产订单超过8,000个。年生产能力为200,000平方米。适用于各种 1 层、2 层或多层 PCB。 JLC是一家规模大、设备精良、管理严格、质量上乘的PCB专业制造商。

回到我们的项目

为了生产PCB，我比较了许多PCB生产商的价格，我选择了JLCPCB最好的PCB供应商和最便宜的PCB供应商来订购这个电路。我需要做的只是点击一些简单的点击来上传 gerber 文件并设置一些参数，比如 PCB 厚度颜色和数量，然后我只花了 2 美元就在五天后得到了我的 PCB。

因为它显示了相关原理图的图片，我使用了 Arduino Nano 来控制整个系统，我还设计了机器人蜘蛛形状以使这个项目更好。

您可以从此处获取电路 (PDF) 文件。正如您在上面的图片中所看到的，PCB 制造得非常好，我设计的 PCB 蜘蛛形状与我们设计的相同，所有标签和徽标都在那里指导我进行焊接步骤。

如果您想为相同的电路设计下订单，您也可以从这里下载该电路的 Gerber 文件。

第 4 步：配料

现在让我们回顾一下这个项目所需的必要组件，正如我所说的，我正在使用 Arduino Nano 来运行机器人四腿的所有 12 个伺服电机。该项目还包括一个用于显示 Cozmo 人脸的 OLED 显示器和一个通过安卓应用程序控制机器人的蓝牙模块。

为了创建此类项目，我们需要：

• - 我们从 JLCPCB 订购的 PCB

• - 12 个伺服电机，您记得每条腿 3 个伺服电机：https://amzn.to/2B25XbG

• - 一个 Arduino Nano：https://amzn.to/2MmZsVg

• - HC-06 蓝牙模块：https://amzn.to/2B1Z3CY

• - 一个 OLED 显示屏：https://amzn.to/2OySnyn

• - 5 毫米 RGB LED 灯：https://amzn.to/2B56hq3

• - 一些标题连接器：https://amzn.to/2nyZg7i

• - 您需要使用 3D 打印机打印机器人身体

第 5 步：机器人组装

现在我们已经准备好 PCB 并且所有组件都焊接得很好，之后我们需要组装机器人主体，过程非常简单，只需按照我展示的步骤进行操作，我们首先需要准备每条腿的一侧并制作一个 LED 我们需要两个伺服电机用于关节和带有这个小连接部件的 Coxa、股骨和胫骨打印部件。

关于机器人的身体部件，您可以从这里下载其 STL 文件。

从第一个舵机开始，将其放入插座中并用螺丝固定，然后将舵机轴旋转 180° 而不放置附件的螺丝，然后移动到下一部分，即股骨以将其连接到胫骨使用第一个伺服关节轴和附加件。完成腿的最后一步是放置第二个关节，我的意思是第二个伺服器来固定腿的第三部分，即 Coxa 部分。

现在对所有腿重复同样的事情，准备好四条腿。之后取上底盘并将其余的舵机放入插座中，然后将每条腿连接到适当的舵机。只有最后一个印刷部件是底部机器人底盘，我们将在其中放置电路板

第 6 步：Android 应用

说起android up它让你

通过蓝牙连接您的机器人进行前后移动和左右转弯，您还可以通过从该色轮中选择所需的颜色来实时控制机器人的灯光颜色。

您可以通过以下链接免费下载 Android 应用程序：这里

第 7 步：Arduino 代码和测试验证

现在我们的机器人几乎可以运行了，但我们需要先设置关节角度，所以上传设置代码，通过以 90 度角连接舵机，您可以将每个舵机放在正确的位置不要忘记连接 7V用于运行机器人的直流电池。

接下来我们需要上传主程序来使用android app控制机器人。您可以从以下链接下载这两个程序：

- 缩放伺服代码：下载链接- 蜘蛛机器人主程序：下载链接

上传代码后，我连接了 OLED 显示器，以显示我在主代码中制作的 Cozmo 机器人微笑。

正如您在上图中看到的那样，机器人遵循我智能手机发送的所有指令，并进行了一些其他改进以使其更好。

代码

• 蜘蛛机器人主要代码

蜘蛛机器人主要代码Arduino

/**************************************************** ****************************************************** ****************************************************** ********************** * - 作者：BELKHIR Mohamed * * - 职业：（电气工程师）MEGA DAS 所有者 * * - 主要用途：工业应用 * * - 版权所有 (c) 持有人：保留所有权利 * * - 许可证：BSD 2-Clause 许可证 * * - 日期：20/04/2017 * * ******************** ****************************************************** ****************************************************** ****************************************************/ /*********************************** 笔记 ************* *************************//// 允许以源代码和二进制形式重新分发和使用，无论是否修改//，只要满足以下条件满足条件：// * 源代码的重新分发必须保留上述版权声明、此// 条件列表和以下免责声明。// * 二进制形式的重新分发必须重新发布ce上述版权声明，//此条件列表和文档中的以下免责声明//和/或随分发提供的其他材料。//本软件由版权持有人和贡献者“按原样”提供//和任何明示或暗示的保证，包括但不限于// 对特定用途的适销性和适用性的暗示保证不承担任何责任/* */#include  //定义和控制舵机#include //设置定时器来管理所有舵机#include #include #include // OLED显示TWI地址#define OLED_ADDR 0x3CAdafruit_SSD1306 display(-1); /* 伺服 ----------------------------------------------- ---------------------*////为4条腿定义12个舵机舵机舵机[4][3];//定义舵机的端口const int舵机引脚[4] [3] ={ {11, 12, 13}, {2, 4, 7}, {14, 15, 16},{8, 9, 10} };/* 机器人的大小------ -------------------------------------------------- -*/const 浮点数 l ength_a =50;const float length_b =77.1;const float length_c =27.5;const float length_side =71;const float z_absolute =-28;/* 运动常量 ---------------- -------------------------------------*/const float z_default =-50, z_up =-30, z_boot =z_absolute;const float x_default =62, x_offset =0;const float y_start =0, y_step =40;const float y_default =x_default;/* 运动变量-------------- -------------------------------------*/volatile float site_now[4][3]; //每个legvolatile float结束的实时坐标site_expect[4][3]; //每个legfloat结束的预期坐标 temp_speed[4][3]; //每个轴的速度，需要在每次移动之前重新计算float move_speed; //移动速度float speed_multiple =1; //移动速度multipleconst float spot_turn_speed =4;const float leg_move_speed =8;const float body_move_speed =3;const float stand_seat_speed =1;volatile int rest_counter; //+1/0.02s，用于自动休息//函数的参数const float KEEP =255;//定义PI进行计算const float pi =3.1415926;/* 转向常量------------- ----------------------------------------------------*////温度长度常量浮点数temp_a =sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2));const float temp_b =2 * (y_start + y_step) + length_side;const float temp_c =sqrt(pow(2 * x_default + length_side) , 2) + pow(2 * y_start + y_step + length_side, 2));const float temp_alpha =acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b);//turnconst float turn_x1 =(temp_a - length_side) / 2;const float turn_y1 =y_start + y_step / 2;const float turn_x0 =turn_x1 - temp_b * cos(temp_alpha);const float turn_y0 =temp_b * sin (temp_alpha) - turn_y1 - length_side;const int lightR=3;const int lightG=5;const int lightB=6;int LedR=0;int LedG=0;int LedB=0;char SerialData; // 使用这个变量来读取通过串口接收到的每个caractereString data="";void setup() { Serial.begin(9600); display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR); display.clearDisplay();显示。显示（）；延迟（10000）； set_site(0, x_default - x_offset, y_start + y_step, z_boot); set_site(1, x_default - x_offset, y_start + y_step, z_boot); set_site(2, x_default + x_offset, y_start, z_boot); set_site(3, x_default + x_offset, y_start, z_boot); for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { site_now[i][j] =site_expect[i][j]; } } //启动伺服服务FlexiTimer2::set(20,servo_service); FlexiTimer2::start(); //初始化舵机servo_attach();站立();// 延迟(3000);// 坐();// 延迟(3000);// 站立();// 延迟(3000);快乐的（）;延迟（随机（500、1000））；塞拉（）；延迟（150）； enfado();延迟（随机（1000、3000））；塞拉（）；延迟（150）；入口（）；延迟（随机（1000、3000））；塞拉（）；延迟（150）； enfado1();延迟（随机（1000、3000））；塞拉（）；延迟（150）；特里斯特（）；延迟（随机（1000、3000））；塞拉（）；延迟（150）； abre();延迟（随机（500、3000））；塞拉（）；延迟（150）；快乐的（）; delay (random (500, 1000));}void loop() { while(Serial.available()) // 当串行数据可用时，我们将其存储 { delay(10); } SerialData=Serial.read(); if(SerialData=='b') LedR=Serial.parseInt(); else if(SerialData=='g') LedG=Serial.parseInt();否则 if(SerialData=='r') LEDB=Serial.parseInt();否则数据+=串行数据； } if(data=="f") // 如果存储的数据是向前移动 { cierra();延迟（150）；快乐的（）; step_forward(1); } if(data=="p") // 如果存储的数据是向后移动 { cierra();延迟（150）；特里斯特（）；后退（1）； } if(data=="l") // 如果存储的数据是汽车左转 { cierra();延迟（150）； enfado1();左转（1）； } if(data=="m") // 如果存储的数据是汽车右转 { cierra();延迟（150）； enfado();右转（5）； } 数据="";模拟写入（lightR，LedR）；模拟写入（lightG，LedG）；模拟写入（lightB，LEDB）；}void 伺服附加（void）{ for（int i =0；i <4；i++）{ for（int j =0；j <3；j++）{伺服[i][j]。附加（伺服销[i][j]）；延迟（100）； } }}voidservo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { 伺服[i][j].detach();延迟（100）； } }}无效坐（无效）{ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach();}/* - Stand - 阻塞函数 --------------------- --------------------------------------*/void stand(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_default); } wait_all_reach();}/* - 点向左转 - 阻塞功能 - 参数步步要转------------------------ ------------------------------------------------*/ void turn_left(unsigned int step){ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //leg 3&1 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_default); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start, z_up); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&2 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_default); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_up); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - 点向右转 - 阻挡功能 - 参数步步想要转------------------------------ ---------------------------------------------*/void turn_right(无符号整数步）{ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[2][1] ==y_start) { //leg 2&0 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_up); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&3 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_up); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - 前进 - 阻塞功能 - 参数步骤想要走的步骤-------------------------------- -----------------------------------------------------*/void step_forward(unsigned int step){ move_speed =leg_move_speed; while (step--> 0) { if (site_now[2][1] ==y_start) { //leg 2&1 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&3 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - 返回 - 阻塞功能 - 参数步骤想要走的步骤-------------------------------- ----------------------------------------------------*/void step_back(unsigned int step){ move_speed =leg_move_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //leg 3&0 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&2 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}// 通过 RegisHsuvoid 添加 body_left(int i){ set_site(0, site_now[0][0] + i, KEEP, KEEP); set_site(1, site_now[1][0] + i, KEEP, KEEP); set_site(2, site_now[2][0] - i, KEEP, KEEP); set_site(3, site_now[3][0] - i, KEEP, KEEP); wait_all_reach();}void body_right(int i){ set_site(0, site_now[0][0] - i, KEEP, KEEP); set_site(1, site_now[1][0] - i, KEEP, KEEP); set_site(2, site_now[2][0] + i, KEEP, KEEP); set_site(3, site_now[3][0] + i, KEEP, KEEP); wait_all_reach();}void hand_wave(int i){ float x_tmp;浮动 y_tmp;浮动 z_tmp;移动速度 =1;如果 (site_now[3][1] ==y_start) { body_right(15); x_tmp =site_now[2][0]; y_tmp =site_now[2][1]; z_tmp =site_now[2][2]; move_speed =body_move_speed; for (int j =0; j  i / 4)// move_speed =body_dance_speed * 2;// if (j> i / 2)// move_speed =body_dance_speed * 3;/ / set_site(0, KEEP, y_default - 20, KEEP);// set_site(1, KEEP, y_default + 20, KEEP);// set_site(2, KEEP, y_default - 20, KEEP);// set_site(3, KEEP, y_default + 20, KEEP);// wait_all_reach();// set_site(0, KEEP, y_default + 2 0, KEEP);// set_site(1, KEEP, y_default - 20, KEEP);// set_site(2, KEEP, y_default + 20, KEEP);// set_site(3, KEEP, y_default - 20, KEEP); // wait_all_reach();// }// move_speed =body_dance_speed;// head_down(30);//}/* - 微伺服服务/定时器中断函数/50Hz - 当设置站点预期时，该函数将终点移动到它在直线上 - temp_speed[4][3] 应该在设置期望站点之前设置，它确保终点沿直线移动，并决定移动速度。 -------------------------------------------------- -------------------------*/voidservo_service(void){ sei();静态浮点阿尔法、贝塔、伽马； for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { if (abs(site_now[i][j] - site_expect[i][j])>=abs(temp_speed[i][j])) site_now[i][j] +=temp_speed[i][j];否则 site_now[i][j] =site_expect[i][j]; } cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, alpha, beta, gamma); } rest_counter++;}/* - 设置端点之一的期望站点 - 该函数将同时设置 temp_speed[4][3] - 非阻塞函数 --------------- -------------------------------------------------- ----------*/void set_site(int leg, float x, float y, float z){ float length_x =0, length_y =0, length_z =0; if (x !=KEEP) length_x =x - site_now[leg][0]; if (y !=KEEP) length_y =y - site_now[leg][1]; if (z !=KEEP) length_z =z - site_now[leg][2];浮点长度 =sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); temp_speed[leg][0] =length_x / 长度 * move_speed * speed_multiple； temp_speed[leg][1] =length_y / 长度 * move_speed * speed_multiple； temp_speed[leg][2] =length_z / 长度 * move_speed * speed_multiple； if (x !=KEEP) site_expect[leg][0] =x; if (y !=KEEP) site_expect[leg][1] =y; if (z !=KEEP) site_expect[leg][2] =z;}/* - 等待端点之一移动到期望站点 - 阻塞功能 ----------------- -------------------------------------------------- --------*/void wait_reach(int leg){ while (1) if (site_now[leg][0] ==site_expect[leg][0]) if (site_now[leg][1] ==site_expect[leg][1]) if (site_now[leg][2] ==site_expect[leg][2]) break;}/* - 等待所有端点移动到预期站点 - 阻塞函数 ---- -------------------------------------------------- ---------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - trans site from cartesian to polar - mathematical model 2/2 ------------------------------------- --------------------------------------*/void cartesian_to_polar(volatile float &alpha, volatile float &beta , volatile float &gamma, volatile float x, volatile float y, volatile float z){ //calculate wz degree float v, w; w =(x>=0 ? 1 :-1) * (sqrt(pow(x, 2) + pow(y, 2))); v =w - length_c; alpha =atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2))); beta =acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b); //calculate x-y-z degree gamma =(w>=0) ? atan2(y, x) :atan2(-y, -x); //trans degree pi->180 alpha =alpha / pi * 180; beta =beta / pi * 180; gamma =gamma / pi * 180;}/* - trans site from polar to microservos - mathematical model map to fact - the errors saved in eeprom will be add ---------------------------------------------------------------------------*/void polar_to_servo(int leg, float alpha, float beta, float gamma){ if (leg ==0) { alpha =90 - alpha; beta =beta; gamma +=90; } else if (leg ==1) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==2) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==3) { alpha =90 - alpha; beta =beta; gamma +=90; } servo[leg][0].write(alpha); servo[leg][1].write(beta); servo[leg][2].write(gamma);}void abre() { display.clearDisplay(); display.fillCircle (50, 15, 12, WHITE); //ojo izquierdo grande display.fillCircle (82, 20, 7, WHITE); //ojo derecho pequeo display.display();}void cierra() { display.clearDisplay(); display.drawFastHLine(40, 15, 20, WHITE); display.drawFastHLine(72, 20, 15, WHITE); display.display();}void entorna() { display.clearDisplay(); display.fillCircle (42, 10, 20, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 15, WHITE); //ojo derecho pequeo display.fillRect (0, 0, 128, 15, BLACK); //ceja superior display.fillRect (0, 40, 128, 15, BLACK); //ceja inferior display.display();}void triste() { display.clearDisplay(); display.fillCircle (42, 10, 17, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 17, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 0, 35, 78, 0, BLACK); //ceja superior display.fillTriangle (50, 0, 128, 35, 128, 0, BLACK); //ceja superior display.display();}void happy() { display.clearDisplay(); display.fillCircle (42, 25, 15, WHITE); //ojo izquierdo grande display.fillCircle (82, 25, 15, WHITE); //ojo derecho pequeo display.fillCircle (42, 33, 20, BLACK); //ojo izquierdo grande display.fillCircle (82, 33, 20, BLACK); //ojo derecho pequeo display.display();}void enfado() { display.clearDisplay(); display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 54, 26, 118, 0, BLACK); //ceja superior display.display();}void enfado1() { display.clearDisplay(); display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 65, 15, 120, 0, BLACK); //ceja superior display.display();}

定制零件和外壳

示意图