This code is written for an Arduino-based robotics project and...

September 15, 2025 at 06:03 AM

//  --------                 --------
// |  D9    |               |  D7    |
// | joint9 |               | joint7 |
//  ----- --------     --------- -----
//       |  D8    |   |  D6    |
//       | joint8 |   | joint6 |
//        --------     --------
//       |  D2    |  |   D4    |
//       | joint2 |  |  joint4 |
//  ----- --------    --------- -----
// |  D3    |               |  D5    |
// | joint3 |               | joint5 |
//  --------                 --------
//                Front



#include <Servo.h>

// Декларация сервопривода
Servo joint2;  // переднее правое бедро
Servo joint3;  // Правая передняя нога
Servo joint4;  // левое переднее бедро
Servo joint5;  // Левая передняя нога
Servo joint6;  // левое заднее бедро
Servo joint7;  // Левая задняя нога
Servo joint8;  // правое заднее бедро
Servo joint9;  // Правая задняя нога
Servo neck_servo;  // шейный сервопривод

// исходное положение
int home_joint2 = 120;
int home_joint3 = 15;
int home_joint4 = 60;
int home_joint5 = 165;
int home_joint6 = 100;
int home_joint7 = 15;
int home_joint8 = 70;
int home_joint9 = 165;

// шейный сервопривод
#define neck_survoPin 10
#define trigPin 11
#define echoPin 12

float Dulation = 0;
float Distance = 0;
int angle = 90;
int sum = 0;


void setup(){

  joint2.attach(2);
  joint3.attach(3);
  joint4.attach(4);
  joint5.attach(5);
  joint6.attach(6);
  joint7.attach(7);
  joint8.attach(8);
  joint9.attach(9);

  neck_servo.attach(neck_survoPin, 500, 2400);
  neck_servo.write(90);
  
  pinMode(echoPin,INPUT);
  pinMode(trigPin,OUTPUT);

  joint2.write(home_joint2);
  joint3.write(home_joint3);
  joint4.write(home_joint4);
  joint5.write(home_joint5);
  joint6.write(home_joint6);
  joint7.write(home_joint7);
  joint8.write(home_joint8);
  joint9.write(home_joint9);

  delay(3000);

}


void loop(){

  standhome();
  forward(1);


// Использование ультразвукового датчика
  digitalWrite(trigPin,LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin,LOW);
  Dulation = pulseIn(echoPin,HIGH);

  if(Dulation>0){
    int Distance = Dulation*340*100/2/1000000;

    if(Distance < 10){
      int right = 0;
      int left = 0;
      int sumright = 0;
      int sumleft = 0;
      
      int i = 170;
      delay(10);
      while(i >= 120){
        neck_servo.write(i);
        i=i-2;
        digitalWrite(trigPin,LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin,HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin,LOW);
        Dulation = pulseIn(echoPin,HIGH);
        int Distance = Dulation*340*100/2/1000000;
        
        if(Distance <15){
          left = 1;
          sumleft++;
        }else {
            left = 0;
          }
          delay(10);           
      }
      
     i =10;
     delay(10);
     while(i <= 60){
        neck_servo.write(i);
        i=i+2;
        digitalWrite(trigPin,LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin,HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin,LOW);
        Dulation = pulseIn(echoPin,HIGH);
        int Distance = Dulation*340*100/2/1000000;

        if(Distance <15){
          right = 1;
          sumright++;
        }else {
          right = 0;
        }
          delay(10);       
     }


     neck_servo.write(90);
     delay(10);

      if(sumright > sumleft){
      leftturn(3);     
     } else {
      rightturn(3);
     }

     if(sumright >= 20){
      backward(3);
      leftturn(3);
     } else if(sumleft >= 20){
      backward(3);
      rightturn(3);
     }  
   }
}
delay(10);

}


// время простоя
void idle(){
  delay(100);
  }


// Исходное положение
void standhome(){
  joint2.write(home_joint2);
  joint3.write(home_joint3);
  joint4.write(home_joint4);
  joint5.write(home_joint5);
  joint6.write(home_joint6);
  joint7.write(home_joint7);
  joint8.write(home_joint8);
  joint9.write(home_joint9);
}


// Вытяните руки и коснитесь земли
void sithome(){
    joint2.write(135);
    joint3.write(65);
    joint4.write(45);
    joint5.write(110);
    joint6.write(135);
    joint7.write(80);
    joint8.write(45);
    joint9.write(110);
}


// Вставание 1
void stand1(){
  sithome();
  joint2.write(170);
  delay(300);
  joint2.write(home_joint2);
  delay(300);
  joint4.write(10);
  delay(300);
  joint4.write(home_joint4);
  delay(300);
  joint6.write(170);
  delay(300);
  joint6.write(home_joint6);
  delay(300);
  joint8.write(10);
  delay(300);
  joint8.write(home_joint8);
  delay(300);
  
  joint3.write(home_joint3);
  joint5.write(home_joint5);
  joint7.write(home_joint7);
  joint9.write(home_joint9);
}


