Полное руководство по ESP32 MPU-6050: освоение распознавания движений с помощью Arduino IDE

// Professional I2C address management for multiple MPU-6050 units
class MPU6050_Array {
private:
    инт addressPins[4] = {15, 16, 17, 18}; // GPIOs controlling AD0
    инт sensorCount = 0;
    
public:
    пустота initializeArray() {
        // Configure multiple sensors on same I2C bus
        для(инт i = 0; i < 4; i++) {
            pinMode(addressPins[i], ВЫХОД);
            digitalWrite(addressPins[i], НИЗКИЙ); // Default address 0x68
            
            // Test each potential sensor
            Wire.beginTransmission(0x68 + i);
            если(Wire.endTransmission() == 0) {
                sensorCount++;
                Серийный номер.printf("Found MPU-6050 at address 0x%X\n", 0x68 + i);
            }
        }
    }
    
    инт getSensorCount() { возвращаться sensorCount; }
};

// PlatformIO platformio.ini configuration for production projects
[env:esp32dev]
платформа = espressif32
board = esp32dev
framework = arduino

lib_deps = 
    adafruit/Adafruit MPU6050 @ ^2.1.0
    adafruit/Adafruit Unified Sensor @ ^1.1.7
    adafruit/Adafruit BusIO @ ^1.14.1
    adafruit/Adafruit SSD1306 @ ^2.5.7
    
monitor_speed = 115200
build_flags = 
    -D MPU6050_CALIBRATION_ENABLED
    -D ENABLE_SENSOR_FUSION

// I2C bus diagnostics and optimization
пустота diagnoseI2CBus(инт sdaPin, инт sclPin) {
    Wire.начинать(sdaPin, sclPin);
    Wire.setClock(400000); // Fast-mode 400kHz
    
    Серийный номер.println("\n=== I2C BUS DIAGNOSTICS ===");
    
    // Scan for devices
    byte error, address;
    инт devicesFound = 0;
    
    для(address = 1; address < 127; address++) {
        Wire.beginTransmission(address);
        error = Wire.endTransmission();
        
        если(error == 0) {
            Серийный номер.printf("Device found at address 0x%02X\n", address);
            devicesFound++;
        }
    }
    
    Серийный номер.printf("Total devices found: %d\n", devicesFound);
    
    // Test communication speed
    без подписи длинный startTime = micros();
    для(инт i = 0; i < 100; i++) {
        Wire.beginTransmission(0x68); // MPU-6050 address
        Wire.писать(0x3B); // ACCEL_XOUT_H register
        Wire.endTransmission(ЛОЖЬ);
        Wire.requestFrom(0x68, 14);
        пока(Wire.доступный()) Wire.читать();
    }
    без подписи длинный endTime = micros();
    
    плавать transferTime = (endTime - startTime) / 100.0;
    Серийный номер.printf("Average read time: %.2f µs\n", transferTime);
    
    если(transferTime > 1000) {
        Серийный номер.println("Warning: I2C communication slower than expected");
        Серийный номер.println("Check pull-up resistors (4.7kΩ recommended)");
    }
}

class AdaptiveMPU6050 {
private:
    Adafruit_MPU6050 mpu;
    плавать currentMaxAccel = 0;
    плавать currentMaxGyro = 0;
    без подписи длинный lastRangeCheck = 0;
    константа без подписи длинный rangeCheckInterval = 1000; // Check every second
    
public:
    пустота initialize() {
        если(!mpu.начинать()) {
            Серийный номер.println("MPU6050 initialization failed!");
            пока(1);
        }
        
        // Start with conservative ranges
        mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
        mpu.setGyroRange(MPU6050_RANGE_250_DEG);
        mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
    }
    
    пустота обновлять() {
        sensors_event_t a, g, temp;
        mpu.getEvent(&a, &g, &temp);
        
        // Track maximum values for adaptive configuration
        trackPeaks(a, g);
        
        // Periodically adjust ranges if needed
        если(миллис() - lastRangeCheck > rangeCheckInterval) {
            adjustRanges();
            lastRangeCheck = миллис();
        }
    }
    
private:
    пустота trackPeaks(sensors_event_t& a, sensors_event_t& g) {
        // Calculate vector magnitudes
        плавать accelMag = sqrt(a.acceleration.x * a.acceleration.x + 
                               a.acceleration.y * a.acceleration.y + 
                               a.acceleration.z * a.acceleration.z);
        
        плавать gyroMag = sqrt(g.gyro.x * g.gyro.x + 
                             g.gyro.y * g.gyro.y + 
                             g.gyro.z * g.gyro.z);
        
        // Update peaks with decay
        currentMaxAccel = макс(currentMaxAccel * 0.95, accelMag);
        currentMaxGyro = макс(currentMaxGyro * 0.95, gyroMag * 180/PI); // Convert to °/s
    }
    
