feat: auto tune pid

[gabrielfrasantos]

One of the most popular and practical methods is the Ziegler-Nichols method, specifically the oscillation method (also known as the Ultimate Gain method). This method involves:

• Setting Ki and Kd to zero
• Increasing Kp until the system oscillates with constant amplitude (this is called the ultimate gain Ku)
• Measuring the oscillation period (Tu)
• Using these values to calculate the final PID parameters

This implementation uses the Ziegler-Nichols oscillation method for auto-tuning your PID controller. Here's how it works:

The PidAutotuner class takes several parameters:

1. initialKp: Starting proportional gain
2. kpStep: How much to increase Kp when searching for oscillations
3. targetAmplitude: Desired oscillation amplitude
4. minOscillations: Minimum number of oscillations needed before calculating parameters
5. maxSteps: Maximum number of steps before giving up

The Tune method:

Sets Ki and Kd to zero and starts with the initial Kp
Gradually increases Kp until sustained oscillations are detected
Measures the oscillation period and amplitude
Calculates final PID parameters using the Ziegler-Nichols formulas

The auto-tuner detects peaks and valleys in the process variable to measure oscillations

Here's how you would use it:

// Create PID controller with initial parameters
controllers::Pid<float> pid({1.0f, 0.0f, 0.0f}, {-100.0f, 100.0f});

// Create auto-tuner
controllers::PidAutotuner<float> tuner(
    0.1f,    // initial Kp
    0.1f,    // Kp step size
    5.0f,    // target oscillation amplitude
    3,       // minimum oscillations
    1000     // maximum steps
);

// Run auto-tuning
auto result = tuner.Tune(pid, [&]() {
    // Your function to get process measurements
    return getMeasurement();
});

if (result.success) {
    // Apply the tuned parameters
    pid.SetTunnings(result.tunnings);
    std::cout << "Tuned parameters: Kp=" << result.tunnings.kp 
              << " Ki=" << result.tunnings.ki 
              << " Kd=" << result.tunnings.kd << "\n";
}

Possible implementation:

#ifndef CONTROLLERS_PID_AUTOTUNER_HPP
#define CONTROLLERS_PID_AUTOTUNER_HPP

#include "Pid.hpp"
#include <vector>
#include <optional>

namespace controllers {

template<typename QNumberType>
class PidAutotuner {
public:
    struct TuningState {
        QNumberType currentKp;
        std::vector<QNumberType> oscillationPeaks;
        QNumberType lastValue;
        bool isRising;
        uint32_t stepCount;
        uint32_t oscillationCount;
    };

    struct AutotuneResult {
        bool success;
        typename Pid<QNumberType>::Tunnings tunnings;
        QNumberType ultimateGain;     // Ku
        QNumberType ultimatePeriod;   // Tu
    };

    PidAutotuner(
        QNumberType initialKp,
        QNumberType kpStep,
        QNumberType targetAmplitude,
        uint32_t minOscillations,
        uint32_t maxSteps
    )
        : initialKp(initialKp)
        , kpStep(kpStep)
        , targetAmplitude(targetAmplitude)
        , minOscillations(minOscillations)
        , maxSteps(maxSteps)
    {}

    AutotuneResult Tune(Pid<QNumberType>& pid, const std::function<QNumberType()>& getMeasurement) {
        // Initialize state
        TuningState state{
            .currentKp = initialKp,
            .lastValue = QNumberType(0),
            .isRising = false,
            .stepCount = 0,
            .oscillationCount = 0
        };

        // Start with P-only control
        pid.SetTunnings({state.currentKp, QNumberType(0), QNumberType(0)});

        while (state.stepCount < maxSteps) {
            auto measurement = getMeasurement();
            DetectOscillation(state, measurement);

            // Check if we have enough oscillations to calculate parameters
            if (state.oscillationPeaks.size() >= minOscillations * 2) {
                return CalculatePIDParameters(state);
            }

            // If oscillations are too small, increase Kp
            if (state.stepCount % 100 == 0) {  // Check amplitude every 100 steps
                if (GetOscillationAmplitude(state) < targetAmplitude) {
                    state.currentKp += kpStep;
                    pid.SetTunnings({state.currentKp, QNumberType(0), QNumberType(0)});
                }
            }

            state.stepCount++;
        }

        // If we get here, we failed to find good parameters
        return {false, {QNumberType(0), QNumberType(0), QNumberType(0)}, QNumberType(0), QNumberType(0)};
    }

private:
    void DetectOscillation(TuningState& state, QNumberType currentValue) {
        bool wasRising = state.isRising;
        state.isRising = currentValue > state.lastValue;

        // Detect peak when direction changes
        if (wasRising != state.isRising) {
            state.oscillationPeaks.push_back(state.lastValue);
        }

        state.lastValue = currentValue;
    }

    QNumberType GetOscillationAmplitude(const TuningState& state) {
        if (state.oscillationPeaks.size() < 2) return QNumberType(0);

        QNumberType maxPeak = state.oscillationPeaks[0];
        QNumberType minPeak = state.oscillationPeaks[0];

        for (const auto& peak : state.oscillationPeaks) {
            if (peak > maxPeak) maxPeak = peak;
            if (peak < minPeak) minPeak = peak;
        }

        return maxPeak - minPeak;
    }

    AutotuneResult CalculatePIDParameters(const TuningState& state) {
        // Calculate ultimate gain (Ku) and ultimate period (Tu)
        QNumberType Ku = state.currentKp;
        
        // Calculate period from peak-to-peak time
        // We'll use the average of multiple oscillations
        QNumberType Tu = QNumberType(0);
        int periodCount = 0;
        
        for (size_t i = 0; i < state.oscillationPeaks.size() - 2; i += 2) {
            Tu += QNumberType(2);  // Assuming fixed time step of 1, multiply by 2 for full period
            periodCount++;
        }
        
        Tu /= QNumberType(periodCount);

        // Calculate PID parameters using Ziegler-Nichols method
        QNumberType Kp = QNumberType(0.6) * Ku;      // 0.6 Ku
        QNumberType Ki = QNumberType(1.2) * Ku / Tu; // 2 Kp / Tu
        QNumberType Kd = QNumberType(0.075) * Ku * Tu; // Kp * Tu / 8

        return {
            true,
            {Kp, Ki, Kd},
            Ku,
            Tu
        };
    }

private:
    QNumberType initialKp;
    QNumberType kpStep;
    QNumberType targetAmplitude;
    uint32_t minOscillations;
    uint32_t maxSteps;
};

} // namespace controllers

#endif

[github-actions]

Thank you for adding your first issue. We will be looking into it as soon as possible.