// Вставание 2
void stand2(){
  sithome();
  joint2.write(175);
  joint4.write(5);
  joint6.write(175);
  joint8.write(5);
  delay(600);
    
  joint2.write(home_joint2);
  joint4.write(home_joint4);
  joint6.write(home_joint6);
  joint8.write(home_joint8);
  delay(600);
  
  joint3.write(home_joint3);
  joint5.write(home_joint5);
  joint7.write(home_joint7);
  joint9.write(home_joint9);
}


// Вставание 3
void stand3(){ 
  sithome();
  int i;
  int j = 90;
  int k = 90;
  joint2.write(home_joint2);
  joint4.write(home_joint4);
  joint6.write(home_joint6);
  joint8.write(home_joint8);
  for(i = 90; i < 165; i++)
  {
    joint5.write(i);
    j = j - 1;
    joint3.write(j);
    delay(20);
  }
  
  for(i = 115; i < 165; i++)
  {
    joint9.write(i);
    k = k - 1;
    joint7.write(k);
    delay(20);
  }
}


// быстро вверх и вниз
void downaction(unsigned int step){
  while (step-- > 0){
  sithome();
  delay(100);
  standhome();
  delay(100);
  }
}


// махать рукой
void wink(unsigned int step){
  standhome();
  joint9.write(home_joint9-30);
  
  while (step-- > 0){
  joint5.write(30);
  joint4.write(home_joint4 + 60);
  delay(300);
  joint4.write(home_joint4 - 60);
  delay(300);
  }
}


// Покачивайте своим телом
void twist(){
  joint3.write(home_joint3);
  joint5.write(home_joint5);
  joint7.write(home_joint7);
  joint9.write(home_joint9);
  
  for(int right=90;right<170;right++){
      joint2.write(right);
      joint6.write(right);
      joint4.write(right-90);
      joint8.write(right-90);
      delay(10);
  }

   for(int right=170;right>90;right--){
      joint2.write(right);
      joint6.write(right);
      joint4.write(right-90);
      joint8.write(right-90);
      delay(10);
  }

}


// Вперед (шаги)
void forward(unsigned int step){
  while (step-- > 0){
  joint3.write(home_joint3+30);
  joint7.write(home_joint7+30);
  delay(100);
  joint2.write(home_joint2+30);
  joint8.write(home_joint8-30);
  joint4.write(home_joint4);
  joint6.write(home_joint6);
  delay(100);
  joint3.write(home_joint3);
  joint7.write(home_joint7);
  idle();
  
  joint5.write(home_joint5-30);
  joint9.write(home_joint9-30);
  delay(100);
  joint2.write(home_joint2);
  joint8.write(home_joint8);
  joint4.write(home_joint4-30);
  joint6.write(home_joint6+30);
  delay(100);
  joint5.write(home_joint5);
  joint9.write(home_joint9);
  idle();
  }
}


// Шаг назад (количество шагов)
void backward(unsigned int step){
  while (step-- > 0){
  joint3.write(home_joint3+30);
  joint7.write(home_joint7+30);
  delay(100);
  joint2.write(home_joint2);
  joint8.write(home_joint8);
  joint4.write(home_joint4-30);
  joint6.write(home_joint6+30);
  delay(100);
  joint3.write(home_joint3);
  joint7.write(home_joint7);
  idle();
  
  joint5.write(home_joint5-30);
  joint9.write(home_joint9-30);
  delay(100);
  joint2.write(home_joint2+30);
  joint8.write(home_joint8-30);
  joint4.write(home_joint4);
  joint6.write(home_joint6);
  delay(100);
  joint5.write(home_joint5);
  joint9.write(home_joint9);
  idle();
  }
}


// По часовой стрелке (количество шагов)
void rightturn(unsigned int step){
  neck_rightrotate();
  while (step-- > 0){
  joint5.write(home_joint5-30);
  joint9.write(home_joint9-30);
  delay(100);
  joint2.write(home_joint2+30);
  joint8.write(home_joint8-30);
  joint4.write(home_joint4-30);
  joint6.write(home_joint6+30);
  delay(100);
  joint5.write(home_joint5);
  joint9.write(home_joint9);
  idle();
  
  joint3.write(home_joint3+30);
  joint7.write(home_joint7+30);
  delay(100);
  joint2.write(home_joint2);
  joint8.write(home_joint8);
  joint4.write(home_joint4);
  joint6.write(home_joint6);
  delay(100);
  joint3.write(home_joint3);
  joint7.write(home_joint7);
  idle();
  }
  neck_home();
}


// Против часовой стрелки (количество шагов)
void leftturn(unsigned int step){
  neck_leftrotate();
  while (step-- > 0){
  joint3.write(home_joint3+30);
  joint7.write(home_joint7+30);
  delay(100);
  joint2.write(home_joint2+30);
  joint8.write(home_joint8-30);
  joint4.write(home_joint4-30);
  joint6.write(home_joint6+30);
  delay(100);
  joint3.write(home_joint3);
  joint7.write(home_joint7);
  idle();
  
  joint5.write(home_joint5-30);
  joint9.write(home_joint9-30);
  delay(100);
  joint2.write(home_joint2);
  joint8.write(home_joint8);
  joint4.write(home_joint4);
  joint6.write(home_joint6);
  delay(100);
  joint5.write(home_joint5);
  joint9.write(home_joint9);
  idle();
  }
  neck_home();
}


