Andrew's git - arduino-weather.git/blob - libraries/Mitov/Mitov

   1////////////////////////////////////////////////////////////////////////////////
   2//                                                                            //
   3//     This software is supplied under the terms of a license agreement or    //
   4//     nondisclosure agreement with Mitov Software and may not be copied      //
   5//     or disclosed except in accordance with the terms of that agreement.    //
   6//         Copyright(c) 2002-2016 Mitov Software. All Rights Reserved.        //
   7//                                                                            //
   8////////////////////////////////////////////////////////////////////////////////
   9
  10#ifndef _MITOV_PID_CONTROLLER_h
  11#define _MITOV_PID_CONTROLLER_h
  12
  13#include <Mitov.h>
  14
  15namespace Mitov
  16{
  17        class PIDController : public Mitov::CommonEnableFilter, public Mitov::ClockingSupport
  18        {
  19                typedef Mitov::CommonEnableFilter inherited;
  20
  21        public:
  22                OpenWire::SinkPin       ManualControlInputPin;
  23
  24        public:
  25                float ProportionalGain = 0.1f;
  26                float IntegralGain = 0.1f;
  27                float DerivativeGain = 0.1f;
  28
  29                float SetPoint = 0.0f;
  30
  31                float InitialValue = 0.0f;
  32
  33        protected:
  34                unsigned long   FLastTime;
  35                float   FOutput = 0.0f;
  36                float   FInput;
  37                float   FLastInput;
  38                float   FITerm;
  39
  40        public:
  41                void SetEnabled( bool AValue )
  42                {
  43            if( Enabled == AValue )
  44                return;
  45
  46            Enabled = AValue;
  47                        if( Enabled )
  48                                Initialize();
  49
  50                }
  51
  52        protected:
  53                virtual void DoReceive( void *_Data )
  54                {
  55                        FInput = *(float *)_Data;
  56                }
  57
  58                void DoManualControlReceive( void *_Data )
  59                {
  60                        if( Enabled )
  61                                return;
  62
  63                        FOutput = *(float *)_Data;
  64                }
  65
  66                virtual void DoClockReceive( void *_Data ) override
  67                {
  68                        OutputPin.Notify( &FOutput );
  69                }
  70
  71        protected:
  72                void Initialize()
  73                {
  74                        FITerm = FOutput;
  75                        FLastInput = FInput;
  76                        FLastTime = micros();
  77                }
  78
  79        protected:
  80                virtual void SystemStart()
  81                {
  82                        FInput = InitialValue;
  83                        Initialize();
  84                        inherited::SystemStart();
  85                }
  86
  87                virtual void SystemLoopBegin( unsigned long currentMicros )
  88                {
  89                        inherited::SystemLoopBegin( currentMicros );
  90                        if( ! Enabled ) 
  91                                return;
  92
  93                        unsigned long timeChange = ( currentMicros - FLastTime );
  94                        float ANormalizedTime = timeChange / 1000000;
  95
  96                        // Compute all the working error variables
  97                        double error = SetPoint - FInput;
  98//                      ITerm += ( ki * error ) * ANormalizedTime;
  99                        FITerm = constrain( FITerm + ( IntegralGain * error ) * ANormalizedTime, 0, 1 );
 100
 101                        double dInput = ( FInput - FLastInput ) * ANormalizedTime;
 102 
 103                        // Compute PID Output
 104                        float AOutput = constrain( ProportionalGain * error + FITerm - DerivativeGain * dInput, 0, 1 );
 105          
 106                        // Remember some variables for next time
 107                        FLastInput = FInput;
 108                        FLastTime = currentMicros;
 109
 110                        if( AOutput == FOutput )
 111                                return;
 112
 113                        if( ClockInputPin.IsConnected() )
 114                                return;
 115
 116                        OutputPin.Notify( &FOutput );
 117                }
 118
 119        public:
 120                PIDController()
 121                {
 122                        ManualControlInputPin.SetCallback( MAKE_CALLBACK( PIDController::DoManualControlReceive ));
 123                }
 124
 125        };
 126//---------------------------------------------------------------------------
 127}
 128
 129#endif