🦾 Создаем робота-манипулятора

1     [M]   [M]
2     /       \
3[O]═══════════[O]  ← Колеса
4    │       │
5    │   🦾  │     ← Манипулятор
6    │       │
7[O]═══════════[O]

1      [M]
2       │
3   [O] │ [O]    ← Передние колеса
4    \  🦾  /
5     \ │ /
6      \│/
7      [O]       ← Заднее колесо

1[▓▓▓▓▓▓▓▓▓▓]   ← Гусеницы
2    │ 🦾 │
3[▓▓▓▓▓▓▓▓▓▓]

1   [Схват] ←─── Вращение схвата (1 степень)
2       │
3   ════╪════ ←─── Подъем/опускание (1 степень)  
4       │
5   [Основание]

1   [Схват] ←─── Вращение схвата (1 степень)
2       │
3   ════╪════ ←─── Выдвижение (1 степень)
4       │
5   ════╪════ ←─── Подъем/опускание (1 степень)
6       │
7   [Основание]

1   [Схват] ←─── Вращение схвата (1 степень)
2       │
3   ════╪════ ←─── Наклон "кисти" (1 степень)
4       │
5   ════╪════ ←─── "Локоть" (1 степень)
6       │  
7   ════╪════ ←─── Поворот "плеча" (1 степень)
8       │
9   [Основание]

 1// Планирование конструкции
 2struct PlatformSpecs {
 3    int wheelBase;      // Колесная база (см)
 4    int trackWidth;     // Ширина колеи (см)  
 5    int groundClearance; // Клиренс (см)
 6    float maxLoad;      // Максимальная нагрузка (кг)
 7};
 8
 9PlatformSpecs specs = {
10    .wheelBase = 25,      // 25 см для устойчивости
11    .trackWidth = 20,     // 20 см ширина
12    .groundClearance = 5, // 5 см клиренс
13    .maxLoad = 2.0        // 2 кг максимум
14};