    пустота adjustRanges() {
        // Adjust accelerometer range
        если(currentMaxAccel > 14) {
            mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
            Серийный номер.println("Accelerometer range increased to ±16g");
        } еще если(currentMaxAccel > 6 && currentMaxAccel <= 14) {
            mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
        } еще если(currentMaxAccel <= 6) {
            mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
        }
        
        // Adjust gyroscope range (convert to °/s for comparison)
        если(currentMaxGyro > 1500) {
            mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
            Серийный номер.println("Gyroscope range increased to ±2000°/s");
        } еще если(currentMaxGyro > 750 && currentMaxGyro <= 1500) {
            mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
        } еще если(currentMaxGyro <= 750) {
            mpu.setGyroRange(MPU6050_RANGE_500_DEG);
        }
        
        // Reset tracking
        currentMaxAccel = 0;
        currentMaxGyro = 0;
    }
};

пустота analyzeFilterPerformance() {
    Серийный номер.println("\n=== FILTER PERFORMANCE ANALYSIS ===");
    
    // Test each filter setting
    инт bandwidths[] = {5, 10, 21, 44, 94, 184, 260};
    константа чар* bandwidthNames[] = {"5 Hz", "10 Hz", "21 Hz", "44 Hz", 
                                    "94 Hz", "184 Hz", "260 Hz"};
    
    для(инт i = 0; i < 7; i++) {
        // Configure filter
        switch(bandwidths[i]) {
            case 5: mpu.setFilterBandwidth(MPU6050_BAND_5_HZ); перерыв;
            case 10: mpu.setFilterBandwidth(MPU6050_BAND_10_HZ); перерыв;
            case 21: mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); перерыв;
            case 44: mpu.setFilterBandwidth(MPU6050_BAND_44_HZ); перерыв;
            case 94: mpu.setFilterBandwidth(MPU6050_BAND_94_HZ); перерыв;
            case 184: mpu.setFilterBandwidth(MPU6050_BAND_184_HZ); перерыв;
            case 260: mpu.setFilterBandwidth(MPU6050_BAND_260_HZ); перерыв;
        }
        
        задерживать(100); // Allow filter to settle
        
        // Collect samples for analysis
        плавать noiseLevel = 0;
        константа инт samples = 100;
        
        для(инт j = 0; j < samples; j++) {
            sensors_event_t a, g, temp;
            mpu.getEvent(&a, &g, &temp);
            
            // Calculate noise (deviation from running average)
            static плавать avgAccelZ = 9.81;
            avgAccelZ = avgAccelZ * 0.99 + a.acceleration.z * 0.01;
            noiseLevel += abs(a.acceleration.z - avgAccelZ);
            
            задерживать(10);
        }
        
        noiseLevel /= samples;
        
        Серийный номер.printf("%-6s: Noise ≈ %.3f m/s², Latency ≈ %.1f ms\n", 
                      bandwidthNames[i], noiseLevel, 1000.0/(bandwidths[i]*2));
    }
}

