TRC Library for FRC (2011) 2011
TRC Library Code Project

C:/Users/Michael/Frc/2011/code/trclib/TrcPIDCtrl.h

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 #endif
00015 
00016 #ifndef _TRCPIDCTRL_H
00017 #define _TRCPIDCTRL_H
00018 
00019 #ifdef MOD_ID
00020     #undef MOD_ID
00021 #endif
00022 #define MOD_ID                  MOD_PIDCTRL
00023 #ifdef MOD_NAME
00024     #undef MOD_NAME
00025 #endif
00026 #define MOD_NAME                "TrcPIDCtrl"
00027 
00034 class PIDInput
00035 {
00036 public:
00043     virtual
00044     double
00045     GetInput(
00046         __in TrcPIDCtrl *pidCtrl
00047         ) = 0;
00048 };  //class PIDInput
00049 
00064 class TrcPIDCtrl
00065 {
00066 private:
00067     #define PIDCTRLF_ABS_SETPT  0x00000001
00068     #define PIDCTRLF_INVERSE    0x00000002
00069 
00070     float     m_Kp;
00071     float     m_Ki;
00072     float     m_Kd;
00073     PIDInput *m_pidInput;
00074     UINT32    m_flags;
00075     float     m_minInput;
00076     float     m_maxInput;
00077     float     m_minOutput;
00078     float     m_maxOutput;
00079     float     m_prevError;
00080     double    m_totalError;
00081     float     m_setPoint;
00082     float     m_tolerance;
00083 
00084 public:
00096     TrcPIDCtrl(
00097         __in float     Kp,
00098         __in float     Ki,
00099         __in float     Kd,
00100         __in PIDInput *pidInput,
00101         __in bool      fAbsSetPt = false,
00102         __in bool      fInverse = false
00103         ): m_Kp(Kp),
00104            m_Ki(Ki),
00105            m_Kd(Kd),
00106            m_pidInput(pidInput)
00107     {
00108         TLevel(INIT);
00109         TEnterMsg(("Kp=%f,Ki=%f,Kd=%f", Kp, Ki, Kd));
00110 
00111         m_flags = 0;
00112         if (fAbsSetPt)
00113         {
00114             m_flags |= PIDCTRLF_ABS_SETPT;
00115         }
00116         if (fInverse)
00117         {
00118             m_flags |= PIDCTRLF_INVERSE;
00119         }
00120 
00121         m_minInput = 0.0;
00122         m_maxInput = 0.0;
00123 
00124         m_minOutput = -1.0;
00125         m_maxOutput = 1.0;
00126 
00127         m_prevError = 0.0;
00128         m_totalError = 0.0;
00129 
00130         m_setPoint = 0.0;
00131         m_tolerance = 0.0;
00132 
00133         TExit();
00134     }   //TrcPIDCtrl
00135 
00139     ~TrcPIDCtrl(
00140         void
00141         )
00142     {
00143         TLevel(INIT);
00144         TEnter();
00145         TExit();
00146     }   //~TrcPIDCtrl
00147 
00151     void
00152     Reset(
00153         void
00154         )
00155     {
00156         TLevel(API);
00157         TEnter();
00158 
00159         m_prevError = 0.0;
00160         m_totlError = 0.0;
00161 
00162         TExit();
00163         return;
00164     }   //Reset
00165 
00171     float
00172     GetKp(
00173         void
00174         )
00175     {
00176         TLevel(API);
00177         TEnter();
00178         TExitMsg(("=%f", m_Kp);
00179         return m_Kp;
00180     }   //GetKp
00181 
00187     float
00188     GetKi(
00189         void
00190         )
00191     {
00192         TLevel(API);
00193         TEnter();
00194         TExitMsg(("=%f", m_Ki);
00195         return m_Ki;
00196     }   //GetKi
00197 
00203     float
00204     GetKd(
00205         void
00206         )
00207     {
00208         TLevel(API);
00209         TEnter();
00210         TExitMsg(("=%f", m_Kd);
00211         return m_Kd;
00212     }   //GetKd
00213 
00221     void
00222     SetPID(
00223         __in float Kp,
00224         __in float Ki,
00225         __in float Kd
00226         )
00227     {
00228         TLevel(API);
00229         TEnterMsg(("Kp=%f,Ki=%f,Kd=%f", Kp, Ki, Kd));
00230 
00231         m_Kp = Kp;
00232         m_Ki = Ki;
00233         m_Kd = Kd;
00234 
00235         TExit();
00236         return;
00237     }   //SetPID
00238 
00244     float
00245     GetError(
00246         void
00247         )
00248     {
00249         TLevel(API);
00250         TEnter();
00251         TExitMsg(("=%f", m_prevError));
00252         return m_prevError;
00253     }   //GetError
00254 
00260     float
00261     GetTarget(
00262         void
00263         )
00264     {
00265         TLevel(API);
00266         TEnter();
00267         TExitMsg(("=%f", m_setPoint));
00268         return m_setPoint;
00269     }   //GetTarget
00270 
00277     void
00278     SetTarget(
00279         __in float setpt,
00280         __in float tolerance
00281         )
00282     {
00283         TLevel(API);
00284         TEnterMsg(("setpt=%f", setpt));
00285 
00286         if (!(m_flags & PIDCTRLF_ABS_SETPT))
00287         {
00288             setpt += m_pidInput->GetInput(this);
00289         }
00290 
00291         if (m_maxInput > m_minInput)
00292         {
00293             if (setpt > m_maxInput)
00294             {
00295                 m_setPoint = m_maxInput;
00296             }
00297             else if (setpt < m_minInput)
00298             {
00299                 m_setPoint = m_minInput;
00300             }
00301             else
00302             {
00303                 m_setPoint = setpt;
00304             }
00305         }
00306         else
00307         {
00308             m_setPoint = setpt;
00309         }
00310         m_tolerance = tolerance;
00311 
00312         TExit();
00313         return;
00314     }   //SetTarget
00315 
00322     bool
00323     OnTarget(
00324         void
00325         )
00326     {
00327         bool fOnTarget;
00328 
00329         TLevel(API);
00330         TEnter();
00331 
00332         fOnTarget = fabs(m_prevError) <= m_tolerance;
00333 
00334         TExitMsg(("=%x", fOnTarget));
00335         return fOnTarget;
00336     }   //OnTarget
00337 
00345     void
00346     SetInputRange(
00347         __in float minInput,
00348         __in float maxInput
00349         )
00350     {
00351         TLevel(API);
00352         TEnterMsg(("min=%f,max=%f", minInput, maxInput));
00353 
00354         m_minInput = minInput;
00355         m_maxInput = maxInput;
00356         SetTarget(m_setPoint, m_tolerance);
00357 
00358         TExit();
00359         return;
00360     }   //SetInputRange
00361 
00368     void
00369     SetOutputRange(
00370         __in float minOutput,
00371         __in float maxOutput
00372         )
00373     {
00374         TLevel(API);
00375         TEnterMsg(("min=%f,max=%f", minOutput, maxOutput));
00376 
00377         m_minOutput = minOutput;
00378         m_maxOutput = maxOutput;
00379 
00380         TExit();
00381         return;
00382     }   //SetInputRange
00383 
00390     float
00391     CalcPIDOutput(
00392         void
00393         )
00394     {
00395         float output;
00396         float error;
00397         double adjTotalError;
00398 
00399         TLevel(API);
00400         TEnterMsg(("input=%f", input));
00401 
00402         error = m_setPoint - m_pidInput->GetInput(this);
00403         if (m_flags & PIDCTRLF_INVERSE)
00404         {
00405             error = -error;
00406         }
00407 
00408         adjTotalError = m_Ki*(m_totalError + error);
00409         if ((adjTotalError > m_minOutput) && (adjTotalError < m_maxOutput))
00410         {
00411             m_totalError += error;
00412         }
00413 
00414         output = m_Kp*error + m_Ki*m_totalError + m_Kd*(error - m_prevError);
00415         m_prevError = error;
00416 
00417         if (output < m_minOutput)
00418         {
00419             output = m_minOutput;
00420         }
00421         else if (output > m_maxOutput)
00422         {
00423             output = m_maxOutput;
00424         }
00425 
00426         TExitMsg(("=%f", output));
00427         return output;
00428     }   //CalcPIDOutput
00429 };  //class TrcPIDCtrl
00430 
00431 #endif  //ifndef _TRCPIDCTRL_H
 All Classes Files Functions Variables Defines