1    ┌─────────────┐ ← Площадка для манипулятора
2    │    [🦾]     │
3    │             │
4    │   [CTRL]    │ ← Контроллер
5    │             │
6    │   [BATT]    │ ← Батарея (центр тяжести)
7    │             │
8[M] └─[O]─────[O]─┘ [M] ← Моторы и колеса

 1class BalanceCalculator {
 2private:
 3    struct Component {
 4        float mass;     // Масса компонента
 5        float x, y, z;  // Координаты
 6    };
 7    
 8    vector<Component> components;
 9    
10public:
11    void addComponent(float mass, float x, float y, float z) {
12        components.push_back({mass, x, y, z});
13    }
14    
15    Point3D calculateCenterOfMass() {
16        float totalMass = 0;
17        float totalX = 0, totalY = 0, totalZ = 0;
18        
19        for (auto& comp : components) {
20            totalMass += comp.mass;
21            totalX += comp.mass * comp.x;
22            totalY += comp.mass * comp.y;
23            totalZ += comp.mass * comp.z;
24        }
25        
26        return {
27            totalX / totalMass,
28            totalY / totalMass, 
29            totalZ / totalMass
30        };
31    }
32    
33    bool isStable(Point3D centerOfMass, float wheelBase, float trackWidth) {
34        // Проверяем, находится ли центр тяжести в пределах опорного контура
35        return (abs(centerOfMass.x) < wheelBase / 2) && 
36               (abs(centerOfMass.y) < trackWidth / 2);
37    }
38};

 1class MobilePlatform {
 2private:
 3    Motor leftMotor, rightMotor;
 4    int baseSpeed = 150;
 5    
 6public:
 7    void moveForward(int distance_cm) {
 8        // Расчет количества оборотов колеса
 9        int wheelDiameter = 5; // см
10        float wheelCircumference = PI * wheelDiameter;
11        int rotations = distance_cm / wheelCircumference;
12        
13        leftMotor.rotateDegrees(rotations * 360);
14        rightMotor.rotateDegrees(rotations * 360);
15    }
16    
17    void turnLeft(int angle_degrees) {
18        // Расчет дифференциального поворота
19        float wheelBase = 20; // см между колесами
20        float arcLength = (angle_degrees * PI * wheelBase) / 180;
21        
22        leftMotor.rotateDegrees(-arcLength * 360 / (PI * 5)); // Левое колесо назад
23        rightMotor.rotateDegrees(arcLength * 360 / (PI * 5));  // Правое колесо вперед
24    }
25    
26    void stop() {
27        leftMotor.stop();
28        rightMotor.stop();
29    }
30    
31    // Плавное ускорение для предотвращения проскальзывания
32    void smoothAccelerate(int targetSpeed, int acceleration) {
33        int currentSpeed = 0;
34        while (currentSpeed < targetSpeed) {
35            currentSpeed += acceleration;
36            if (currentSpeed > targetSpeed) currentSpeed = targetSpeed;
37            
38            leftMotor.setPower(currentSpeed);
39            rightMotor.setPower(currentSpeed);
40            delay(50);
41        }
42    }
43};

 1class ActiveStabilizer {
 2private:
 3    AccelerometerSensor accel;
 4    GyroscopeSensor gyro;
 5    float tiltThreshold = 15.0; // градусы
 6    
 7public:
 8    void maintainBalance() {
 9        float tiltAngle = accel.getTiltAngle();
10        float angularVelocity = gyro.getAngularVelocity();
11        
12        // Предиктивная коррекция
13        float predictedTilt = tiltAngle + angularVelocity * 0.1;
14        
15        if (abs(predictedTilt) > tiltThreshold) {
16            // Экстренная стабилизация
17            emergencyStabilization(predictedTilt);
18        } else if (abs(tiltAngle) > tiltThreshold / 2) {
19            // Мягкая коррекция
20            gentleCorrection(tiltAngle);
21        }
22    }
23    
24private:
25    void emergencyStabilization(float tiltAngle) {
26        if (tiltAngle > 0) {
27            // Наклон вперед - срочно назад
28            platform.moveBackward(5);
29        } else {
30            // Наклон назад - срочно вперед  
31            platform.moveForward(5);
32        }
33    }
34    
35    void gentleCorrection(float tiltAngle) {
36        // Плавная коррекция через изменение скорости
37        float correction = tiltAngle * 2; // Пропорциональная коррекция
38        
39        if (tiltAngle > 0) {
40            platform.adjustSpeed(-correction, -correction);
41        } else {
42            platform.adjustSpeed(correction, correction);
43        }
44    }
45};

 1class RotaryBase {
 2private:
 3    Motor rotationMotor;
 4    int currentAngle = 0;
 5    int maxAngle = 180;  // ±90 градусов от центра
 6    
 7public:
 8    void rotateTo(int targetAngle) {
 9        targetAngle = constrain(targetAngle, -maxAngle/2, maxAngle/2);
10        
11        int deltaAngle = targetAngle - currentAngle;
12        rotationMotor.rotateDegrees(deltaAngle);
13        currentAngle = targetAngle;
14    }
15    
16    void rotateRelative(int deltaAngle) {
17        rotateTo(currentAngle + deltaAngle);
18    }
19};

 1class LiftMechanism {
 2private:
 3    Motor liftMotor;
 4    int currentHeight = 0;
 5    int maxHeight = 15; // см
 6    int minHeight = 0;
 7    
 8public:
 9    void liftTo(int targetHeight) {
10        targetHeight = constrain(targetHeight, minHeight, maxHeight);
11        
12        int deltaHeight = targetHeight - currentHeight;
13        
14        // Расчет оборотов для подъема
15        float pulleyDiameter = 2; // см
16        float pulleyCircumference = PI * pulleyDiameter;
17        int motorRotations = (deltaHeight / pulleyCircumference) * 360;
18        
19        liftMotor.rotateDegrees(motorRotations);
20        currentHeight = targetHeight;
21    }
22    
23    void liftRelative(int deltaHeight) {
24        liftTo(currentHeight + deltaHeight);
25    }
26};

 1class Gripper {
 2private:
 3    Motor gripMotor;
 4    bool isOpen = true;
 5    int gripStrength = 50; // Процент мощности
 6    
 7public:
 8    void open() {
 9        if (!isOpen) {
10            gripMotor.rotateDegrees(-90); // Открываем схват
11            isOpen = true;
12            delay(500); // Время на открытие
13        }
14    }
15    
16    void close(int strength = 50) {
17        if (isOpen) {
18            gripStrength = constrain(strength, 10, 100);
19            gripMotor.setPower(gripStrength);
20            gripMotor.rotateDegrees(90); // Закрываем схват
21            isOpen = false;
22            delay(500); // Время на закрытие
23        }
24    }
25    
26    bool hasObject() {
27        // Проверка тока мотора для определения захвата объекта
28        return gripMotor.getCurrentDraw() > normalCurrent * 1.5;
29    }
30    
31    void adjustGrip() {
32        // Автоматическая регулировка силы захвата
33        if (hasObject()) {
34            int current = gripMotor.getCurrentDraw();
35            if (current > safeMaxCurrent) {
36                gripMotor.setPower(gripMotor.getPower() - 5);
37            }
38        }
39    }
40};

 1class FullManipulator {
 2private:
 3    RotaryBase base;
 4    LiftMechanism lift;
 5    Gripper gripper;
 6    
 7    struct Position {
 8        int angle;    // Угол поворота основания
 9        int height;   // Высота подъема
10        bool grip;    // Состояние схвата
11    };
12    
13    Position homePosition = {0, 5, true}; // Домашняя позиция
14    
15public:
16    void moveTo(Position target) {
17        // Безопасная последовательность движений
18        
19        // 1. Сначала поднимаем (избегаем столкновений)
20        if (target.height > lift.getCurrentHeight()) {
21            lift.liftTo(target.height);
22            delay(1000);
23        }
24        
25        // 2. Поворачиваем основание
26        base.rotateTo(target.angle);
27        delay(1000);
28        
29        // 3. Опускаем до целевой высоты
30        lift.liftTo(target.height);
31        delay(1000);
32        
33        // 4. Управляем схватом
34        if (target.grip) {
35            gripper.close();
36        } else {
37            gripper.open();
38        }
39        delay(500);
40    }
41    
42    void pickupSequence(Position pickupPos) {
43        Serial.println("Starting pickup sequence...");
44        
45        // Подходим к объекту
46        gripper.open();
47        moveTo({pickupPos.angle, pickupPos.height + 3, true});
48        
49        // Опускаемся к объекту
50        lift.liftTo(pickupPos.height);
51        delay(1000);
52        
53        // Захватываем
54        gripper.close();
55        delay(1000);
56        
57        // Проверяем захват
58        if (gripper.hasObject()) {
59            Serial.println("Object picked up successfully");
60            lift.liftTo(pickupPos.height + 5); // Поднимаем для безопасности
61        } else {
62            Serial.println("Failed to pick up object");
63        }
64    }
65    
66    void dropoffSequence(Position dropoffPos) {
67        Serial.println("Starting dropoff sequence...");
68        
69        // Подходим к месту размещения
70        moveTo({dropoffPos.angle, dropoffPos.height + 3, false});
71        
72        // Опускаемся
73        lift.liftTo(dropoffPos.height);
74        delay(1000);
75        
76        // Отпускаем объект
77        gripper.open();
78        delay(1000);
79        
80        // Отходим
81        lift.liftTo(dropoffPos.height + 5);
82        Serial.println("Object dropped off successfully");
83    }
84    
85    void goHome() {
86        moveTo(homePosition);
87    }
88};

 1class ManipulatorKinematics {
 2private:
 3    float L1 = 10; // Длина первого звена (см)
 4    float L2 = 8;  // Длина второго звена (см)
 5    
 6public:
 7    struct CartesianPos {
 8        float x, y, z;
 9    };
10    
11    struct JointAngles {
12        float base, shoulder, elbow;
13    };
14    
15    CartesianPos forwardKinematics(JointAngles joints) {
16        float baseRad = joints.base * PI / 180;
17        float shoulderRad = joints.shoulder * PI / 180;
18        float elbowRad = joints.elbow * PI / 180;
19        
20        // Расчет позиции схвата в декартовых координатах
21        float x = (L1 * cos(shoulderRad) + L2 * cos(shoulderRad + elbowRad)) * cos(baseRad);
22        float y = (L1 * cos(shoulderRad) + L2 * cos(shoulderRad + elbowRad)) * sin(baseRad);
23        float z = L1 * sin(shoulderRad) + L2 * sin(shoulderRad + elbowRad);
24        
25        return {x, y, z};
26    }
27    
28    JointAngles inverseKinematics(CartesianPos target) {
29        // Обратная кинематическая задача
30        float r = sqrt(target.x * target.x + target.y * target.y);
31        
32        // Угол поворота основания
33        float baseAngle = atan2(target.y, target.x) * 180 / PI;
34        
35        // Углы плеча и локтя (используем закон косинусов)
36        float distance = sqrt(r * r + target.z * target.z);
37        
38        float cosElbow = (L1*L1 + L2*L2 - distance*distance) / (2 * L1 * L2);
39        float elbowAngle = acos(constrain(cosElbow, -1, 1)) * 180 / PI;
40        
41        float alpha = atan2(target.z, r) * 180 / PI;
42        float beta = acos((L1*L1 + distance*distance - L2*L2) / (2 * L1 * distance)) * 180 / PI;
43        float shoulderAngle = alpha + beta;
44        
45        return {baseAngle, shoulderAngle, elbowAngle};
46    }
47    
48    bool isReachable(CartesianPos target) {
49        float distance = sqrt(target.x*target.x + target.y*target.y + target.z*target.z);
50        return (distance <= L1 + L2) && (distance >= abs(L1 - L2));
51    }
52};

 1class TransportRobot {
 2private:
 3    MobilePlatform platform;
 4    FullManipulator manipulator;
 5    
 6    // Предопределенные позиции
 7    Position pickupZone = {-45, 2, false};   // Зона захвата
 8    Position dropoffZone = {45, 2, false};   // Зона размещения
 9    Position homePos = {0, 5, true};         // Домашняя позиция
10    
11    // Координаты точек на платформе (см)
12    Point2D pickupLocation = {30, 0};
13    Point2D dropoffLocation = {30, 30};
14    
15public:
16    void executeTransportMission() {
17        Serial.println("=== TRANSPORT MISSION START ===");
18        
19        // Этап 1: Инициализация
20        manipulator.goHome();
21        delay(2000);
22        
23        // Этап 2: Движение к зоне захвата
24        moveToLocation(pickupLocation);
25        
26        // Этап 3: Захват объекта
27        manipulator.pickupSequence(pickupZone);
28        
29        // Этап 4: Движение к зоне размещения
30        moveToLocation(dropoffLocation);
31        
32        // Этап 5: Размещение объекта
33        manipulator.dropoffSequence(dropoffZone);
34        
35        // Этап 6: Возврат домой
36        manipulator.goHome();
37        moveToLocation({0, 0});
38        
39        Serial.println("=== MISSION COMPLETED ===");
40    }
41    
42private:
43    void moveToLocation(Point2D target) {
44        Serial.print("Moving to: ");
45        Serial.print(target.x); Serial.print(", "); Serial.println(target.y);
46        
47        // Простая навигация (для более сложной используйте одометрию)
48        float distance = sqrt(target.x*target.x + target.y*target.y);
49        float angle = atan2(target.y, target.x) * 180 / PI;
50        
51        // Поворот к цели
52        platform.turnLeft(angle);
53        delay(1000);
54        
55        // Движение к цели
56        platform.moveForward(distance);
57        delay(2000);
58    }
59};

 1class ErrorHandler {
 2private:
 3    int maxRetries = 3;
 4    
 5public:
 6    bool executeWithRetry(function<bool()> operation, string operationName) {
 7        for (int attempt = 1; attempt <= maxRetries; attempt++) {
 8            Serial.print("Attempting "); Serial.print(operationName);
 9            Serial.print(" (attempt "); Serial.print(attempt); Serial.println(")");
10            
11            if (operation()) {
12                Serial.println("Success!");
13                return true;
14            }
15            
16            Serial.print("Attempt "); Serial.print(attempt); Serial.println(" failed");
17            
18            if (attempt < maxRetries) {
19                Serial.println("Retrying...");
20                delay(1000);
21                recoverFromError();
22            }
23        }
24        
25        Serial.println("All attempts failed!");
26        return false;
27    }
28    
29private:
30    void recoverFromError() {
31        // Базовое восстановление
32        manipulator.goHome();
33        platform.stop();
34        delay(2000);
35    }
36};
37
38// Использование обработчика ошибок
39ErrorHandler errorHandler;
40
41void smartPickup(Position pos) {
42    auto pickupOperation = [pos]() -> bool {
43        manipulator.pickupSequence(pos);
44        return manipulator.gripper.hasObject();
45    };
46    
47    bool success = errorHandler.executeWithRetry(pickupOperation, "pickup");
48    
49    if (!success) {
50        Serial.println("CRITICAL ERROR: Unable to pick up object");
51        emergencyStop();
52    }
53}

 1class LearningSystem {
 2private:
 3    struct TaskPerformance {
 4        string taskName;
 5        int attempts;
 6        int successes;
 7        float averageTime;
 8        vector<string> commonErrors;
 9    };
10    
11    map<string, TaskPerformance> taskStats;
12    
13public:
14    void recordTaskStart(string taskName) {
15        taskStats[taskName].attempts++;
16        lastTaskStart = millis();
17    }
18    
19    void recordTaskSuccess(string taskName) {
20        taskStats[taskName].successes++;
21        
22        float taskTime = (millis() - lastTaskStart) / 1000.0;
23        updateAverageTime(taskName, taskTime);
24        
25        // Адаптация параметров на основе успеха
26        adaptParameters(taskName, true);
27    }
28    
29    void recordTaskFailure(string taskName, string error) {
30        taskStats[taskName].commonErrors.push_back(error);
31        
32        // Адаптация параметров на основе неудачи
33        adaptParameters(taskName, false);
34    }
35    
36    void adaptParameters(string taskName, bool success) {
37        if (taskName == "pickup") {
38            if (success) {
39                // Можем увеличить скорость
40                manipulator.setSpeed(min(manipulator.getSpeed() + 5, 100));
41            } else {
42                // Снижаем скорость для большей точности
43                manipulator.setSpeed(max(manipulator.getSpeed() - 10, 30));
44            }
45        }
46    }
47    
48    float getSuccessRate(string taskName) {
49        auto& stats = taskStats[taskName];
50        return stats.attempts > 0 ? (float)stats.successes / stats.attempts : 0;
51    }
52    
53    void printStatistics() {
54        Serial.println("=== LEARNING STATISTICS ===");
55        for (auto& [taskName, stats] : taskStats) {
56            Serial.print(taskName); Serial.print(": ");
57            Serial.print(getSuccessRate(taskName) * 100); Serial.println("% success");
58        }
59    }
60};