In this tutorial, we will learn how to make an Arduino Spider Robot. The mechanical components of this innovative spider robot are exclusively crafted through 3D printing techniques. The tutorial will guide you through the assembly and programming using an Arduino NANO, enabling you to understand and build your own quadruped robot.
Update Version – 3 in 1 Spider Robot – How To Make 3 in 1 Spider Robot
You can watch the following video or read the written tutorial below.
Overview
All the mechanical parts of the spider robot need to be 3d printed. The details are below. An Arduino Nano board and twelve SG90 servo motors are used for this. Two 18650 li-ion batteries and a step-down voltage module are used to provide power.
While this spider robot is operating, you may observe some repetitive movements of the same kind. If that’s,
- stand up, wait 2 sec
- step forward 5 steps, wait 2 sec
- backward 5 steps, wait 2 sec
- turn right, wait 2 sec
- turn left, wait 2 sec
- wave the hand,, wait 2 sec
- shake the hand, wait 2 sec
- body dancing
- sit down, wait 2 sec
- back to 1
Components Needed
Before we begin, make sure you have gathered all the necessary components. The purchase links are in the description of my YouTube video.
- 3D-printed spider robot parts (chassis, legs, connectors)
- 12 x SG90 servo motors
- Arduino Nano or compatible board
- Dot board
- 40 Pin Female headers
- 2 x 18650 Li-Ion batteries and holder
- Step-down voltage module (Buck converter)
- Switch
- Jumper wires
- Screws and nuts for assembly
- Screwdriver and assorted tools
- Glue or adhesive for securing components
- USB cable for programming the Arduino Nano
3D Print the Robot Parts
To 3D print the essential components for the Spider Robot, allocate approximately seven to eight hours of printing time. Configure your 3D printer with a layer height of 0.2mm, infill set at 15%, and a printing speed of 65 for optimal results. Access the provided link to download the necessary STL files required for the 3D printing process. These files contain the specific parts essential for constructing the robot. Once downloaded, load the STL files into your preferred slicing software and initiate the printing process. Following these parameters and utilizing the recommended settings ensures the production of durable and accurately detailed parts for assembling the Arduino Spider Robot.
Print parts list:
- 1x body_d.stl
- 1x body_u.stl
- 2x coxa_l.stl
- 2x coxa_r.stl
- 2x tibia_l.stl
- 2x tibia_r.stl
- 4x femur_1.stl
- 8x s_hold.stl
Assemble the Legs and Servos
According to this image, I named Spider Robot’s legs.
- Here, the first leg and the fourth leg are the same. If you look closely, you can see it.
- The second leg and the third leg are the same
- It will be convenient for you to watch the YouTube video before making the feet
Let’s make the fourth and first legs first. Since both are similar, I will describe how to make one leg.First understand the three 3d printing parts in the images below
There are two parts coxa I and coxa r. It selects the coxa I part for the first and fourth legs
All four parts of Femur are same, so take any part.
There are two parts Tibia I and Tibia r. It selects the Tibia I part for the first and fourth legs
Two S Holds are also required.
To make the first leg, select the 3D parts as above. Next we need three SG90 servo motors and screws to fix them.
Install the servo motor as shown in the image
As shown in the figure, the shaft of the servo motor should be aligned with the part connected to the s hold.
Fix the motor with two screws
As shown in the image, the servo motor with tibia i should be connected to the long part.
As shown in the image, S hold should be connected to the long part.
Attach the S hold to the Tibia I with screws. If there is no screw, stick it with glue.
As shown in the figure, the shaft of the servo motor should be aligned with the part connected to the s hold.
Fix it like this
Make the first leg as shown above. Make the fourth leg in the same way.
How to make the second and third leg is shown below.
First understand the three 3d printing parts in the images below.
There are two parts coxa I and coxa r. It selects the coxa r part for the second and third legs
All four parts of Femur are same, so take any part.
There are two parts Tibia I and Tibia r. It selects the Tibia r part for the second and third legs.
As shown in the figure, the shaft of the servo motor should be aligned with the part connected to the s hold.
As shown in the image, the servo motor with tibia i should be connected to the long part.
As shown in the image, S hold should be connected to the long part.
Attach the S hold to the Tibia I with screws. If there is no screw, stick it with glue.
As shown in the figure, the shaft of the servo motor should be aligned with the part connected to the s hold.
Make the second leg as shown above. Make the third leg in the same way.
If the four legs are fixed, the next step is to attach the four legs to the body.
Assemble the Body
Before installing the legs of the spider robot, the four servo motors must be installed on the Body u
This is the Body u 3d printing part
Connect the four servo motors like this.
Next is to connect the four legs.
Spider Robot Circuit Diagram
The next stage is connecting the electronics. This is a rather complex circuit. As depicted in the circuit diagram, the procedure for connecting the module wires is further explained below. You can download this circuit diagram. For that, click on the Download button below and download it.
Servo Motor Number
Wiring and Electronics
This video explains the process of completing the circuit according to the diagram and provides guidance on making power connections. Connect the 12 SG90 servo motors to the Arduino Nano. You may need a servo driver board or use a combination of power and signal wires to control them.
Connect the two 18650 Li-Ion batteries to the step-down voltage module. Set the output voltage to the operating voltage of the Arduino Nano (usually 7V).
Connect the output of the step-down voltage module to the Arduino Nano’s VIN pin and ground (GND) pin to provide a stable power supply. Connect the signal wires of the servo motors to the appropriate digital pins on the Arduino Nano. Make sure to use a common ground connection.
- Connect the 12 SG90 servo motors to the Arduino Nano. You may need a servo driver board or use a combination of power and signal wires to control them.
- Connect the two 18650 Li-Ion batteries to the step-down voltage module. Set the output voltage to the operating voltage of the Arduino Nano (usually 7V).
- Connect the output of the step-down voltage module to the Arduino Nano’s VIN pin and ground (GND) pin to provide a stable power supply.
- Connect the signal wires of the servo motors to the appropriate digital pins on the Arduino Nano. Make sure to use a common ground connection.
Upload the Arduino Sketch 1 ( explains how to locate the initial position for the legs)
This video explains how to locate the initial position for the legs.
Scaling_servos Arduino Code
//Robot Lk
#include <Servo.h>
Servo servo[4][3];
//define servos' ports
const int servo_pin[4][3] = { {11, 12, 13}, {2, 4, 7}, {14, 15, 16},{8, 9, 10} };
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);
}
}
}
Code language: PHP (php)
When the above code is uploaded, the foot position of the spider robot should be like this.
Upload the Arduino Sketch
Download Libraries before uploading the code. Download from below.
In the Arduino IDE, navigate to Sketch > Include Library > Add . ZIP Library. At the top of the drop-down list, select the option to “Add . ZIP Library”.
After that, copy the following Arduino code and paste it into the new sketch in the Arduino IDE. Select the board and port and upload the code. Before uploading the code to Arduino NANO board select its processor as old bootloader. The bootloader selected via Tools > Processor > ATmega328P (Old Bootloader) is the “ATmegaBOOT” bootloader. If this is difficult to do, watch a tutorial video.
You can download the Arduino code and open it directly through the Arduino IDE. Click the Download button below to download the Arduino code
#include <Servo.h> //to define and control servos
#include <FlexiTimer2.h>//to set a timer to manage all servos
/* Servos --------------------------------------------------------------------*/
//define 12 servos for 4 legs
Servo servo[4][3];
//define servos' ports
const int servo_pin[4][3] = { {11, 12, 13}, {2, 4, 7}, {14,15, 16}, {8, 9, 10} };
/* 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()
{
//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()
{
Serial.println("Stand");
stand();
delay(2000);
Serial.println("Step forward");
step_forward(5);
delay(2000);
Serial.println("Step back");
step_back(5);
delay(2000);
Serial.println("Turn left");
turn_left(5);
delay(2000);
Serial.println("Turn right");
turn_right(5);
delay(2000);
Serial.println("Hand wave");
hand_wave(3);
delay(2000);
Serial.println("Hand wave");
hand_shake(3);
delay(2000);
Serial.println("Body dance");
body_dance(10);
delay(2000);
Serial.println("Sit");
sit();
delay(5000);
}
/*
- 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);
}
Code language: PHP (php)
This video explains – Upload Arduino Code With Basic Moves
Conclusion
In conclusion, this tutorial provides a comprehensive guide for constructing an Arduino Spider Robot, a quadrupedal marvel crafted through 3D printing and powered by Arduino Nano. The process involves downloading the necessary library, 3D printing the robot parts with specific printing parameters, and assembling the body by installing servo motors meticulously. The integration of the scaling_servos Arduino code ensures precise control over the robot’s movements. Following each step, including board and port selection and code upload, is crucial for a successful project.