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

     [M]   [M]
     /       \
[O]═══════════[O]  ← Колеса
    │       │
    │   🦾  │     ← Манипулятор
    │       │
[O]═══════════[O]

      [M]
       │
   [O] │ [O]    ← Передние колеса
    \  🦾  /
     \ │ /
      \│/
      [O]       ← Заднее колесо

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

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

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

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

// Планирование конструкции
struct PlatformSpecs {
    int wheelBase;      // Колесная база (см)
    int trackWidth;     // Ширина колеи (см)  
    int groundClearance; // Клиренс (см)
    float maxLoad;      // Максимальная нагрузка (кг)
};

PlatformSpecs specs = {
    .wheelBase = 25,      // 25 см для устойчивости
    .trackWidth = 20,     // 20 см ширина
    .groundClearance = 5, // 5 см клиренс
    .maxLoad = 2.0        // 2 кг максимум
};

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

class BalanceCalculator {
private:
    struct Component {
        float mass;     // Масса компонента
        float x, y, z;  // Координаты
    };
    
    vector<Component> components;
    
public:
    void addComponent(float mass, float x, float y, float z) {
        components.push_back({mass, x, y, z});
    }
    
    Point3D calculateCenterOfMass() {
        float totalMass = 0;
        float totalX = 0, totalY = 0, totalZ = 0;
        
        for (auto& comp : components) {
            totalMass += comp.mass;
            totalX += comp.mass * comp.x;
            totalY += comp.mass * comp.y;
            totalZ += comp.mass * comp.z;
        }
        
        return {
            totalX / totalMass,
            totalY / totalMass, 
            totalZ / totalMass
        };
    }
    
    bool isStable(Point3D centerOfMass, float wheelBase, float trackWidth) {
        // Проверяем, находится ли центр тяжести в пределах опорного контура
        return (abs(centerOfMass.x) < wheelBase / 2) && 
               (abs(centerOfMass.y) < trackWidth / 2);
    }
};

class MobilePlatform {
private:
    Motor leftMotor, rightMotor;
    int baseSpeed = 150;
    
public:
    void moveForward(int distance_cm) {
        // Расчет количества оборотов колеса
        int wheelDiameter = 5; // см
        float wheelCircumference = PI * wheelDiameter;
        int rotations = distance_cm / wheelCircumference;
        
        leftMotor.rotateDegrees(rotations * 360);
        rightMotor.rotateDegrees(rotations * 360);
    }
    
    void turnLeft(int angle_degrees) {
        // Расчет дифференциального поворота
        float wheelBase = 20; // см между колесами
        float arcLength = (angle_degrees * PI * wheelBase) / 180;
        
        leftMotor.rotateDegrees(-arcLength * 360 / (PI * 5)); // Левое колесо назад
        rightMotor.rotateDegrees(arcLength * 360 / (PI * 5));  // Правое колесо вперед
    }
    
    void stop() {
        leftMotor.stop();
        rightMotor.stop();
    }
    
    // Плавное ускорение для предотвращения проскальзывания
    void smoothAccelerate(int targetSpeed, int acceleration) {
        int currentSpeed = 0;
        while (currentSpeed < targetSpeed) {
            currentSpeed += acceleration;
            if (currentSpeed > targetSpeed) currentSpeed = targetSpeed;
            
            leftMotor.setPower(currentSpeed);
            rightMotor.setPower(currentSpeed);
            delay(50);
        }
    }
};

class ActiveStabilizer {
private:
    AccelerometerSensor accel;
    GyroscopeSensor gyro;
    float tiltThreshold = 15.0; // градусы
    
public:
    void maintainBalance() {
        float tiltAngle = accel.getTiltAngle();
        float angularVelocity = gyro.getAngularVelocity();
        
        // Предиктивная коррекция
        float predictedTilt = tiltAngle + angularVelocity * 0.1;
        
        if (abs(predictedTilt) > tiltThreshold) {
            // Экстренная стабилизация
            emergencyStabilization(predictedTilt);
        } else if (abs(tiltAngle) > tiltThreshold / 2) {
            // Мягкая коррекция
            gentleCorrection(tiltAngle);
        }
    }
    
private:
    void emergencyStabilization(float tiltAngle) {
        if (tiltAngle > 0) {
            // Наклон вперед - срочно назад
            platform.moveBackward(5);
        } else {
            // Наклон назад - срочно вперед  
            platform.moveForward(5);
        }
    }
    
