zoukankan      html  css  js  c++  java
  • 温控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));
        }
    }
  • 相关阅读:
    hdu 4027 Can you answer these queries?
    hdu 4041 Eliminate Witches!
    hdu 4036 Rolling Hongshu
    pku 2828 Buy Tickets
    hdu 4016 Magic Bitwise And Operation
    pku2886 Who Gets the Most Candies?(线段树+反素数打表)
    hdu 4039 The Social Network
    hdu 4023 Game
    苹果官方指南:Cocoa框架(2)(非原创)
    cocos2d 中 CCNode and CCAction
  • 原文地址:https://www.cnblogs.com/xuyouzhu/p/10829847.html
Copyright © 2011-2022 走看看