四足蜘蛛机器人--制作过程记录

更新：老有人问舵机的编号，https://www.instructables.com/id/DIY-Spider-RobotQuad-robot-Quadruped/

原作者链接里面什么都有，有些人他就不看。不过这网站国内链接比较慢，我把那个图放在这了。

还有一个就是才开始安装，是这样的：

这是安装的舵机校正程序，就是把舵机都运动到90度，有需要自己下载：

https://wwa.lanzous.com/iQE8xfc380d 密码:e9r4

这是机器人的实物视频，UP是和我的同学，和我一起做的这个：https://www.bilibili.com/video/BV1Mb41127E6

===================================================================================

这学期我们开单片机的课，有个玩的好的朋友买了一个3D打印机，想搞点东西出来玩玩。我们商量了一下，决定做一个机器人，材料啥的越少越好，于是就决定做一个四足的蜘蛛机器人熟悉一下书本上的知识。

成品是这样的：

这个项目是参照网上的蜘蛛机器人做的，我们给小机器人加了一个蓝牙模块，可以通过手机进行遥控。

该项目原作者：https://www.thingiverse.com/thing:1009659（所需要的资料这个链接里面都有）

下面是机器人的需要的材料：

1.Arduino Nano 控制板

2.Nano 扩展板

3.电池盒

4.9g舵机12个

5.3D打印骨架（STL文件：https://download.csdn.net/download/c1664510416/11128822）

6.HC-06 蓝牙控制板

7.蓝牙调试串口APP（https://download.csdn.net/download/c1664510416/10701781）

目前机器人的部分程序还没有完成，写完之后会更新出来的。

----------------------------------------------------------------------------------------------------------------------------------------------------------------------

2019年5月8日更新：

有网友说字符串处理的库找不到，所以放在这里，需要的自取：

https://wwa.lanzoui.com/i42y87a

-----------------------------------------------------------------------------------------------------------------------------------------------------------------------

2018年12月11日更新：

四足运动代码（arduino）：