class MPU6050_Calibrator {
private:
    struct CalibrationData {
        плавать accelBias[3] = {0, 0, 0};
        плавать accelScale[3] = {1, 1, 1};
        плавать gyroBias[3] = {0, 0, 0};
        плавать tempCompensation = 0;
    } calibration;
    
public:
    пустота performFullCalibration() {
        Серийный номер.println("\n=== MPU-6050 SIX-POSITION CALIBRATION ===");
        Серийный номер.println("Place sensor in each orientation as instructed");
        Серийный номер.println("Each position requires 2 seconds of stable data");
        
        // Array of positions: +X, -X, +Y, -Y, +Z, -Z
        константа чар* positions[] = {"+X axis up", "-X axis up", 
                                   "+Y axis up", "-Y axis up", 
                                   "+Z axis up", "-Z axis up"};
        
        плавать readings[6][3]; // Store averages for each position
        
        для(инт pos = 0; pos < 6; pos++) {
            Серийный номер.printf("\nPosition %d/%d: %s\n", pos+1, 6, positions[pos]);
            Серийный номер.println("Place sensor and press any key to continue...");
            пока(!Серийный номер.доступный());
            Серийный номер.читать(); // Clear the key
            
            // Wait for stabilization
            задерживать(1000);
            
            // Collect data
            плавать sum[3] = {0, 0, 0};
            константа инт samples = 200;
            
            для(инт i = 0; i < samples; i++) {
                sensors_event_t a, g, temp;
                mpu.getEvent(&a, &g, &temp);
                
                sum[0] += a.acceleration.x;
                sum[1] += a.acceleration.y;
                sum[2] += a.acceleration.z;
                
                задерживать(10);
            }
            
            readings[pos][0] = sum[0] / samples;
            readings[pos][1] = sum[1] / samples;
            readings[pos][2] = sum[2] / samples;
            
            Серийный номер.printf("  Average: X=%.3f, Y=%.3f, Z=%.3f m/s²\n", 
                         readings[pos][0], readings[pos][1], readings[pos][2]);
        }
        
        // Calculate calibration parameters
        calculateCalibration(readings);
        saveCalibrationToEEPROM();
        
        Серийный номер.println("\n✓ Calibration complete!");
        printCalibrationReport();
    }
    
private:
    пустота calculateCalibration(плавать readings[6][3]) {
        // Calculate biases (offsets)
        calibration.accelBias[0] = (readings[0][0] + readings[1][0]) / 2;
        calibration.accelBias[1] = (readings[2][1] + readings[3][1]) / 2;
        calibration.accelBias[2] = (readings[4][2] + readings[5][2]) / 2;
        
        // Calculate scale factors
        плавать expectedGravity = 9.80665;
        
        calibration.accelScale[0] = (2 * expectedGravity) / 
                                    abs(readings[0][0] - readings[1][0]);
        calibration.accelScale[1] = (2 * expectedGravity) / 
                                    abs(readings[2][1] - readings[3][1]);
        calibration.accelScale[2] = (2 * expectedGravity) / 
                                    abs(readings[4][2] - readings[5][2]);
    }
    
    пустота saveCalibrationToEEPROM() {
        // Implementation for saving to ESP32's EEPROM or preferences
        Серийный номер.println("Calibration data saved to non-volatile memory");
    }
    
    пустота printCalibrationReport() {
        Серийный номер.println("\n--- CALIBRATION RESULTS ---");
        Серийный номер.println("Accelerometer Biases (m/s²):");
        Серийный номер.printf("  X: %.4f\n", calibration.accelBias[0]);
        Серийный номер.printf("  Y: %.4f\n", calibration.accelBias[1]);
        Серийный номер.printf("  Z: %.4f\n", calibration.accelBias[2]);
        
        Серийный номер.println("\nAccelerometer Scale Factors:");
        Серийный номер.printf("  X: %.6f\n", calibration.accelScale[0]);
        Серийный номер.printf("  Y: %.6f\n", calibration.accelScale[1]);
        Серийный номер.printf("  Z: %.6f\n", calibration.accelScale[2]);
    }
    
public:
    пустота applyCalibration(sensors_event_t& a) {
        // Apply calibration to raw readings
        a.acceleration.x = (a.acceleration.x - calibration.accelBias[0]) * 
                           calibration.accelScale[0];
        a.acceleration.y = (a.acceleration.y - calibration.accelBias[1]) * 
                           calibration.accelScale[1];
        a.acceleration.z = (a.acceleration.z - calibration.accelBias[2]) * 
                           calibration.accelScale[2];
    }
};

пустота calibrateGyroscope() {
    Серийный номер.println("\nCalibrating gyroscope... Keep sensor absolutely still!");
    Серийный номер.println("Starting in 3 seconds...");
    задерживать(3000);
    
    плавать biasSum[3] = {0, 0, 0};
    константа инт calibrationSamples = 2000;
    
    для(инт i = 0; i < calibrationSamples; i++) {
        sensors_event_t a, g, temp;
        mpu.getEvent(&a, &g, &temp);
        
        biasSum[0] += g.gyro.x;
        biasSum[1] += g.gyro.y;
        biasSum[2] += g.gyro.z;
        
        если(i % 100 == 0) {
            Серийный номер.printf("Progress: %d/%d samples\n", i, calibrationSamples);
        }
        
        задерживать(1);
    }
    
    calibration.gyroBias[0] = biasSum[0] / calibrationSamples;
    calibration.gyroBias[1] = biasSum[1] / calibrationSamples;
    calibration.gyroBias[2] = biasSum[2] / calibrationSamples;
    
    Серийный номер.println("\nGyroscope biases (rad/s):");
    Серийный номер.printf("  X: %.6f\n", calibration.gyroBias[0]);
    Серийный номер.printf("  Y: %.6f\n", calibration.gyroBias[1]);
    Серийный номер.printf("  Z: %.6f\n", calibration.gyroBias[2]);
}

