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

1Arduino Uno:
2📍 PIN 3  → Сервомотор 1 (базовый поворот)
3📍 PIN 5  → Сервомотор 2 (плечо)
4📍 PIN 6  → Сервомотор 3 (предплечье)
5📍 PIN 9  → Сервомотор 4 (захват)
6
7🔴 5V    → Все красные провода
8⚫ GND   → Все черные провода

 1#include <Servo.h>
 2
 3// Создаем объекты для каждого сервомотора
 4Servo servo1;  // базовый поворот
 5Servo servo2;  // плечо
 6Servo servo3;  // предплечье
 7Servo servo4;  // захват
 8
 9void setup() {
10  // Подключаем сервомоторы к пинам
11  servo1.attach(3);
12  servo2.attach(5);
13  servo3.attach(6);
14  servo4.attach(9);
15  
16  // Устанавливаем начальные положения
17  moveToHome();
18}
19
20void moveToHome() {
21  servo1.write(90);  // центральное положение
22  servo2.write(90);
23  servo3.write(90);
24  servo4.write(90);
25  delay(1000);
26}

 1void smoothMove(int angle1, int angle2, int angle3, int totalTime) {
 2  int currentAngle1 = servo1.read();
 3  int currentAngle2 = servo2.read();
 4  int currentAngle3 = servo3.read();
 5  
 6  int steps = totalTime / 20;  // 20 мс на шаг
 7  
 8  for (int i = 0; i <= steps; i++) {
 9    float progress = (float)i / steps;
10    
11    int newAngle1 = currentAngle1 + (angle1 - currentAngle1) * progress;
12    int newAngle2 = currentAngle2 + (angle2 - currentAngle2) * progress;
13    int newAngle3 = currentAngle3 + (angle3 - currentAngle3) * progress;
14    
15    servo1.write(newAngle1);
16    servo2.write(newAngle2);
17    servo3.write(newAngle3);
18    
19    delay(20);
20  }
21}

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

 1#include <Servo.h>
 2
 3Servo baseServo;     // основание
 4Servo shoulderServo; // плечо
 5Servo elbowServo;    // предплечье
 6Servo gripperServo;  // захват
 7
 8void setup() {
 9  baseServo.attach(3);
10  shoulderServo.attach(5);
11  elbowServo.attach(6);
12  gripperServo.attach(9);
13  
14  homePosition();
15  delay(2000);
16  
17  // Выполняем задание
18  pickAndPlace();
19}
20
21void homePosition() {
22  baseServo.write(90);      // центр
23  shoulderServo.write(90);  // горизонтально
24  elbowServo.write(90);     // прямо
25  gripperServo.write(90);   // полуоткрыт
26}
27
28void pickAndPlace() {
29  // 1. Поворот к объекту
30  smoothMove(45, 90, 90, 90, 1000);
31  
32  // 2. Опускание к объекту
33  smoothMove(45, 120, 60, 90, 1500);
34  
35  // 3. Захват
36  smoothMove(45, 120, 60, 180, 800);
37  
38  // 4. Подъем
39  smoothMove(45, 90, 90, 180, 1500);
40  
41  // 5. Поворот к цели
42  smoothMove(135, 90, 90, 180, 1000);
43  
44  // 6. Опускание
45  smoothMove(135, 120, 60, 180, 1500);
46  
47  // 7. Отпускание
48  smoothMove(135, 120, 60, 90, 800);
49  
50  // 8. Возврат домой
51  smoothMove(90, 90, 90, 90, 2000);
52}
53
54void smoothMove(int baseAngle, int shoulderAngle, 
55                int elbowAngle, int gripperAngle, int duration) {
56  int startBase = baseServo.read();
57  int startShoulder = shoulderServo.read();
58  int startElbow = elbowServo.read();
59  int startGripper = gripperServo.read();
60  
61  int steps = duration / 20;
62  
63  for (int i = 0; i <= steps; i++) {
64    float progress = (float)i / steps;
65    
66    baseServo.write(startBase + (baseAngle - startBase) * progress);
67    shoulderServo.write(startShoulder + (shoulderAngle - startShoulder) * progress);
68    elbowServo.write(startElbow + (elbowAngle - startElbow) * progress);
69    gripperServo.write(startGripper + (gripperAngle - startGripper) * progress);
70    
71    delay(20);
72  }
73}

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

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

 1#include <Servo.h>
 2
 3Servo leftHip;    // левый тазобедренный
 4Servo leftKnee;   // левое колено
 5Servo rightHip;   // правый тазобедренный  
 6Servo rightKnee;  // правое колено
 7
 8// Углы для разных позиций
 9int hipCenter = 90;     // центральное положение бедра
10int hipForward = 120;   // бедро вперед
11int hipBackward = 60;   // бедро назад
12int kneeDown = 90;      // нога на земле
13int kneeUp = 45;        // нога поднята
14
15void setup() {
16  leftHip.attach(3);
17  leftKnee.attach(5);
18  rightHip.attach(6);
19  rightKnee.attach(9);
20  
21  standingPosition();
22  delay(2000);
23  
24  // Начинаем ходьбу
25  for(int i = 0; i < 4; i++) {
26    walkCycle();
27  }
28  
29  standingPosition();
30}
31
32void standingPosition() {
33  leftHip.write(hipCenter);
34  leftKnee.write(kneeDown);
35  rightHip.write(hipCenter);
36  rightKnee.write(kneeDown);
37  delay(500);
38}
39
40void walkCycle() {
41  // Шаг правой ногой
42  stepRight();
43  delay(200);
44  
45  // Шаг левой ногой
46  stepLeft();
47  delay(200);
48}
49
50void stepRight() {
51  // 1. Поднимаем правую ногу
52  rightKnee.write(kneeUp);
53  delay(300);
54  
55  // 2. Двигаем правую ногу вперед
56  rightHip.write(hipForward);
57  delay(400);
58  
59  // 3. Опускаем правую ногу
60  rightKnee.write(kneeDown);
61  delay(300);
62  
63  // 4. Переносим вес - отталкиваемся левой ногой
64  leftHip.write(hipBackward);
65  rightHip.write(hipCenter);
66  delay(400);
67}
68
69void stepLeft() {
70  // 1. Поднимаем левую ногу
71  leftKnee.write(kneeUp);
72  delay(300);
73  
74  // 2. Двигаем левую ногу вперед
75  leftHip.write(hipForward);
76  delay(400);
77  
78  // 3. Опускаем левую ногу
79  leftKnee.write(kneeDown);
80  delay(300);
81  
82  // 4. Переносим вес - отталкиваемся правой ногой
83  rightHip.write(hipBackward);
84  leftHip.write(hipCenter);
85  delay(400);
86}

 1void smoothStep(float stepLength, float stepHeight, int duration) {
 2  int steps = duration / 20;
 3  
 4  for(int i = 0; i <= steps; i++) {
 5    float t = (float)i / steps;
 6    
 7    // Параболическая траектория подъема ноги
 8    float height = stepHeight * sin(PI * t);
 9    
10    // Линейное движение вперед
11    float forward = stepLength * t;
12    
13    // Преобразуем в углы сервомоторов
14    int hipAngle = calculateHipAngle(forward);
15    int kneeAngle = calculateKneeAngle(height);
16    
17    rightHip.write(hipAngle);
18    rightKnee.write(kneeAngle);
19    
20    delay(20);
21  }
22}