Lesson 17

// Combining multiple distance sensors
class SimpleSensorFusion {
private:
    float lastGoodReading = 0;
    
public:
    // Simple average of two sensors
    float averageTwoSensors(float sensor1, float sensor2) {
        return (sensor1 + sensor2) / 2.0;
    }
    
    // Weighted average - give sensor1 more importance
    float weightedAverage(float sensor1, float sensor2) {
        return (sensor1 * 0.8) + (sensor2 * 0.2);
    }
    
    // Choose the best reading (closest to previous good reading)
    float chooseBestReading(float sensor1, float sensor2) {
        float diff1 = abs(sensor1 - lastGoodReading);
        float diff2 = abs(sensor2 - lastGoodReading);
        
        if (diff1 < diff2) {
            lastGoodReading = sensor1;
            return sensor1;
        } else {
            lastGoodReading = sensor2;
            return sensor2;
        }
    }
};

// Simple data cleaning for sensor readings
class DataCleaner {
private:
    static const int BUFFER_SIZE = 5;
    float readings[BUFFER_SIZE];
    int currentIndex = 0;
    float lastGoodReading = 0;
    
public:
    float cleanReading(float newReading) {
        // Step 1: Check if reading seems reasonable
        if (isBadReading(newReading)) {
            Serial.println("Bad reading detected, using last good value");
            return lastGoodReading;
        }
        
        // Step 2: Add to our buffer of recent readings
        readings[currentIndex] = newReading;
        currentIndex = (currentIndex + 1) % BUFFER_SIZE;
        
        // Step 3: Calculate average of recent readings
        float average = calculateAverage();
        
        // Step 4: Make gradual changes to prevent jumps
        float smoothReading = makeGradualChange(average);
        
        lastGoodReading = smoothReading;
        return smoothReading;
    }
    
private:
    bool isBadReading(float reading) {
        // Check if reading is way different from last good reading
        float difference = abs(reading - lastGoodReading);
        return difference > 50; // Adjust this threshold as needed
    }
    
    float calculateAverage() {
        float sum = 0;
        for (int i = 0; i < BUFFER_SIZE; i++) {
            sum += readings[i];
        }
        return sum / BUFFER_SIZE;
    }
    
    float makeGradualChange(float targetReading) {
        // Don't change too quickly - move 20% toward target
        float change = (targetReading - lastGoodReading) * 0.2;
        return lastGoodReading + change;
    }
};

// Simple sensor calibration and error handling
class SmartSensorManager {
private:
    struct SensorCalibration {
        float offset = 0;
        float scale = 1.0;
        float confidence = 1.0;
        unsigned long lastCalibration = 0;
        int errorCount = 0;
    };
    
    SensorCalibration ultrasonicCal;
    SensorCalibration imuCal;
    SensorCalibration compassCal;
    
    static const int CALIBRATION_INTERVAL = 300000; // 5 minutes
    
public:
    void performSystemCalibration() {
        Serial.println("Starting intelligent calibration...");
        
        // Ultrasonic calibration using known reference
        calibrateUltrasonic();
        
        // IMU calibration using gravity reference
        calibrateIMU();
        
        // Compass calibration using magnetic declination
        calibrateCompass();
        
        Serial.println("Calibration complete!");
    }
    
    float getCalibratedReading(int sensorType, float rawValue) {
        SensorCalibration* cal = getSensorCalibration(sensorType);
        
        // Apply calibration
        float calibratedValue = (rawValue + cal->offset) * cal->scale;
        
        // Validate reading
        if (validateReading(sensorType, calibratedValue)) {
            cal->confidence = min(cal->confidence + 0.01, 1.0);
            cal->errorCount = 0;
            return calibratedValue;
        } else {
            cal->confidence = max(cal->confidence - 0.05, 0.1);
            cal->errorCount++;
            
            // Trigger recalibration if too many errors
            if (cal->errorCount > 10) {
                triggerRecalibration(sensorType);
            }
            
            return -1; // Error value
        }
    }
    
private:
    void calibrateUltrasonic() {
        // Multi-point calibration using known distances
        float knownDistances[] = {10, 20, 50, 100}; // cm
        float measurements[4];
        
        for (int i = 0; i < 4; i++) {
            Serial.print("Place object at ");
            Serial.print(knownDistances[i]);
            Serial.println("cm and press button...");
            
            waitForButtonPress();
            measurements[i] = getUltrasonicReading();
            delay(1000);
        }
        
        // Calculate linear calibration coefficients
        calculateLinearCalibration(knownDistances, measurements, 4, &ultrasonicCal);
    }
    
    bool validateReading(int sensorType, float value) {
        // Implement sensor-specific validation logic
        switch (sensorType) {
            case ULTRASONIC:
                return (value > 2 && value < 400); // Valid range for HC-SR04
            case IMU_ACCEL:
                return (abs(value) <= 16); // ±16g range
            case COMPASS:
                return (value >= 0 && value <= 360); // Degree range
            default:
                return true;
        }
    }
};