class AttitudeEstimator {
private:
    // Orientation estimates
    плавать roll = 0, pitch = 0, yaw = 0;
    
    // Complementary filter coefficient (0-1)
    // Higher = more trust in gyro, lower = more trust in accelerometer
    константа плавать альфа = 0.98;
    
    // Timing for integration
    без подписи длинный lastUpdate = 0;
    
public:
    пустота обновлять(sensors_event_t& a, sensors_event_t& g) {
        // Calculate time delta
        без подписи длинный now = micros();
        плавать dt = (now - lastUpdate) / 1000000.0;
        lastUpdate = now;
        
        если(dt > 0.1) dt = 0.1; // Prevent large jumps
        
        // Calculate roll and pitch from accelerometer (in radians)
        плавать accelRoll = atan2(a.acceleration.y, a.acceleration.z);
        плавать accelPitch = atan2(-a.acceleration.x, 
                                 sqrt(a.acceleration.y * a.acceleration.y + 
                                      a.acceleration.z * a.acceleration.z));
        
        // Integrate gyroscope readings (convert to radians if needed)
        плавать gyroRoll = g.gyro.x;
        плавать gyroPitch = g.gyro.y;
        плавать gyroYaw = g.gyro.z;
        
        // Apply complementary filter
        roll = альфа * (roll + gyroRoll * dt) + (1 - альфа) * accelRoll;
        pitch = альфа * (pitch + gyroPitch * dt) + (1 - альфа) * accelPitch;
        yaw += gyroYaw * dt; // No accelerometer reference for yaw
        
        // Convert to degrees for display
        плавать rollDeg = roll * 180.0 / PI;
        плавать pitchDeg = pitch * 180.0 / PI;
        плавать yawDeg = yaw * 180.0 / PI;
        
        // Keep angles in reasonable range
        если(rollDeg > 180) rollDeg -= 360;
        если(rollDeg < -180) rollDeg += 360;
        если(pitchDeg > 180) pitchDeg -= 360;
        если(pitchDeg < -180) pitchDeg += 360;
        если(yawDeg > 180) yawDeg -= 360;
        если(yawDeg < -180) yawDeg += 360;
    }
    
    пустота printAttitude() {
        Серийный номер.printf("Roll: %6.1f°, Pitch: %6.1f°, Yaw: %6.1f°\n", 
                     roll * 180/PI, pitch * 180/PI, yaw * 180/PI);
    }
    
    плавать getRoll() { возвращаться roll * 180.0 / PI; }
    плавать getPitch() { возвращаться pitch * 180.0 / PI; }
    плавать getYaw() { возвращаться yaw * 180.0 / PI; }
};

class MotionDetector {
private:
    struct MotionProfile {
        плавать threshold;
        без подписи длинный duration;
        константа чар* name;
    };
    
    MotionProfile profiles[6] = {
        {5.0, 500, "Sharp Tap"},      // High acceleration, short duration
        {2.0, 1000, "Tilt"},          // Moderate, longer duration
        {8.0, 200, "Double Tap"},     // Very high, very short
        {1.5, 2000, "Slow Turn"},     // Low, long duration
        {3.0, 300, "Quick Shake"},    // Medium, medium duration
        {10.0, 100, "Impact"}         // Extreme, very short
    };
    
    плавать accelHistory[10][3]; // Store last 10 readings
    инт historyIndex = 0;
    
public:
    пустота addReading(sensors_event_t& a) {
        // Store reading in history buffer
        accelHistory[historyIndex][0] = a.acceleration.x;
        accelHistory[historyIndex][1] = a.acceleration.y;
        accelHistory[historyIndex][2] = a.acceleration.z;
        
        historyIndex = (historyIndex + 1) % 10;
        
        // Check for motion patterns
        detectMotion();
    }
    
private:
    пустота detectMotion() {
        // Calculate vector magnitude of current reading
        плавать currentMag = sqrt(
            accelHistory[(historyIndex+9)%10][0] * accelHistory[(historyIndex+9)%10][0] +
            accelHistory[(historyIndex+9)%10][1] * accelHistory[(historyIndex+9)%10][1] +
            accelHistory[(historyIndex+9)%10][2] * accelHistory[(historyIndex+9)%10][2]
        );
        
        // Compare against gravity (remove static component)
        плавать dynamicAccel = abs(currentMag - 9.81);
        
        // Check against profiles
        для(инт i = 0; i < 6; i++) {
            если(dynamicAccel > profiles[i].threshold) {
                // Additional duration checking would go here
                Серийный номер.printf("Motion detected: %s (%.1f m/s²)\n", 
                             profiles[i].name, dynamicAccel);
                перерыв;
            }
        }
    }
};

