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

C:/Users/Michael/Ftc/2011/code/trclib/pidctrl.h

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 #endif
00014 
00015 #ifndef _PIDCTRL_H
00016 #define _PIDCTRL_H
00017 
00018 #pragma systemFile
00019 
00020 #ifdef MOD_ID
00021     #undef MOD_ID
00022 #endif
00023 #define MOD_ID                  MOD_PIDCTRL
00024 
00025 //
00026 // Constants.
00027 //
00028 #define PIDCTRLF_USER_MASK      0x00ff
00029 #define PIDCTRLF_INVERSE        0x0001
00030 #define PIDCTRLF_ABS_SETPOINT   0x0002
00031 
00032 #define PID_TOLERANCE_NO_STOP   -1.0
00033 
00034 #define PIDCtrlGetTarget(p)     (p.setPoint)
00035 
00036 //
00037 // Type definitions.
00038 //
00039 typedef struct
00040 {
00041     int   pidCtrlID;
00042     float Kp;
00043     float Ki;
00044     float Kd;
00045     float minOutput;
00046     float maxOutput;
00047     int   pidCtrlFlags;
00048     long  time;
00049     float input;
00050     float prevError;
00051     float totalError;
00052     float setPoint;
00053     float targetTolerance;
00054 } PIDCTRL;
00055 
00056 //
00057 // Import function prototypes.
00058 //
00059 float
00060 PIDCtrlGetInput(
00061     __in int pidCtrlID
00062     );
00063 
00073 #define PIDCtrlIsOnTarget(p,t)  (abs(p.prevError) <= p.targetTolerance)
00074 
00080 void
00081 PIDCtrlReset(
00082     __out PIDCTRL &pidCtrl
00083     )
00084 {
00085     TFuncName("PIDCtrlReset");
00086     TLevel(API);
00087     TEnter();
00088 
00089     pidCtrl.time = nPgmTime;
00090     pidCtrl.input = PIDCtrlGetInput(pidCtrl.pidCtrlID);
00091     pidCtrl.prevError = 0.0;
00092     pidCtrl.totalError = 0.0;
00093 
00094     TExit();
00095     return;
00096 }   //PIDCtrlReset
00097 
00110 void
00111 PIDCtrlInit(
00112     __out PIDCTRL &pidCtrl,
00113     __in  int pidCtrlID,
00114     __in  float Kp,
00115     __in  float Ki,
00116     __in  float Kd,
00117     __in  float minOutput,
00118     __in  float maxOutput,
00119     __in  int pidCtrlFlags
00120     )
00121 {
00122     TFuncName("PIDCtrlInit");
00123     TLevel(INIT);
00124     TEnter();
00125 
00126     pidCtrl.pidCtrlID = pidCtrlID;
00127     pidCtrl.Kp = Kp;
00128     pidCtrl.Ki = Ki;
00129     pidCtrl.Kd = Kd;
00130     pidCtrl.minOutput = minOutput;
00131     pidCtrl.maxOutput = maxOutput;
00132     pidCtrl.pidCtrlFlags = pidCtrlFlags & PIDCTRLF_USER_MASK;
00133     pidCtrl.setPoint = 0.0;
00134     pidCtrl.targetTolerance = 0.0;
00135     PIDCtrlReset(pidCtrl);
00136 
00137     TExit();
00138     return;
00139 }   //PIDCtrlInit
00140 
00148 void
00149 PIDCtrlSetPowerLimit(
00150     __out PIDCTRL &pidCtrl,
00151     __in  float minOutput,
00152     __in  float maxOutput
00153     )
00154 {
00155     TFuncName("PIDCtrlSetPwrLimit");
00156     TLevel(API);
00157     TEnterMsg(("Min=%5.1f,Max=%5.1f", minOutput, maxOutput));
00158 
00159     pidCtrl.minOutput = minOutput;
00160     pidCtrl.maxOutput = maxOutput;
00161 
00162     TExit();
00163     return;
00164 }   //PIDCtrlSetPowerLimit
00165 
00173 void
00174 PIDCtrlSetTarget(
00175     __inout PIDCTRL &pidCtrl,
00176     __in    float setPoint,
00177     __in    float targetTolerance
00178     )
00179 {
00180     TFuncName("PIDCtrlSetTarget");
00181     TLevel(API);
00182     TEnterMsg(("Tg=%5.1f,To=%5.1f", setPoint, targetTolerance));
00183 
00184     if (!(pidCtrl.pidCtrlFlags & PIDCTRLF_ABS_SETPOINT))
00185     {
00186         setPoint += PIDCtrlGetInput(pidCtrl.pidCtrlID);
00187     }
00188     pidCtrl.setPoint = setPoint;
00189     pidCtrl.targetTolerance = targetTolerance;
00190 
00191     TExit();
00192     return;
00193 }   //PIDCtrlSetTarget
00194 
00202 float
00203 PIDCtrlOutput(
00204     __inout PIDCTRL &pidCtrl
00205     )
00206 {
00207     float output;
00208     long currTime;
00209     float currInput;
00210     float error;
00211     float adjTotalError;
00212 
00213     TFuncName("PIDCtrlOutput");
00214     TLevel(API);
00215     TEnter();
00216 
00217     currTime = nPgmTime;
00218     currInput = PIDCtrlGetInput(pidCtrl.pidCtrlID);
00219     error = pidCtrl.setPoint - currInput;
00220     if (pidCtrl.pidCtrlFlags & PIDCTRLF_INVERSE)
00221     {
00222         error = -error;
00223     }
00224     adjTotalError = pidCtrl.Ki*(pidCtrl.totalError + error);
00225     if ((adjTotalError >= pidCtrl.minOutput) &&
00226         (adjTotalError <= pidCtrl.maxOutput))
00227     {
00228         pidCtrl.totalError += error;
00229     }
00230 
00231     output = pidCtrl.Kp*error +
00232              pidCtrl.Ki*pidCtrl.totalError +
00233              pidCtrl.Kd*(error - pidCtrl.prevError);
00234 
00235     pidCtrl.time = currTime;
00236     pidCtrl.input = currInput;
00237     pidCtrl.prevError = error;
00238     if (output < pidCtrl.minOutput)
00239     {
00240         output = pidCtrl.minOutput;
00241     }
00242     else if (output > pidCtrl.maxOutput)
00243     {
00244         output = pidCtrl.maxOutput;
00245     }
00246 
00247     TExitMsg(("=%f", output));
00248     return output;
00249 }   //PIDCtrlOutput
00250 
00251 #endif  //ifndef _PIDCTRL_H
 All Data Structures Files Functions Variables Defines