/*项目名称：遥控蜘蛛机器人*作者：小川子，许康元*日期：2018/09/30*参考：Sunfounder设计的爬行机器人*概述：通过组装蜘蛛机器人，感受Arduino的控制魅力，与单片机的强大。通过实践感受到自己能力的欠缺之处。*---该项目前期是用Arduino UNO进行制作的，但是由于UNO 太大了，于是在项目完成之后改用 Nano*   进行重新组装*---在安装机器人没有问题后再进行本程序的调试*---该程序动作函数参考Sunfounder设计的爬行机器人的演示代码*---请确保正确安装了库文件
*///库函数----------------------------------------------------------------------
#include <Servo.h>    //舵机控制库
#include <FlexiTimer2.h>//设置定时器去同时控制多个舵机
#include <SerialCommand.h> //处理串行命令的库
SerialCommand SCmd;
// C 0 1: stand
// C 0 0: sit
// C 1 x: forward x step
// C 2 x: back x step
// C 3 x: right turn x step
// C 4 x: left turn x step
// C 5 x: hand shake x times
// C 6 x: hand wave x times
#define C_STAND_SIT    0
#define C_FORWARD      1
#define C_BACKWARD     2
#define C_LEFT         3
#define C_RIGHT        4
#define C_SHAKE        5
#define C_WAVE         6
#define S_STRAIGHT     1
#define S_BACK         2  //舵机对象--------4条腿 每条腿3个舵机 一共12个舵机------------------------------
Servo servo[4][3];
//设置信号输出引脚
const int servo_pin[4][3] = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} };
/* 机器的尺寸和大小 ---------------------------------------------------------*/
const float length_a = 55;
const float length_b = 77.5;
const float length_c = 27.5;
const float length_side = 71;
const float z_absolute = -28;
/* 运动常量----------------------------------------------------*/
const float z_default = -40, z_up = 90, 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;
int  In1 = A1;
int  In2 = A2;
int  In3 = A3;
int  In4 = A4;
int  EnA = A0;
int  EnB = A5;
int Check_angle=1;
int straiht_angle=180;
/* 运动变量----------------------------------------------------*/
volatile float site_now[4][3];    //每只脚到末端的实时距离
volatile float site_expect[4][3]; //预计每只脚到末端的实时距离
float temp_speed[4][3];   //每个轴的速度  注意：需要在每次运动前重新计算
float speed_multiple = 1; //动作速度执行倍数
const 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;
float move_speed=leg_move_speed;     //动作速度
//π值
const float pi = 3.1415926;
/* 转动常量--------------------------------------------------------*/
//临时长度
const float 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);
//site for turn
const 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;
/* ---------------------------------------------------------------------------*///初始化函数
void setup()
{//启动串口Serial.begin(9600);Serial.println("Robot Starts Initialization");SCmd.addCommand("c", control_model);SCmd.addCommand("d",speed_set);//SCmd.addCommand("O",obstacle_model);//SCmd.addCommand("F",follow_model);SCmd.addCommand("s",straight_model);SCmd.setDefaultHandler(unrecognized);//初始4条腿的初始大小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(); //开始定时器Serial.println("Servo service strated");//初始化舵机servo_attach();//设置舵机接口函数Serial.println("Servos Initialized");Serial.println("Robot initialization Complete");//直流电机初始化straight_attach();Serial.println("Straight Initialized");
}void servo_attach(void)   //设置舵机连接串口
{for (int i = 0; i < 4; i++)  //4条腿{for (int j = 0; j < 3; j++)  //每条腿3个舵机{servo[i][j].attach(servo_pin[i][j]);  //设定舵机的接口delay(100);  //等待100毫秒}}
}void servo_detach(void) //舵机分离串口
{for (int i = 0; i < 4; i++)  //4条腿{ for (int j = 0; j < 3; j++) //每条腿3个舵机{servo[i][j].detach();  //使舵机与其接口分离delay(100);  //等待100毫秒}}
}//主循环函数接受外部数据
void loop(){SCmd.readSerial();
}//检测舵机是否达到直流电机运动角度
int check_angel(void){if(straiht_angle==0)return 1;elsereturn 0;
}
//设置直流电机运动四足角度
void set_angle(void){set_site(0, x_default + x_offset, y_start , 20);set_site(1, x_default + x_offset, y_start , 20);set_site(2, x_default + x_offset, y_start , 20);set_site(3, x_default + x_offset, y_start , 20);for (int i = 0; i < 4; i++){for (int j = 0; j < 3; j++){site_now[i][j] = site_expect[i][j];}}straiht_angle=0;
}
//设置运动速率
void speed_set(void){char *arg;int set_speed,no_do;Serial.println("Action:");arg = SCmd.next();set_speed= atoi(arg);speed_multiple=set_speed;arg = SCmd.next();no_do = atoi(arg);
}
//串口库函数的默认错误调用函数
void unrecognized(const char *command) {Serial.println("What?");
}  //设置直流电机接口
void straight_attach(void){pinMode(In1,OUTPUT);pinMode(In2,OUTPUT);pinMode(In3,OUTPUT);pinMode(In4,OUTPUT);
}//直行模式
void straight_model(void){char *arg;int action_model,no_do;Serial.println("Action:");arg = SCmd.next();action_model= atoi(arg);arg = SCmd.next();no_do = atoi(arg);switch(action_model){case S_STRAIGHT:Serial.println("Straight Format");analogWrite(EnA,255);analogWrite(EnB,255);sit();if(!check_angel()){set_angle();}s_straight();break;case S_BACK:Serial.println("Straight Back");analogWrite(EnA,255);analogWrite(EnB,255);sit();if(!check_angel()){set_angle();}s_back();break;default:Serial.println("Undefine");break;}
}//直流电机前进
void s_straight(void){digitalWrite(In1,HIGH);digitalWrite(In2,LOW);digitalWrite(In3,LOW);digitalWrite(In4,HIGH);
}
//直流电机退后
void s_back(void){digitalWrite(In1,LOW);digitalWrite(In2,HIGH);digitalWrite(In3,HIGH);digitalWrite(In4,LOW);
}//直流电机制动
void s_stop(void){straiht_angle=180;digitalWrite(In1,LOW);digitalWrite(In2,LOW);digitalWrite(In3,LOW);digitalWrite(In4,LOW);
}//循迹模式
void follow_model(void){}
//避障模式
void obstacle_model(void)
{}//控制模式
void control_model(void)
{char *arg;int action_mode, n_step; //动作模式，移动步数Serial.println("Action:");arg = SCmd.next();action_mode = atoi(arg);arg = SCmd.next();n_step = atoi(arg);switch (action_mode){case C_FORWARD:Serial.println("Step forward");if (!is_stand()){stand();}s_stop();step_forward(n_step);break;case C_BACKWARD:Serial.println("Step back");if (!is_stand()){stand();}s_stop();step_back(n_step);break;case C_LEFT:Serial.println("Turn left");if (!is_stand())stand();s_stop();turn_left(n_step);break;case C_RIGHT:Serial.println("Turn right");if (!is_stand())stand();s_stop();turn_right(n_step);break;case C_STAND_SIT:Serial.println("1:up,0:dn");if (n_step){s_stop();stand();} else{s_stop();sit();}break;case C_SHAKE:Serial.println("Hand shake");s_stop();hand_shake(n_step);break;case C_WAVE:Serial.println("Hand wave");s_stop();hand_wave(n_step);break;default:Serial.println("Error");break;}
}//是否站立
bool is_stand(void)
{if (site_now[0][2] == z_default)return true;elsereturn false;
}//坐下
void sit(void)
{move_speed = stand_seat_speed;for (int leg = 0; leg < 4; leg++){set_site(leg, KEEP, KEEP, z_boot);}wait_all_reach();
}//站立
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 moveset_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 moveset_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(unsigned int step)
{move_speed = spot_turn_speed;while (step-- > 0){if (site_now[2][1] == y_start){//leg 2&0 moveset_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 moveset_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 moveset_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 moveset_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 moveset_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 moveset_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();}}
}//身体左倾
void 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;float y_tmp;float z_tmp;move_speed = 1;if (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; j++){set_site(2, turn_x1, turn_y1, 50);wait_all_reach();set_site(2, turn_x0, turn_y0, 50);wait_all_reach();}set_site(2, x_tmp, y_tmp, z_tmp);wait_all_reach();move_speed = 1;body_left(15);}else{body_left(15);x_tmp = site_now[0][0];y_tmp = site_now[0][1];z_tmp = site_now[0][2];move_speed = body_move_speed;for (int j = 0; j < i; j++){set_site(0, turn_x1, turn_y1, 50);wait_all_reach();set_site(0, turn_x0, turn_y0, 50);wait_all_reach();}set_site(0, x_tmp, y_tmp, z_tmp);wait_all_reach();move_speed = 1;body_right(15);}
}//招手
void hand_shake(int i)
{float x_tmp;float y_tmp;float z_tmp;move_speed = 1;if (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; j++){set_site(2, x_default - 30, y_start + 2 * y_step, 55);wait_all_reach();set_site(2, x_default - 30, y_start + 2 * y_step, 10);wait_all_reach();}set_site(2, x_tmp, y_tmp, z_tmp);wait_all_reach();move_speed = 1;body_left(15);}else{body_left(15);x_tmp = site_now[0][0];y_tmp = site_now[0][1];z_tmp = site_now[0][2];move_speed = body_move_speed;for (int j = 0; j < i; j++){set_site(0, x_default - 30, y_start + 2 * y_step, 55);wait_all_reach();set_site(0, x_default - 30, y_start + 2 * y_step, 10);wait_all_reach();}set_site(0, x_tmp, y_tmp, z_tmp);wait_all_reach();move_speed = 1;body_right(15);}
}/*- 舵机服务/定时器中断功能/50Hz- 当设置site expected时，这个函数会移动到目标直线- 在设置expect之前，应该设置temp_speed[4][3]，确保直线移动，决定移动速度。---------------------------------------------------------------------------*/
void servo_service(void)
{sei();static float alpha, beta, gamma;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];elsesite_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++;
}void set_site(int leg, float x, float y, float z)  //设置某一条腿的最终坐标
{float length_x = 0, length_y = 0, length_z = 0;  //初始化float类型变量length_x,length_y,length_zif (x != KEEP)  //假若x轴不是保持状态length_x = x - site_now[leg][0];  //计算x轴长度if (y != KEEP)  //假若y轴不是保持状态length_y = y - site_now[leg][1];  //计算y轴长度if (z != KEEP)  //假若z轴不是保持状态length_z = z - site_now[leg][2];  //计算z轴长度float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2));   //计算宗长度temp_speed[leg][0] = length_x / length * move_speed * speed_multiple;  //计算对应腿的舵机1移动速度temp_speed[leg][1] = length_y / length * move_speed * speed_multiple;  //计算对应腿的舵机2移动速度temp_speed[leg][2] = length_z / length * move_speed * speed_multiple;  //计算对应腿的舵机3移动速度if (x != KEEP)  //假若x轴不是保持状态，则设置目标角度site_expect[leg][0] = x;if (y != KEEP)  //假若y轴不是保持状态，则设置目标角度site_expect[leg][1] = y;if (z != KEEP)  //假若z轴不是保持状态，则设置目标角度site_expect[leg][2] = z;
}/*单条腿部动作完成度检测
-----------------------------------------------------------------------*/
void wait_reach(int leg)  //等待某条腿动作完成函数
{while (1) //死循环if (site_now[leg][0] == site_expect[leg][0])    //等待目标腿的 舵机 1达到目标角度if (site_now[leg][1] == site_expect[leg][1])//等待目标腿的 舵机 2达到目标角度if (site_now[leg][2] == site_expect[leg][2])//等待目标腿的 舵机 3达到目标角度break;  //跳出循环
}/*四条动作完成度检测
-----------------------------------------------------------------------*/
void wait_all_reach(void) //等待全部腿动作完成函数
{for (int i = 0; i < 4; i++)   //依次等待4条腿动作完成wait_reach(i);
}/*- 从笛卡尔坐标系到极坐标转化- 数学模型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 w-z degreefloat 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 degreegamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);//trans degree pi->180alpha = alpha / pi * 180;beta = beta / pi * 180;gamma = gamma / pi * 180;
}/*- 用对应的极坐标控制舵机- 数学模型与事实相吻合的情况下- EEprom中存储的错误将被添加---------------------------------------------------------------------------*/
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;}//Serial.println(alpha);//Serial.println(beta);//Serial.println(gamma);servo[leg][0].write(alpha);  //设定对应腿上的舵机1旋转角度servo[leg][1].write(beta);   //设定对应腿上的舵机2旋转角度servo[leg][2].write(gamma);  //设定舵机3旋转角度
}