    void gentleCorrection(float tiltAngle) {
        // Плавная коррекция через изменение скорости
        float correction = tiltAngle * 2; // Пропорциональная коррекция
        
        if (tiltAngle > 0) {
            platform.adjustSpeed(-correction, -correction);
        } else {
            platform.adjustSpeed(correction, correction);
        }
    }
};

class RotaryBase {
private:
    Motor rotationMotor;
    int currentAngle = 0;
    int maxAngle = 180;  // ±90 градусов от центра
    
public:
    void rotateTo(int targetAngle) {
        targetAngle = constrain(targetAngle, -maxAngle/2, maxAngle/2);
        
        int deltaAngle = targetAngle - currentAngle;
        rotationMotor.rotateDegrees(deltaAngle);
        currentAngle = targetAngle;
    }
    
    void rotateRelative(int deltaAngle) {
        rotateTo(currentAngle + deltaAngle);
    }
};

class LiftMechanism {
private:
    Motor liftMotor;
    int currentHeight = 0;
    int maxHeight = 15; // см
    int minHeight = 0;
    
public:
    void liftTo(int targetHeight) {
        targetHeight = constrain(targetHeight, minHeight, maxHeight);
        
        int deltaHeight = targetHeight - currentHeight;
        
        // Расчет оборотов для подъема
        float pulleyDiameter = 2; // см
        float pulleyCircumference = PI * pulleyDiameter;
        int motorRotations = (deltaHeight / pulleyCircumference) * 360;
        
        liftMotor.rotateDegrees(motorRotations);
        currentHeight = targetHeight;
    }
    
    void liftRelative(int deltaHeight) {
        liftTo(currentHeight + deltaHeight);
    }
};

class Gripper {
private:
    Motor gripMotor;
    bool isOpen = true;
    int gripStrength = 50; // Процент мощности
    
public:
    void open() {
        if (!isOpen) {
            gripMotor.rotateDegrees(-90); // Открываем схват
            isOpen = true;
            delay(500); // Время на открытие
        }
    }
    
    void close(int strength = 50) {
        if (isOpen) {
            gripStrength = constrain(strength, 10, 100);
            gripMotor.setPower(gripStrength);
            gripMotor.rotateDegrees(90); // Закрываем схват
            isOpen = false;
            delay(500); // Время на закрытие
        }
    }
    
    bool hasObject() {
        // Проверка тока мотора для определения захвата объекта
        return gripMotor.getCurrentDraw() > normalCurrent * 1.5;
    }
    
    void adjustGrip() {
        // Автоматическая регулировка силы захвата
        if (hasObject()) {
            int current = gripMotor.getCurrentDraw();
            if (current > safeMaxCurrent) {
                gripMotor.setPower(gripMotor.getPower() - 5);
            }
        }
    }
};

class FullManipulator {
private:
    RotaryBase base;
    LiftMechanism lift;
    Gripper gripper;
    
    struct Position {
        int angle;    // Угол поворота основания
        int height;   // Высота подъема
        bool grip;    // Состояние схвата
    };
    
    Position homePosition = {0, 5, true}; // Домашняя позиция
    
public:
    void moveTo(Position target) {
        // Безопасная последовательность движений
        
        // 1. Сначала поднимаем (избегаем столкновений)
        if (target.height > lift.getCurrentHeight()) {
            lift.liftTo(target.height);
            delay(1000);
        }
        
        // 2. Поворачиваем основание
        base.rotateTo(target.angle);
        delay(1000);
        
        // 3. Опускаем до целевой высоты
        lift.liftTo(target.height);
        delay(1000);
        
        // 4. Управляем схватом
        if (target.grip) {
            gripper.close();
        } else {
            gripper.open();
        }
        delay(500);
    }
    
