温控PID自测验程序

#pragma once
#ifndef _PID_H_
#define _PID_H_
#include <vector> 
#include <map>
using namespace std;

struct kpid { float kp, ki, kd; long i; };
struct eval
{
    float minrate;
    float maxrate;
    float avgrate;
    float avgcalc;
    float maxhold;
    float minhold;
    float avghold;
    float varhold;
    float countif;//平稳时间
    float gradual;
};
struct score { kpid pid; eval _eval[2]; };
typedef vector<score> scoretable;
typedef vector<scoretable> favortable; 
class cmd
{
public:
    virtual void setpid(const kpid& k)const {
        printf("-------------------------cmdpid:%.2f,%.2f,%.2f
", k.kp, k.ki, k.kd);
    }
    virtual void settarget(float t)const{
        printf("-------------------------cmdtarget:%.2f
",t);
    }
    virtual void report(const scoretable& table)const
    {
        FILE* file;
        if (0 == fopen_s(&file, "pid.table.txt", "a"))
        { 
            for (scoretable::const_iterator it = table.begin(); it != table.end(); ++it)
            {
                const score& score = *it;
                output(score, file);
            }
        }
        fclose(file);
    }
    virtual void report(const favortable& table)const
    { 
        printf("*************************print
");
        FILE* file;
        if (0 == fopen_s(&file, "pid.score.txt", "a"))
        {
            favortable::const_iterator it = table.begin();
            for (; it != table.end(); ++it)
            {
                const scoretable& table = *it; 
                for (scoretable::const_iterator it = table.begin(); it != table.end(); ++it)
                {
                    const score& score = *it;
                    output(score, file);
                }
                fprintf_s(file, "
");
            } 
        }
        fclose(file);
    }
    virtual void trace(const score& score)const
    {
        FILE* file;
        if (0 == fopen_s(&file, "pid.trace.txt", "a"))
        {
            output(score, file);
        }
        fclose(file);
    }
private:
    void output(const score& score ,FILE* file)const
    {
        fprintf_s(file, "PID %5.2f %5.2f %5.2f : ", score.pid.kp, score.pid.ki, score.pid.kd);
        for (int i = 0; i < 2; ++i) {
            fprintf_s(file, "[%c] avg:%.2f/%.2f min:%.2f max:%.2f T:%.2f  [T:%.2f (%.2f ~ %.2f) avg:%.2f]."
                , "+-"[i] 
                , score._eval[i].avgrate
                , score._eval[i].avgcalc
                , score._eval[i].minrate
                , score._eval[i].maxrate
                , score._eval[i].gradual
                , score._eval[i].countif
                , score._eval[i].minhold
                , score._eval[i].maxhold
                , score._eval[i].avghold);
        }
        fprintf_s(file, "
");
    }
};
class pid
{   
    template<class T ,class H>
    struct Template{     
        T val;
        vector<H*> list[2];
        ~Template() {  
            for (int i = 0; i < 2; ++i)
                for (; !list[i].empty(); list[i].pop_back())
                    delete list[i].back();
        }
    };
    enum tendecy { eupper = 0, elower = 1, efirst,equiet,etail,eidle};
    struct data { float targetvalue; enum tendecy tendecy;  int duration; };
    struct temp { float temperature, interval; 
    temp(float t,float i):temperature(t),interval(i){}
    }; 
    typedef Template<data, temp> segment;
    typedef Template<kpid, segment> piddata;
    typedef vector<piddata *> pidsort;
public: 
    class policy
    { 
    public:
        policy(const kpid pid = { 1.0,0.0,0.0,0})
            : _target{ 95,50,95,50,95,50 }
            , _pid(pid),i(0),k(0)
            , entry(3)
        {}
        virtual const kpid& trimpid(const favortable&);
        virtual float target()const;
        virtual bool segment();
        virtual bool oncepid();
        virtual int count() { return 20; }
    protected:
        kpid _pid;
        float _target[6];
        unsigned int i,k;
        map<float, int> _score;
        const int entry;
    };
public:
    explicit pid(cmd*);
    virtual ~pid();
    void apply(policy*);
    void init(); 
    void fini();  
    void input(float temperature, float interval); 
private:
    void sort();
    void score(); 
    void eval(const segment * _segment,struct eval&);
    
    void incr(temp *);
    void quiet(temp *);
    void decr(temp *);
    void first(float t);
    void tail(); 
    void stage();
    void ctrl();  
    void newpid(const kpid&);
    void newtarget(float target);
    void endtarget();
    void endpid();
    void newapply();
    void endapply(); 
private: 
    segment * _segment;
    piddata * _piddata;
    pidsort * _pidsort;
private:
    enum tendecy _state;
private: 
    scoretable _scoretable;
    favortable _favortable;
    cmd* _cmd;
    policy* _policy;
    vector<policy*> _policylist;
};
#endif



#include "pid.h"
#include <assert.h>
#include <algorithm>
#include <limits>
 
class defaultpolicy :public pid::policy
{
public:
    const kpid& trimpid(const favortable& t)
    {
        switch (k)
        {
        case 0:
            if (_pid.kp < 2.0f)
            {
                _pid.kp += 0.01;
                break;
            }
            else 
            {
                _pid.i = k = 1;
            } 
        case 1:
            if (_pid.ki < 1.2f)
            {
                _pid.ki += 0.001;
                break;
            }
            else
            {
                _pid.i = k = 2;
            } 
        case 2:
            if (_pid.kd < 1.2f)
            {
                _pid.kd += 0.001;
                break;
            }
            else
            {
                _pid.i = k = 3;
            } 
        default:
            break;
        }  
        return _pid;
    }
};
const kpid& pid::policy::trimpid(const favortable& t)
{
    if ( k < t.size())
    {
        const scoretable& _table = t[k];  

        struct kpid pid;
        struct eval val = {0};
        if (!_table.empty())
        {
            pid = _table[0].pid;
            val = _table[0]._eval[0];
        }
        else
            pid = _pid;
        switch (k)
        {
        case 0: 
            _pid.kp += pid.kp >= _pid.kp ? +0.005f : -0.005f;  
            if (++_score[_pid.kp] > entry)
            {
                _pid.i = k = 1;
                _score.clear();
            }
            if (val.gradual < 50)
            {
                break;
            } 
        case 1:
            _pid.ki += pid.ki >= _pid.ki ? +0.001f : -0.001f; 
            if (++_score[_pid.ki] > entry)
            {
                _pid.i = k = 2;
                _score.clear();
            }
        break;
        case 2:
            _pid.kd += pid.kd >= _pid.kd ? +0.001f : -0.001f; 
            if (++_score[_pid.kd] > entry)
            {
                _pid.i = k = 3;
                _score.clear();
            }
        break;
        default:
            break;
        }
    } 
    return _pid;
}
bool pid::policy::oncepid()
{
    return k < 3;
}
float pid::policy::target()const
{  
    return _target[i% _countof(_target)];
}
bool pid::policy::segment()
{
    return 0 == (++i % _countof(_target));
}

pid::pid(cmd* c) 
    : _state(eidle)
    , _cmd(c)
    , _policy(new defaultpolicy())
{ 
    assert(c);
    apply(_policy);
}
void pid::apply(policy* p)
{
    _policylist.push_back(p);
    _policy = p;
}
void pid::init()
{  
    _favortable.resize(3);
    _pidsort = new pidsort();
    newpid(_policy->trimpid(_favortable));
    newtarget(_policy->target());
}
void pid::fini()
{
    endtarget();
    endpid();
    score();
    sort();
    _cmd->trace(_scoretable.back()); 
    std::sort(_scoretable.begin(), _scoretable.end(),[](auto a, auto b){return a._eval[0].avgrate > b._eval[0].avgrate; });
    _cmd->report(_scoretable);
    _cmd->report(_favortable); 
    _favortable.clear(); 
    while (!_pidsort->empty())
    {
        delete _pidsort->back();
        _pidsort->pop_back();
    } 
    delete _pidsort;
    while (!_policylist.empty())
    {
        delete _policylist.back();
        _policylist.pop_back();
    } 
}
pid::~pid()
{ 
    delete _cmd; 
}
void pid::newapply()
{
    _policy = _policylist.back();
}
void pid::endapply()
{ 
    std::rotate(_policylist.begin(), _policylist.begin() + 1, _policylist.end());
}
void pid::newpid(const kpid& k)
{
    _cmd->setpid(k);
    _piddata = new piddata();
    _piddata->val = k;
}
void pid::newtarget(float target)
{
    _cmd->settarget(target);
    _segment = new segment();
    _segment->val.targetvalue = target;
    _state = efirst;
} 
void pid::ctrl()
{
    endtarget(); 
    if (_policy->segment())
    {
        endpid();
        score();
        sort(); 
        _cmd->trace(_scoretable.back());
        if (_policy->oncepid())
        {
            endapply();
            newapply();
        }  
        newpid(_policy->trimpid(_favortable));
    }
    newtarget(_policy->target()); 
}
void pid::stage()
{
    _segment->val.duration = 0; 
    _state = equiet;
}
void pid::quiet(temp * t)
{
    _segment->list[1].push_back(t);
    _segment->val.duration++; 
}
void pid::incr(temp * t)
{
    _segment->list[0].push_back(t);
}
void pid::decr(temp * t)
{
    _segment->list[0].push_back(t);
}
void pid::endpid()
{
    _pidsort->push_back(_piddata);
}
void pid::endtarget()
{
    _piddata->list[_segment->val.tendecy].push_back(_segment);
}
void pid::first(float t)
{
    _state = _segment->val.targetvalue > t ? eupper : elower;
    _segment->val.tendecy = _state;
}
void pid::tail()
{ 
    _state = etail;
} 
void pid::input(float t, float i)
{
    assert(_segment); 
    for(;;)
        switch (_state)
        { 
        case efirst:
            first(t);
            break;
        case eupper:
            incr(new temp(t,i));
            if (_segment->val.targetvalue < t)
            { 
                stage();
                break;
            } 
            return;
        case elower:
            decr(new temp(t, i));
            if (_segment->val.targetvalue > t)
            { 
                stage();
                break;
            } 
            return;
        case equiet:
            quiet(new temp(t, i));
            if (_segment->val.duration > _policy->count())
            {
                tail();
                break;
            } 
            return;
        case etail: 
            ctrl();
            return;
        case eidle: 
            return;
        }
}
void pid::eval(const segment * _segment, struct eval& _eval)
{
    int i;
    auto v = _segment->list;
    switch (_segment->val.tendecy)
    {
    case eupper:
    case elower:
        struct cnt
        {
            cnt(float t, float& q, bool b = true)
                : target(t)
                , quiet(q)
                , cmpval(b)
            {
                quiet = 0.f;
            }
            bool operator()(const struct temp* a)
            {
                bool b = a->temperature > target + 0.5
                    || a->temperature < target - 0.5;
                if (cmpval == b)
                {
                    quiet += a->interval;
                    return true;
                }
                else
                    return false;
            }
            float &quiet;
            float target;
            bool cmpval;
        };
        i = 0;
        if (!v[i].empty())
        {
            _eval.minrate = FLT_MAX;
            _eval.maxrate = FLT_MIN;
            _eval.avgrate = .0f;
            auto it = v[i].begin();
            float _t = (*it)->temperature;
            while (++it != v[i].end())
            {
                float r = ((*it)->temperature - _t) / (*it)->interval;
                _t = (*it)->temperature;
                if (r > _eval.maxrate)
                    _eval.maxrate = r;
                if (r < _eval.minrate)
                    _eval.minrate = r;
                _eval.avgrate += r;
            } 
            _eval.avgrate /= v[i].size();
            std::count_if(v[i].begin(), v[i].end(), cnt(_segment->val.targetvalue, _eval.gradual, false));
            /**/
            float cost = 0.f;
            it = v[i].begin();
            while (++it != v[i].end())
                cost += (*it)->interval; 
            _eval.avgcalc = (v[i].back()->temperature - v[i].front()->temperature) / cost;
        }
        i = 1;
        if (!v[i].empty())
        {
            struct cmp {
                bool operator()(const struct temp* a, const struct temp* b) {
                    return a->temperature > b->temperature;
                }
            };
            auto it = v[i].begin();
            auto minmax = std::minmax_element(it + 1, v[i].end(), cmp());
            _eval.maxhold = (*minmax.first)->temperature - _segment->val.targetvalue;
            _eval.minhold = (*minmax.second)->temperature - _segment->val.targetvalue;
            std::count_if(v[i].begin(), v[i].end(), cnt(_segment->val.targetvalue, _eval.countif, true));
            /**/
            float temp = 0.f;
            for (; it != v[i].end(); ++it)
                temp +=abs((*it)->temperature - _segment->val.targetvalue);
            _eval.avghold = temp / v[i].size(); 
            _eval.varhold = 0.f;
        }
        break;
    }
}
void pid::score()
{
    struct score _score;
    struct eval _eval;
    _score.pid = _piddata->val;
    for (int i = 0; i < 2; ++i)
    {
        _score._eval[i].maxrate = FLT_MIN;
        _score._eval[i].minrate = FLT_MAX;
        _score._eval[i].gradual = FLT_MIN;
        _score._eval[i].maxhold = FLT_MIN;
        _score._eval[i].minhold = FLT_MAX;
        _score._eval[i].varhold = FLT_MIN;
        _score._eval[i].countif = FLT_MIN;
        _score._eval[i].avghold = 0.f;
        _score._eval[i].avgcalc = 0.f;
        _score._eval[i].avgrate = 0.f;
        for (int j = 0; j < _piddata->list[i].size(); ++j)
        {
            eval(_piddata->list[i][j], _eval);

            if (_eval.maxrate > _score._eval[i].maxrate)
                _score._eval[i].maxrate = _eval.maxrate;
            if (_eval.minrate < _score._eval[i].minrate)
                _score._eval[i].minrate = _eval.minrate; 
            if (_eval.gradual > _score._eval[i].gradual)
                _score._eval[i].gradual = _eval.gradual;

            if (_eval.maxhold > _score._eval[i].maxhold)
                _score._eval[i].maxhold = _eval.maxhold; 
            if (_eval.minhold < _score._eval[i].minhold)
                _score._eval[i].minhold = _eval.minhold; 
            
            if (_eval.varhold > _score._eval[i].varhold)
                _score._eval[i].varhold = _eval.varhold;
            if (_eval.countif > _score._eval[i].countif)
                _score._eval[i].countif = _eval.countif;

            _score._eval[i].avgrate += _eval.avgrate;
            _score._eval[i].avgcalc += _eval.avgcalc;

            _score._eval[i].avghold += _eval.avghold;
        }
        _score._eval[i].avgcalc /= _piddata->list[i].size();
        _score._eval[i].avgrate /= _piddata->list[i].size();
        _score._eval[i].avghold /= _piddata->list[i].size();
    }
    _scoretable.push_back(_score);
}
void pid::sort()
{
    struct cmp 
    {
        cmp(int t=0) {}
        bool operator()(const struct score& a, const struct score& b) const 
        {
            switch (t)
            {
            case 0:
                return a._eval[0].avgrate > b._eval[0].avgrate;
            case 1:
                return (a._eval[0].maxhold - a._eval[0].minhold) > (b._eval[0].maxhold - b._eval[0].minhold);
            case 2:
                return (a._eval[0].maxrate - a._eval[0].minrate) < (b._eval[0].maxrate - b._eval[0].minrate);
            default:
                return a._eval[0].maxrate > b._eval[0].maxrate;
            }
        }
        int t;
    };  
    int topnum = _scoretable.end() - _scoretable.begin();
    topnum = min(topnum, 9);
    for (size_t i = 0; i < _favortable.size(); ++i)
    { 
        _favortable[i].resize(topnum);
        std::partial_sort_copy(_scoretable.begin(), _scoretable.end()
            , _favortable[i].begin(), _favortable[i].end(), cmp(i));
    }
}