亲,“电路城”已合并升级到更全、更大、更强的「新与非网」。点击查看「新与非网」

本网页已闲置超过3分钟,按键盘任意键或点击空白处,即可回到网页

家养四足动物—蜘蛛机器人

发布时间:2022-11-08
分享到:

家养四足动物—蜘蛛机器人

发布时间:2022-11-08
分享到:

这个项目的样子是一个四足动物。“四足机器人”的意思是四条腿的机器人,并且它看起来像四条腿的蜘蛛,所以让我们来学习蜘蛛是如何走路的,并尝试用机器人复制它。

第一步:所需组件

  • 1 X Arduino Mega或Arduino Uno
  • 1 X钻孔PCB
  • 12 X伺服电机(9g)
  • 1 X HC-SR04超声波传感器
  • 4 × RGB led
  • 纸板

第二步:保持CG

重心(CG)是影响步行的主要因素。重心保持在身体的中心以保持平衡,如果CG在一定的范围内偏离中心,那么平衡就会受到影响,从而导致摔倒

让我们看看如何在走路时保持CG。

如果每条腿都是45度,那么重心就会完全集中在中心,但如果我们移动任何一条腿,重心就会移到那一边,所以它就会倒在那一边。

因此,为了避免这种情况,机器人的两端腿都保持在大于45度的角度,所以三条腿将形成一个三角形,CG将在其中,第4条腿将自由移动,CG将保持在一个三角形内。

第三步:步行

  • 这是开始的姿势,两条腿(C, D)向一侧伸出,另两条腿(A, B)向内拉。
  • 右上腿(B)抬起并伸出,在机器人的前方很远的地方。
  • 所有的腿向后移动,身体向前移动。
  • 后左腿(D)抬起并向前迈一步。这个位置是起始位置的镜像。
  • 左上角的腿(B)抬起并伸出,在机器人的前方很远。
  • 同样,所有的腿向后移动,身体向前移动。
  • 后右腿抬起(B),并退回身体,使我们回到开始的位置。

第四步:构建身体

第五步:电路连接

Arduino mega有15个pwm引脚,其中12个用于伺服连接,3个用于RBG led,任意两个引脚用于超声传感器。

第六步:伺服初始化

  • 将程序上传到arduino mega,根据图片开始组装腿

#include <Servo.h>   
Servo servo[4][3];
//define servos' ports
const int servo_pin[4][3] = { {10,11,2}, {3,4,5}, {6,7,8}, {9, 12, 13} };
void setup()
{
 //initialize all servos
 for (int i = 0; i < 4; i++)
 {
   for (int j = 0; j < 3; j++)
   {
     servo[i][j].attach(servo_pin[i][j]);
     delay(20);
   }
 }
}
void loop(void)
{
 for (int i = 0; i < 4; i++)
 {
   for (int j = 0; j < 3; j++)
   {
     servo[i][j].write(90);
     delay(20);
   }
 }
}

第七步:完成

 /* Includes ------------------------------------------------------------------*/
#include <Servo.h>    //to define and control servos
#include <FlexiTimer2.h>//to set a timer to manage all servos
#define ledred 46
#define ledblue 44
#define ledgreen 45
/* Servos --------------------------------------------------------------------*/
//define 12 servos for 4 legs
Servo servo[4][3];
//define servos' ports
const int servo_pin[4][3] = { {2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12, 13} };
/* Size of the robot ---------------------------------------------------------*/
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;
/* Constants for movement ----------------------------------------------------*/
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;
/* variables for movement ----------------------------------------------------*/
volatile float site_now[4][3];    //real-time coordinates of the end of each leg
volatile float site_expect[4][3]; //expected coordinates of the end of each leg
float temp_speed[4][3];   //each axis' speed, needs to be recalculated before each movement
float move_speed;     //movement speed
float speed_multiple = 1; //movement speed multiple
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, for automatic rest
//functions' parameter
const float KEEP = 255;
//define PI for calculation
const float pi = 3.1415926;
/* Constants for turn --------------------------------------------------------*/
//temp length
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;
/* ---------------------------------------------------------------------------*/
/*
 - setup function
  ---------------------------------------------------------------------------*/
void setup()
{
  pinMode(ledred,OUTPUT);
pinMode(ledblue,OUTPUT);
pinMode(ledgreen,OUTPUT);
 //start serial for debug
 Serial.begin(115200);
 Serial.println("Robot starts initialization");
 //initialize default parameter
 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];
   }
 }
 //start servo service
 FlexiTimer2::set(20, servo_service);
 FlexiTimer2::start();
 Serial.println("Servo service started");
 //initialize servos
 servo_attach();
 Serial.println("Servos initialized");
 Serial.println("Robot initialization Complete");
}
void servo_attach(void)
{
 for (int i = 0; i < 4; i++)
 {
   for (int j = 0; j < 3; j++)
   {
     servo[i][j].attach(servo_pin[i][j]);
     delay(100);
   }
 }
}
void servo_detach(void)
{
 for (int i = 0; i < 4; i++)
 {
   for (int j = 0; j < 3; j++)
   {
     servo[i][j].detach();
     delay(100);
   }
 }
}
/*
 - loop function
  ---------------------------------------------------------------------------*/