class AdvancedOLEDDisplay {
private:
    Adafruit_SSD1306 display;
    
    // Display regions
    константа инт graphHeight = 32;
    константа инт graphWidth = 128;
    
    // Graph buffers
    плавать rollHistory[128] = {0};
    плавать pitchHistory[128] = {0};
    инт graphIndex = 0;
    
public:
    пустота initialize() {
        если(!display.начинать(SSD1306_SWITCHCAPVCC, 0x3C)) {
            Серийный номер.println("SSD1306 allocation failed");
            для(;;);
        }
        
        display.clearDisplay();
        display.setTextSize(1);
        display.setTextColor(SSD1306_WHITE);
        display.display();
    }
    
    пустота updateDisplay(AttitudeEstimator& attitude, 
                       sensors_event_t& a, 
                       sensors_event_t& g) {
        display.clearDisplay();
        
        // Draw header with sensor status
        display.setCursor(0, 0);
        display.печать("MPU-6050 IMU");
        
        // Display attitude
        display.setCursor(0, 12);
        display.printf("R:%5.1f P:%5.1f Y:%5.1f", 
                      attitude.getRoll(), 
                      attitude.getPitch(), 
                      attitude.getYaw());
        
        // Draw real-time graphs
        drawGraph(rollHistory, 20, "Roll", attitude.getRoll());
        drawGraph(pitchHistory, 44, "Pitch", attitude.getPitch());
        
        // Update history buffers
        rollHistory[graphIndex] = attitude.getRoll() / 90.0; // Normalize to ±1
        pitchHistory[graphIndex] = attitude.getPitch() / 90.0;
        graphIndex = (graphIndex + 1) % graphWidth;
        
        display.display();
    }
    
private:
    пустота drawGraph(плавать* history, инт yPos, константа чар* label, плавать current) {
        // Draw graph background
        display.drawFastHLine(0, yPos + 8, 128, SSD1306_WHITE);
        
        // Draw label
        display.setCursor(0, yPos);
        display.печать(label);
        
        // Draw current value
        display.setCursor(40, yPos);
        display.printf("%5.1f", current);
        
        // Draw graph
        для(инт i = 0; i < 128; i++) {
            инт bufferIndex = (graphIndex + i) % 128;
            инт pixelY = yPos + 8 - (history[bufferIndex] * 8);
            display.drawPixel(i, pixelY, SSD1306_WHITE);
        }
    }
};

// MPU-6050 data processing on Core 0
TaskHandle_t SensorTask;

пустота sensorProcessingTask(пустота * parameter) {
    MPU6050_Calibrator calibrator;
    AttitudeEstimator attitude;
    MotionDetector motionDetector;
    
    // Initialize sensor
    Adafruit_MPU6050 mpu;
    mpu.начинать();
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
    
    // Calibrate if needed
    calibrator.performFullCalibration();
    
    // Main sensor loop
    пока(1) {
        sensors_event_t a, g, temp;
        mpu.getEvent(&a, &g, &temp);
        
        // Apply calibration
        calibrator.applyCalibration(a);
        
        // Update attitude estimation
        attitude.обновлять(a, g);
        
        // Check for motion
        motionDetector.addReading(a);
        
        // Send data to other core via queue or shared variables
        
        vTaskDelay(10 / portTICK_PERIOD_MS); // 100Hz update rate
    }
}

пустота настраивать() {
    Серийный номер.начинать(115200);
    
    // Create sensor processing task on Core 0
    xTaskCreatePinnedToCore(
        sensorProcessingTask,   // Function to implement the task
        "SensorTask",           // Name of the task
        10000,                  // Stack size in words
        NULL,                   // Task input parameter
        1,                      // Priority of the task
        &SensorTask,            // Task handle
        0                       // Core where the task should run
    );
    
    // Main loop on Core 1 can handle display, communication, etc.
}