    void pickupSequence(Position pickupPos) {
        Serial.println("Starting pickup sequence...");
        
        // Подходим к объекту
        gripper.open();
        moveTo({pickupPos.angle, pickupPos.height + 3, true});
        
        // Опускаемся к объекту
        lift.liftTo(pickupPos.height);
        delay(1000);
        
        // Захватываем
        gripper.close();
        delay(1000);
        
        // Проверяем захват
        if (gripper.hasObject()) {
            Serial.println("Object picked up successfully");
            lift.liftTo(pickupPos.height + 5); // Поднимаем для безопасности
        } else {
            Serial.println("Failed to pick up object");
        }
    }
    
    void dropoffSequence(Position dropoffPos) {
        Serial.println("Starting dropoff sequence...");
        
        // Подходим к месту размещения
        moveTo({dropoffPos.angle, dropoffPos.height + 3, false});
        
        // Опускаемся
        lift.liftTo(dropoffPos.height);
        delay(1000);
        
        // Отпускаем объект
        gripper.open();
        delay(1000);
        
        // Отходим
        lift.liftTo(dropoffPos.height + 5);
        Serial.println("Object dropped off successfully");
    }
    
    void goHome() {
        moveTo(homePosition);
    }
};

class ManipulatorKinematics {
private:
    float L1 = 10; // Длина первого звена (см)
    float L2 = 8;  // Длина второго звена (см)
    
public:
    struct CartesianPos {
        float x, y, z;
    };
    
    struct JointAngles {
        float base, shoulder, elbow;
    };
    
    CartesianPos forwardKinematics(JointAngles joints) {
        float baseRad = joints.base * PI / 180;
        float shoulderRad = joints.shoulder * PI / 180;
        float elbowRad = joints.elbow * PI / 180;
        
        // Расчет позиции схвата в декартовых координатах
        float x = (L1 * cos(shoulderRad) + L2 * cos(shoulderRad + elbowRad)) * cos(baseRad);
        float y = (L1 * cos(shoulderRad) + L2 * cos(shoulderRad + elbowRad)) * sin(baseRad);
        float z = L1 * sin(shoulderRad) + L2 * sin(shoulderRad + elbowRad);
        
        return {x, y, z};
    }
    
    JointAngles inverseKinematics(CartesianPos target) {
        // Обратная кинематическая задача
        float r = sqrt(target.x * target.x + target.y * target.y);
        
        // Угол поворота основания
        float baseAngle = atan2(target.y, target.x) * 180 / PI;
        
        // Углы плеча и локтя (используем закон косинусов)
        float distance = sqrt(r * r + target.z * target.z);
        
        float cosElbow = (L1*L1 + L2*L2 - distance*distance) / (2 * L1 * L2);
        float elbowAngle = acos(constrain(cosElbow, -1, 1)) * 180 / PI;
        
        float alpha = atan2(target.z, r) * 180 / PI;
        float beta = acos((L1*L1 + distance*distance - L2*L2) / (2 * L1 * distance)) * 180 / PI;
        float shoulderAngle = alpha + beta;
        
        return {baseAngle, shoulderAngle, elbowAngle};
    }
    
    bool isReachable(CartesianPos target) {
        float distance = sqrt(target.x*target.x + target.y*target.y + target.z*target.z);
        return (distance <= L1 + L2) && (distance >= abs(L1 - L2));
    }
};

class TransportRobot {
private:
    MobilePlatform platform;
    FullManipulator manipulator;
    
    // Предопределенные позиции
    Position pickupZone = {-45, 2, false};   // Зона захвата
    Position dropoffZone = {45, 2, false};   // Зона размещения
    Position homePos = {0, 5, true};         // Домашняя позиция
    