// Двигайте головой влево и вправо (датчик выключен)
void neckrotate(){
  int i=90;
  while(i < 150){
    neck_servo.write(i);
    i++;
    delay(5);
  }
  while(i > 30){
    neck_servo.write(i);
    i--;
    delay(5);
  }
  while(i <= 90){
    neck_servo.write(i);
    i++;
    delay(5);
  }
}


// Поверните голову влево
void neck_leftrotate(){
  int i=90;
  while(i < 150){
    neck_servo.write(i);
    i++;
    delay(5);
  }
}


// Поверните голову направо
void neck_rightrotate(){
  int i=90;
  while(i > 30){
    neck_servo.write(i);
    i--;
    delay(5);
  }
}


// исходное положение шеи
void neck_home(){
  neck_servo.write(90);
}

This code is written for an Arduino-based robotics project and implements a quadruped robot with two legs on each side, along with a neck and a head controlled by servos. Here's an explanation of what the code does:

Key Functionality and Purpose

The code:

Controls Servos for the Robot's Legs and Neck:
- The robot has eight joint servos for the legs (joint2, joint3, ..., joint9) and one servo for the neck (neck_servo).
- Each servo represents a specific joint in a leg or the neck of the robot. The servos define the movement of the robot's legs and head.
Detects Obstacles Using an Ultrasonic Sensor:
- The ultrasonic sensor (trigPin, echoPin) is used to measure the distance to obstacles in front of the robot. It calculates the distance based on the signal reflection time.
Implements Leg and Neck Movements for Basic Actions:
- The robot can perform actions like:
  - Standing up (standhome, stand1, stand2, stand3)
  - Sitting down (sithome)
  - Moving forward or backward (forward, backward)
  - Turning left or right (leftturn, rightturn)
  - Waving a leg (wink)
  - Twisting its body (twist)
  - Tilting its head left or right (neck_leftrotate, neck_rightrotate)
  - Moving the head to the center (neck_home).
Handles Obstacle Avoidance:
- If the obstacle is closer than 10 cm (if (Distance < 10)), the robot will scan the environment by rotating its neck left and right to determine:
  - The direction with fewer obstacles by comparing sumleft and sumright, which count obstacles detected in each direction.
- Based on the scan:
  - The robot decides to turn or move backward to avoid the obstacle.
Adjusts Movement Dynamically Based on the Environment:
- The robot can turn left or right to navigate around objects depending on the proximity and density of obstacles to either side.
- If it detects many obstacles nearby (e.g., sumright or sumleft exceeds 20), the robot can move backward and change its direction.

Overview of the Code Structure

Servo Initialization:
- The setup() function initializes all servos, their pins, and sets the default positions (e.g., home_jointX values for legs and 90 degrees for the neck).
Main Loop:
- Implements periodic movements:
  - The robot starts in a standing position (standhome).
  - Moves forward (forward(1)).
  - Continuously checks for obstacles using the ultrasonic sensor.
  - Reacts to obstacles by turning left, turning right, or moving backward as needed.
Directional Movements:
- The leftturn and rightturn functions use specific servo angles to rotate the robot in place.
- The forward and backward functions define step sequences for forward or backward walking.
Obstacle Scanning with Neck Movement:
- The robot uses the neck servo to sweep its head left and right to determine the direction with fewer obstacles and selects the best path.
Idle State:
- When no specific action is triggered, the robot remains idle (idle()), maintaining its servo positions.

Practical Applications

Quadruped Robotics:
- This code is a basis for controlling a quadruped robot. It implements gait control, obstacle detection, and basic decision-making for navigation.
Educational Purposes:
- Demonstrates how combinations of servo movements and sensors enable robots to replicate animal-like movement and behavior.
Obstacle Avoidance:
- Integrates sensors and servos to create a responsive robot capable of adapting to its environment.

Key Concepts Used

Servo motor control using the Servo library.
Ultrasonic sensor distance measurement (pulseIn).
Decision-making logic for obstacle navigation (e.g., comparing sumleft and sumright).
Timing and delays for synchronized joint movement (delay, delayMicroseconds).

Limitations

Hardcoded angles might limit flexibility for different terrains or servo calibration.
Obstacle avoidance logic only supports detection in front using the neck-rotated sensor.
Movement speed is limited by the use of delay.

Conclusion

This code implements the basic behavior of a quadruped robot for walking, obstacle avoidance using sensors, and basic robotic animations like waving or twisting. It blends servo motor control, ultrasonic sensing, and predefined sequences to create an interactive and adaptive robot.

Generate your own explanations

Download our vscode extension

Read other generated explanations

Built by @thebuilderjr