пустота петля() {
    // Core 1 handles display updates, network communication, etc.
    // This runs independently from sensor processing
    
    static без подписи длинный lastDisplayUpdate = 0;
    если(миллис() - lastDisplayUpdate > 50) { // 20Hz display update
        // Update display with latest sensor data (via shared variables)
        lastDisplayUpdate = миллис();
    }
    
    задерживать(10);
}

class PowerManager {
private:
    enum PowerMode {
        HIGH_PERFORMANCE,
        BALANCED,
        POWER_SAVE,
        SLEEP_MODE
    };
    
    PowerMode currentMode = HIGH_PERFORMANCE;
    без подписи длинный lastMotionTime = 0;
    константа без подписи длинный sleepTimeout = 30000; // 30 seconds
    
public:
    пустота checkPowerMode(AttitudeEstimator& attitude) {
        // Check if we should enter power save mode
        если(миллис() - lastMotionTime > sleepTimeout) {
            setPowerMode(POWER_SAVE);
        }
        
        // Update motion detection time
        если(abs(attitude.getRoll()) > 1.0 || abs(attitude.getPitch()) > 1.0) {
            lastMotionTime = миллис();
            setPowerMode(HIGH_PERFORMANCE);
        }
    }
    
    пустота setPowerMode(PowerMode mode) {
        если(mode == currentMode) возвращаться;
        
        switch(mode) {
            case HIGH_PERFORMANCE:
                // Full speed, all features enabled
                setCpuFrequencyMhz(240);
                mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
                Серийный номер.println("Power mode: High Performance");
                перерыв;
                
            case BALANCED:
                // Reduced frequency, moderate filtering
                setCpuFrequencyMhz(160);
                mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
                Серийный номер.println("Power mode: Balanced");
                перерыв;
                
            case POWER_SAVE:
                // Minimum frequency, aggressive filtering
                setCpuFrequencyMhz(80);
                mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
                Серийный номер.println("Power mode: Power Save");
                перерыв;
                
            case SLEEP_MODE:
                // Deep sleep with wake on motion
                enableSleepMode();
                перерыв;
        }
        
        currentMode = mode;
    }
    
private:
    пустота enableSleepMode() {
        // Configure MPU-6050 motion detection interrupt
        // Configure ESP32 deep sleep with EXT0 wakeup
        Серийный номер.println("Entering deep sleep...");
        esp_deep_sleep_start();
    }
};

class BalanceController {
private:
    плавать targetAngle = 0; // Upright position
    плавать kP = 25.0, kI = 0.5, kD = 15.0;
    плавать errorSum = 0, lastError = 0;
    
public:
    плавать calculateMotorOutput(плавать currentAngle) {
        плавать error = targetAngle - currentAngle;
        плавать errorDerivative = error - lastError;
        errorSum += error;
        
        // Anti-windup
        errorSum = constrain(errorSum, -100, 100);
        
        плавать output = kP * error + kI * errorSum + kD * errorDerivative;
        
        lastError = error;
        возвращаться constrain(output, -255, 255);
    }
};

class IMUDataLogger {
private:
    File dataFile;
    без подписи длинный startTime;
    
public:
    пустота initialize() {
        если(!SD.начинать()) {
            Серийный номер.println("SD card initialization failed!");
            возвращаться;
        }
        
        dataFile = SD.open("/imu_data.csv", FILE_WRITE);
        dataFile.println("Time(ms),AccX,AccY,AccZ,GyroX,GyroY,GyroZ,Roll,Pitch,Yaw");
        startTime = миллис();
    }
    
    пустота logData(sensors_event_t& a, sensors_event_t& g, 
                 AttitudeEstimator& attitude) {
        dataFile.печать(миллис() - startTime);
        dataFile.печать(",");
        dataFile.печать(a.acceleration.x, 4);
        dataFile.печать(",");
        dataFile.печать(a.acceleration.y, 4);
        dataFile.печать(",");
        dataFile.печать(a.acceleration.z, 4);
        dataFile.печать(",");
        dataFile.печать(g.gyro.x, 6);
        dataFile.печать(",");
        dataFile.печать(g.gyro.y, 6);
        dataFile.печать(",");
        dataFile.печать(g.gyro.z, 6);
        dataFile.печать(",");
        dataFile.печать(attitude.getRoll(), 2);
        dataFile.печать(",");
        dataFile.печать(attitude.getPitch(), 2);
        dataFile.печать(",");
        dataFile.println(attitude.getYaw(), 2);
        
        // Flush periodically
        static без подписи длинный lastFlush = 0;
        если(миллис() - lastFlush > 1000) {
            dataFile.flush();
            lastFlush = миллис();
        }
    }
};