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

   更新：老有人问舵机的编号，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://www.lanzous.com/i42y87a

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

2018年12月11日更新：

四足运动代码（arduino）：

/*项目名称：遥控蜘蛛机器人
 *作者：小川子，许康元
 *日期：2018/09/30
 *参考：Sunfounder设计的爬行机器人
 *概述：通过组装蜘蛛机器人，感受Arduino的控制魅力，与单片机的强大。通过实践感受到自己能力的欠缺之处。
 *---该项目前期是用Arduino UNO进行制作的，但是由于UNO 太大了，于是在项目完成之后改用 Nano
 *   进行重新组装
 *---在安装机器人没有问题后再进行本程序的调试
 *---该程序动作函数参考Sunfounder设计的爬行机器人的演示代码
 *---请确保正确安装了库文件
*/

//库函数----------------------------------------------------------------------
#include     //舵机控制库
#include //设置定时器去同时控制多个舵机
#include  //处理串行命令的库
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;
  else
    return 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;
  else
    return 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 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(unsigned int step)
{
  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();
    }
  }
}

//身体左倾
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];
      else
        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++;
}

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_z
  
  if (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 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;
}

/*
  - 用对应的极坐标控制舵机
  - 数学模型与事实相吻合的情况下
  - 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旋转角度
}