void loop()
{
 analogWrite(ledred,255);
 Serial.println("Stand");
 stand();
  delay(2000);
 analogWrite(ledred,0);
 analogWrite(ledblue,255);
 Serial.println("Step forward");
 step_forward(5);
 delay(2000);
 analogWrite(ledblue,0);
 analogWrite(ledgreen,255);
 Serial.println("Step back");
 step_back(5);
 delay(2000);
 analogWrite(ledgreen,0);
 analogWrite(ledred,255);
 analogWrite(ledblue,255);
  Serial.println("Turn left");
 turn_left(5);
 delay(2000);
 analogWrite(ledgreen,255);
 analogWrite(ledred,0);
 analogWrite(ledblue,255);
 Serial.println("Turn right");
 turn_right(5);
 delay(2000);
  analogWrite(ledgreen,255);
 analogWrite(ledred,255);
 analogWrite(ledblue,0);
 Serial.println("Hand wave");
 hand_wave(3);
 delay(2000);
 Serial.println("Hand wave");
 hand_shake(3);
 delay(2000);
 int x=100;
 for(int i=0;i<5;i++)
 {
  analogWrite(ledgreen,255);
 analogWrite(ledred,255);//white
 analogWrite(ledblue,255);
 delay(x);
  analogWrite(ledgreen,255);//yellow
 analogWrite(ledred,255);
 analogWrite(ledblue,0);
 delay(x);
  analogWrite(ledgreen,255);//cyan
 analogWrite(ledred,0);
 analogWrite(ledblue,255); 
 delay(x); 
 analogWrite(ledgreen,0);
 analogWrite(ledred,255);//purple
 analogWrite(ledblue,255); 
 delay(x) ;
 analogWrite(ledgreen,0);
 analogWrite(ledred,255);//red
 analogWrite(ledblue,0); 
 delay(x); 
 analogWrite(ledgreen,0);//blue
 analogWrite(ledred,0);
 analogWrite(ledblue,255); 
 delay(x); 
 analogWrite(ledgreen,255);
 analogWrite(ledred,0);
 analogWrite(ledblue,0); //green
 delay(x) ;
 }
 analogWrite(ledgreen,0);
 analogWrite(ledred,0);
 analogWrite(ledblue,0);
 //Serial.println("Body dance");
 //body_dance(10);
// delay(2000);    
 //Serial.println("Sit");
// sit();
 delay(1000);
}
/*
 - sit
 - blocking function
  ---------------------------------------------------------------------------*/
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();
}
/*
 - stand
 - blocking function
  ---------------------------------------------------------------------------*/
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();
}
/*
 - spot turn to left
 - blocking function
 - parameter step steps wanted to turn
  ---------------------------------------------------------------------------*/
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();
   }
 }
}
/*
 - spot turn to right
 - blocking function
 - parameter step steps wanted to turn
  ---------------------------------------------------------------------------*/
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();
   }
 }
}
/*
 - go forward
 - blocking function
 - parameter step steps wanted to go
  ---------------------------------------------------------------------------*/
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();
   }
 }
}
/*
 - go back
 - blocking function
 - parameter step steps wanted to go
  ---------------------------------------------------------------------------*/
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();
   }
 }
}
// add by RegisHsu
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);
 }
}
void head_up(int i)
{
 set_site(0, KEEP, KEEP, site_now[0][2] - i);
 set_site(1, KEEP, KEEP, site_now[1][2] + i);
 set_site(2, KEEP, KEEP, site_now[2][2] - i);
 set_site(3, KEEP, KEEP, site_now[3][2] + i);
 wait_all_reach();
}
void head_down(int i)
{
 set_site(0, KEEP, KEEP, site_now[0][2] + i);
 set_site(1, KEEP, KEEP, site_now[1][2] - i);
 set_site(2, KEEP, KEEP, site_now[2][2] + i);
 set_site(3, KEEP, KEEP, site_now[3][2] - i);
 wait_all_reach();
}
void body_dance(int i)
{
 float x_tmp;
 float y_tmp;
 float z_tmp;
 float body_dance_speed = 2;
 sit();
 move_speed = 1;
 set_site(0, x_default, y_default, KEEP);
 set_site(1, x_default, y_default, KEEP);
 set_site(2, x_default, y_default, KEEP);
 set_site(3, x_default, y_default, KEEP);
 wait_all_reach();
 //stand();
 set_site(0, x_default, y_default, z_default - 20);
 set_site(1, x_default, y_default, z_default - 20);
 set_site(2, x_default, y_default, z_default - 20);
 set_site(3, x_default, y_default, z_default - 20);
 wait_all_reach();
 move_speed = body_dance_speed;
 head_up(30);
 for (int j = 0; j < i; j++)
 {
   if (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 + 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();
 }
 move_speed = body_dance_speed;
 head_down(30);
}
/*
 - microservos service /timer interrupt function/50Hz
 - when set site expected,this function move the end point to it in a straight line
 - temp_speed[4][3] should be set before set expect site,it make sure the end point
  move in a straight line,and decide move speed.
  ---------------------------------------------------------------------------*/
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++;
}
/*
 - set one of end points' expect site
 - this founction will set temp_speed[4][3] at same time
 - non - blocking function
  ---------------------------------------------------------------------------*/
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];
 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;
 temp_speed[leg][1] = length_y / length * move_speed * speed_multiple;
 temp_speed[leg][2] = length_z / length * 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;
}
/*
 - wait one of end points move to expect site
 - blocking function
  ---------------------------------------------------------------------------*/
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;
}
/*
 - wait all of end points move to expect site
 - blocking function
  ---------------------------------------------------------------------------*/
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 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;
}
/*
 - 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);
}

如果您对此项目有任何想法、意见或问题,请在下方留言。

以上内容翻译自网络,原作者:Renga,如涉及侵权,可联系删除。

加入微信技术交流群

技术交流,职业进阶

关注与非网服务号

获取电子工程师福利

加入电路城 QQ 交流群

与技术大牛交朋友

讨论