    // Координаты точек на платформе (см)
    Point2D pickupLocation = {30, 0};
    Point2D dropoffLocation = {30, 30};
    
public:
    void executeTransportMission() {
        Serial.println("=== TRANSPORT MISSION START ===");
        
        // Этап 1: Инициализация
        manipulator.goHome();
        delay(2000);
        
        // Этап 2: Движение к зоне захвата
        moveToLocation(pickupLocation);
        
        // Этап 3: Захват объекта
        manipulator.pickupSequence(pickupZone);
        
        // Этап 4: Движение к зоне размещения
        moveToLocation(dropoffLocation);
        
        // Этап 5: Размещение объекта
        manipulator.dropoffSequence(dropoffZone);
        
        // Этап 6: Возврат домой
        manipulator.goHome();
        moveToLocation({0, 0});
        
        Serial.println("=== MISSION COMPLETED ===");
    }
    
private:
    void moveToLocation(Point2D target) {
        Serial.print("Moving to: ");
        Serial.print(target.x); Serial.print(", "); Serial.println(target.y);
        
        // Простая навигация (для более сложной используйте одометрию)
        float distance = sqrt(target.x*target.x + target.y*target.y);
        float angle = atan2(target.y, target.x) * 180 / PI;
        
        // Поворот к цели
        platform.turnLeft(angle);
        delay(1000);
        
        // Движение к цели
        platform.moveForward(distance);
        delay(2000);
    }
};

class ErrorHandler {
private:
    int maxRetries = 3;
    
public:
    bool executeWithRetry(function<bool()> operation, string operationName) {
        for (int attempt = 1; attempt <= maxRetries; attempt++) {
            Serial.print("Attempting "); Serial.print(operationName);
            Serial.print(" (attempt "); Serial.print(attempt); Serial.println(")");
            
            if (operation()) {
                Serial.println("Success!");
                return true;
            }
            
            Serial.print("Attempt "); Serial.print(attempt); Serial.println(" failed");
            
            if (attempt < maxRetries) {
                Serial.println("Retrying...");
                delay(1000);
                recoverFromError();
            }
        }
        
        Serial.println("All attempts failed!");
        return false;
    }
    
private:
    void recoverFromError() {
        // Базовое восстановление
        manipulator.goHome();
        platform.stop();
        delay(2000);
    }
};

// Использование обработчика ошибок
ErrorHandler errorHandler;

void smartPickup(Position pos) {
    auto pickupOperation = [pos]() -> bool {
        manipulator.pickupSequence(pos);
        return manipulator.gripper.hasObject();
    };
    
    bool success = errorHandler.executeWithRetry(pickupOperation, "pickup");
    
    if (!success) {
        Serial.println("CRITICAL ERROR: Unable to pick up object");
        emergencyStop();
    }
}

class LearningSystem {
private:
    struct TaskPerformance {
        string taskName;
        int attempts;
        int successes;
        float averageTime;
        vector<string> commonErrors;
    };
    
    map<string, TaskPerformance> taskStats;
    
public:
    void recordTaskStart(string taskName) {
        taskStats[taskName].attempts++;
        lastTaskStart = millis();
    }
    
    void recordTaskSuccess(string taskName) {
        taskStats[taskName].successes++;
        
        float taskTime = (millis() - lastTaskStart) / 1000.0;
        updateAverageTime(taskName, taskTime);
        
        // Адаптация параметров на основе успеха
        adaptParameters(taskName, true);
    }
    
    void recordTaskFailure(string taskName, string error) {
        taskStats[taskName].commonErrors.push_back(error);
        
        // Адаптация параметров на основе неудачи
        adaptParameters(taskName, false);
    }
    
    void adaptParameters(string taskName, bool success) {
        if (taskName == "pickup") {
            if (success) {
                // Можем увеличить скорость
                manipulator.setSpeed(min(manipulator.getSpeed() + 5, 100));
            } else {
                // Снижаем скорость для большей точности
                manipulator.setSpeed(max(manipulator.getSpeed() - 10, 30));
            }
        }
    }
    
    float getSuccessRate(string taskName) {
        auto& stats = taskStats[taskName];
        return stats.attempts > 0 ? (float)stats.successes / stats.attempts : 0;
    }
    
    void printStatistics() {
        Serial.println("=== LEARNING STATISTICS ===");
        for (auto& [taskName, stats] : taskStats) {
            Serial.print(taskName); Serial.print(": ");
            Serial.print(getSuccessRate(taskName) * 100); Serial.println("% success");
        }
    }
};