🤖 Практическая работа: Управление несколькими сервомоторами

Arduino Uno:
📍 PIN 3  → Сервомотор 1 (базовый поворот)
📍 PIN 5  → Сервомотор 2 (плечо)
📍 PIN 6  → Сервомотор 3 (предплечье)
📍 PIN 9  → Сервомотор 4 (захват)

🔴 5V    → Все красные провода
⚫ GND   → Все черные провода

#include <Servo.h>

// Создаем объекты для каждого сервомотора
Servo servo1;  // базовый поворот
Servo servo2;  // плечо
Servo servo3;  // предплечье
Servo servo4;  // захват

void setup() {
  // Подключаем сервомоторы к пинам
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  
  // Устанавливаем начальные положения
  moveToHome();
}

void moveToHome() {
  servo1.write(90);  // центральное положение
  servo2.write(90);
  servo3.write(90);
  servo4.write(90);
  delay(1000);
}

void smoothMove(int angle1, int angle2, int angle3, int totalTime) {
  int currentAngle1 = servo1.read();
  int currentAngle2 = servo2.read();
  int currentAngle3 = servo3.read();
  
  int steps = totalTime / 20;  // 20 мс на шаг
  
  for (int i = 0; i <= steps; i++) {
    float progress = (float)i / steps;
    
    int newAngle1 = currentAngle1 + (angle1 - currentAngle1) * progress;
    int newAngle2 = currentAngle2 + (angle2 - currentAngle2) * progress;
    int newAngle3 = currentAngle3 + (angle3 - currentAngle3) * progress;
    
    servo1.write(newAngle1);
    servo2.write(newAngle2);
    servo3.write(newAngle3);
    
    delay(20);
  }
}

    [Захват] ← Сервомотор 4
         ↑
    [Предплечье] ← Сервомотор 3
         ↑
      [Плечо] ← Сервомотор 2
         ↑
    [Основание] ← Сервомотор 1

#include <Servo.h>

Servo baseServo;     // основание
Servo shoulderServo; // плечо
Servo elbowServo;    // предплечье
Servo gripperServo;  // захват

void setup() {
  baseServo.attach(3);
  shoulderServo.attach(5);
  elbowServo.attach(6);
  gripperServo.attach(9);
  
  homePosition();
  delay(2000);
  
  // Выполняем задание
  pickAndPlace();
}

void homePosition() {
  baseServo.write(90);      // центр
  shoulderServo.write(90);  // горизонтально
  elbowServo.write(90);     // прямо
  gripperServo.write(90);   // полуоткрыт
}

void pickAndPlace() {
  // 1. Поворот к объекту
  smoothMove(45, 90, 90, 90, 1000);
  
  // 2. Опускание к объекту
  smoothMove(45, 120, 60, 90, 1500);
  
  // 3. Захват
  smoothMove(45, 120, 60, 180, 800);
  
  // 4. Подъем
  smoothMove(45, 90, 90, 180, 1500);
  
  // 5. Поворот к цели
  smoothMove(135, 90, 90, 180, 1000);
  
  // 6. Опускание
  smoothMove(135, 120, 60, 180, 1500);
  
  // 7. Отпускание
  smoothMove(135, 120, 60, 90, 800);
  
  // 8. Возврат домой
  smoothMove(90, 90, 90, 90, 2000);
}

void smoothMove(int baseAngle, int shoulderAngle, 
                int elbowAngle, int gripperAngle, int duration) {
  int startBase = baseServo.read();
  int startShoulder = shoulderServo.read();
  int startElbow = elbowServo.read();
  int startGripper = gripperServo.read();
  
  int steps = duration / 20;
  
  for (int i = 0; i <= steps; i++) {
    float progress = (float)i / steps;
    
    baseServo.write(startBase + (baseAngle - startBase) * progress);
    shoulderServo.write(startShoulder + (shoulderAngle - startShoulder) * progress);
    elbowServo.write(startElbow + (elbowAngle - startElbow) * progress);
    gripperServo.write(startGripper + (gripperAngle - startGripper) * progress);
    
    delay(20);
  }
}

bool isReachable(float x, float y, float L1, float L2) {
  float distance = sqrt(x*x + y*y);
  return (distance >= abs(L1 - L2)) && (distance <= L1 + L2);
}

    [Корпус]
    /        \
[Бедро_Л]  [Бедро_П] ← Сервомоторы 1,3 (тазобедренные)
   |          |
[Голень_Л] [Голень_П] ← Сервомоторы 2,4 (коленные)
   |          |
[Стопа_Л]  [Стопа_П]

#include <Servo.h>

Servo leftHip;    // левый тазобедренный
Servo leftKnee;   // левое колено
Servo rightHip;   // правый тазобедренный  
Servo rightKnee;  // правое колено

// Углы для разных позиций
int hipCenter = 90;     // центральное положение бедра
int hipForward = 120;   // бедро вперед
int hipBackward = 60;   // бедро назад
int kneeDown = 90;      // нога на земле
int kneeUp = 45;        // нога поднята

void setup() {
  leftHip.attach(3);
  leftKnee.attach(5);
  rightHip.attach(6);
  rightKnee.attach(9);
  
  standingPosition();
  delay(2000);
  
  // Начинаем ходьбу
  for(int i = 0; i < 4; i++) {
    walkCycle();
  }
  
  standingPosition();
}

void standingPosition() {
  leftHip.write(hipCenter);
  leftKnee.write(kneeDown);
  rightHip.write(hipCenter);
  rightKnee.write(kneeDown);
  delay(500);
}

void walkCycle() {
  // Шаг правой ногой
  stepRight();
  delay(200);
  
  // Шаг левой ногой
  stepLeft();
  delay(200);
}

void stepRight() {
  // 1. Поднимаем правую ногу
  rightKnee.write(kneeUp);
  delay(300);
  
  // 2. Двигаем правую ногу вперед
  rightHip.write(hipForward);
  delay(400);
  
  // 3. Опускаем правую ногу
  rightKnee.write(kneeDown);
  delay(300);
  
  // 4. Переносим вес - отталкиваемся левой ногой
  leftHip.write(hipBackward);
  rightHip.write(hipCenter);
  delay(400);
}

void stepLeft() {
  // 1. Поднимаем левую ногу
  leftKnee.write(kneeUp);
  delay(300);
  
  // 2. Двигаем левую ногу вперед
  leftHip.write(hipForward);
  delay(400);
  
  // 3. Опускаем левую ногу
  leftKnee.write(kneeDown);
  delay(300);
  
  // 4. Переносим вес - отталкиваемся правой ногой
  rightHip.write(hipBackward);
  leftHip.write(hipCenter);
  delay(400);
}

void smoothStep(float stepLength, float stepHeight, int duration) {
  int steps = duration / 20;
  
  for(int i = 0; i <= steps; i++) {
    float t = (float)i / steps;
    
    // Параболическая траектория подъема ноги
    float height = stepHeight * sin(PI * t);
    
    // Линейное движение вперед
    float forward = stepLength * t;
    
    // Преобразуем в углы сервомоторов
    int hipAngle = calculateHipAngle(forward);
    int kneeAngle = calculateKneeAngle(height);
    
    rightHip.write(hipAngle);
    rightKnee.write(kneeAngle);
    
    delay(20);
  }
}