ftclib/pidctrl.h
author Michael Tsang
Sun Aug 11 03:30:20 2013 -0700 (2013-08-11)
changeset 112 51c6eed905fe
parent 57 ce236cfe0443
permissions -rw-r--r--
Merged
     1 #if 0
     2 /// Copyright (c) Titan Robotics Club. All rights reserved.
     3 ///
     4 /// <module name="pidctrl.h" />
     5 ///
     6 /// <summary>
     7 ///     This module contains the library functions for PID Control.
     8 /// </summary>
     9 ///
    10 /// <remarks>
    11 ///     Environment: RobotC for Lego Mindstorms NXT.
    12 /// </remarks>
    13 #endif
    14 
    15 #ifndef _PIDCTRL_H
    16 #define _PIDCTRL_H
    17 
    18 #pragma systemFile
    19 
    20 #ifdef MOD_ID
    21     #undef MOD_ID
    22 #endif
    23 #define MOD_ID                  MOD_PIDCTRL
    24 
    25 //
    26 // Constants.
    27 //
    28 #define PIDCTRLO_INVERSE        0x0001
    29 #define PIDCTRLO_ABS_SETPT      0x0002
    30 #define PIDCTRLO_NO_OSCILLATE   0x0004
    31 #define PIDCTRLO_INTEGRATE      0x0008
    32 
    33 #define PIDCTRLF_LIMIT_SETPT    0x0001
    34 
    35 //
    36 // Macros.
    37 //
    38 /**
    39  *  This macro checks if the two PID controllers are the same.
    40  *
    41  *  @param p1 Points to the first PID controller.
    42  *  @param p2 Points to the second PID controller.
    43  *
    44  *  @return Returns true if the two PID controllers are the same.
    45  */
    46 #define PIDCtrlCheck(p1,p2)     (&(p1) == &(p2))
    47 
    48 /**
    49  *  This macro returns the SetPoint of the PID controller.
    50  *
    51  *  @param p Points to the PID Control structure.
    52  *
    53  *  @return Returns the setpoint value.
    54  */
    55 #define PIDCtrlGetTarget(p)     ((p).setPoint)
    56 
    57 /**
    58  *  This macro returns the previous error of the PID controller.
    59  *
    60  *  @param p Points to the PID Control structure.
    61  *
    62  *  @return Returns the error value.
    63  */
    64 #define PIDCtrlGetError(p)      ((p).prevError)
    65 
    66 /**
    67  *  This macro displays the PID controller info: Target, Error, Input and
    68  *  Output on the LCD display at the given line.
    69  *
    70  *  @param p Points to the PID Control structure.
    71  *  @param n Specifies the LCD line number.
    72  */
    73 #define PIDCtrlDisplayInfo(n,p) nxtDisplayTextLine(n, "T=%5.1f,E=%5.1f",    \
    74                                                    (p).setPoint,            \
    75                                                    (p).prevError);          \
    76                                 nxtDisplayTextLine((n)+1, "I=%5.1f,O=%5.1f",\
    77                                                    PIDCtrlGetInput(p),      \
    78                                                    (p).prevOutput)
    79 
    80 //
    81 // Type definitions.
    82 //
    83 typedef struct
    84 {
    85     float Kp;
    86     float Ki;
    87     float Kd;
    88     float tolerance;
    89     long  settlingTime;
    90     float minOutput;
    91     float maxOutput;
    92     float setPtLowLimit;
    93     float setPtHighLimit;
    94     int   options;
    95     int   flags;
    96     float prevError;
    97     float totalError;
    98     long  startSettling;
    99     float setPoint;
   100     float prevOutput;
   101 } PIDCTRL;
   102 
   103 //
   104 // Import function prototypes.
   105 //
   106 
   107 /**
   108  *  This function is called by the PID controller to get the current input
   109  *  value.
   110  *
   111  *  @param pidCtrl Points to the PID Control structure.
   112  */
   113 float
   114 PIDCtrlGetInput(
   115     PIDCTRL &pidCtrl
   116     );
   117 
   118 /**
   119  *  This function resets the PID control.
   120  *
   121  *  @param pidCtrl Points to the PID Control structure to be reset.
   122  */
   123 void
   124 PIDCtrlReset(
   125     PIDCTRL &pidCtrl
   126     )
   127 {
   128     TFuncName("PIDCtrlReset");
   129     TLevel(API);
   130     TEnter();
   131 
   132     pidCtrl.prevError = 0.0;
   133     pidCtrl.totalError = 0.0;
   134     pidCtrl.prevOutput = 0.0;
   135 
   136     TExit();
   137     return;
   138 }   //PIDCtrlReset
   139 
   140 /**
   141  *  This function initializes the PID control object.
   142  *
   143  *  @param pidCtrl Points to the PID Control structure.
   144  *  @param Kp Specifies the Kp constant.
   145  *  @param Ki Specifies the Ki constant.
   146  *  @param Kd Specifies the Kd constant.
   147  *  @param tolerance Specifies the on-target tolerance.
   148  *  @param settlingTime Specifies the on-target settling time in msec.
   149  *  @param options Optionally specifies the PID controller options:
   150  *         PIDCTRLO_INVERSE - Setpoint is inverse.
   151  *         PIDCTRLO_ABS_SETPT - Setpoint is absolute.
   152  *         PIDCTRLO_NO_OSCILLATE - No oscillation.
   153  *         PIDCTRLO_INTEGRATE - Integrate the output (for speed control).
   154  *  @param initialOutput Optionally specifies the initial prevOutput value,
   155  *         applicable only in speed control mode.
   156  */
   157 void
   158 PIDCtrlInit(
   159     PIDCTRL &pidCtrl,
   160     float Kp,
   161     float Ki,
   162     float Kd,
   163     float tolerance,
   164     long  settlingTime,
   165     int options = 0,
   166     float initialOutput = 0.0
   167     )
   168 {
   169     TFuncName("PIDCtrlInit");
   170     TLevel(INIT);
   171     TEnter();
   172 
   173     pidCtrl.Kp = Kp;
   174     pidCtrl.Ki = Ki;
   175     pidCtrl.Kd = Kd;
   176     pidCtrl.tolerance = tolerance;
   177     pidCtrl.settlingTime = settlingTime;
   178     pidCtrl.minOutput = MOTOR_MIN_VALUE;
   179     pidCtrl.maxOutput = MOTOR_MAX_VALUE;
   180     pidCtrl.setPtLowLimit = 0.0;
   181     pidCtrl.setPtHighLimit = 0.0;
   182     pidCtrl.options = options;
   183     pidCtrl.flags = 0;
   184     pidCtrl.startSettling = 0;
   185     pidCtrl.setPoint = 0.0;
   186     pidCtrl.prevOutput = initialOutput;
   187     PIDCtrlReset(pidCtrl);
   188 
   189     TExit();
   190     return;
   191 }   //PIDCtrlInit
   192 
   193 /**
   194  *  This function sets the output power limits.
   195  *
   196  *  @param pidCtrl Points to the PID Control structure.
   197  *  @param minOutput Specifies the minimum output level.
   198  *  @param maxOutput Specifies the maximum output level.
   199  */
   200 void
   201 PIDCtrlSetPowerLimits(
   202     PIDCTRL &pidCtrl,
   203     float minOutput,
   204     float maxOutput
   205     )
   206 {
   207     TFuncName("PIDCtrlSetPwrLimits");
   208     TLevel(API);
   209     TEnterMsg(("Min=%5.1f,Max=%5.1f", minOutput, maxOutput));
   210 
   211     pidCtrl.minOutput = minOutput;
   212     pidCtrl.maxOutput = maxOutput;
   213 
   214     TExit();
   215     return;
   216 }   //PIDCtrlSetPowerLimits
   217 
   218 /**
   219  *  This function sets the setpoint limits.
   220  *
   221  *  @param pidCtrl Points to the PID Control structure.
   222  *  @param setPtLowLimit Specifies the low limit of the setpoint.
   223  *  @param setPtHighLimit Specifies the high limit of the setpoint.
   224  *
   225  *  @note If both setPtLowLimit and setPtHighLimit are zero, it means the
   226  *        the setpoint limits are cleared.
   227  */
   228 void
   229 PIDCtrlSetSetpointLimits(
   230     PIDCTRL &pidCtrl,
   231     float setPtLowLimit,
   232     float setPtHighLimit
   233     )
   234 {
   235     TFuncName("PIDCtrlSetSetpointLimits");
   236     TLevel(API);
   237     TEnterMsg(("Lo=%5.1f,Hi=%5.1f", setPtLowLimit, setPtHighLimit));
   238 
   239     pidCtrl.setPtLowLimit = setPtLowLimit;
   240     pidCtrl.setPtHighLimit = setPtHighLimit;
   241     if ((setPtLowLimit == 0.0) && (setPtHighLimit == 0.0))
   242     {
   243         pidCtrl.flags &= ~PIDCTRLF_LIMIT_SETPT;
   244     }
   245     else
   246     {
   247         pidCtrl.flags |= PIDCTRLF_LIMIT_SETPT;
   248     }
   249 
   250     TExit();
   251     return;
   252 }   //PIDCtrlSetSetPointLimits
   253 
   254 /**
   255  *  This function sets the SetPoint.
   256  *
   257  *  @param pidCtrl Points to the PID Control structure.
   258  *  @param setPoint Specifies the SetPoint target.
   259  *  @param currInput Specifies the current input value.
   260  */
   261 void
   262 PIDCtrlSetTarget(
   263     PIDCTRL &pidCtrl,
   264     float setPoint,
   265     float currInput
   266     )
   267 {
   268     TFuncName("PIDCtrlSetTarget");
   269     TLevel(API);
   270     TEnter();
   271 
   272     if (!(pidCtrl.options & PIDCTRLO_ABS_SETPT))
   273     {
   274         setPoint += currInput;
   275     }
   276 
   277     if (pidCtrl.flags & PIDCTRLF_LIMIT_SETPT)
   278     {
   279         if (setPoint < pidCtrl.setPtLowLimit)
   280         {
   281             setPoint = pidCtrl.setPtLowLimit;
   282         }
   283         else if (setPoint > pidCtrl.setPtHighLimit)
   284         {
   285             setPoint = pidCtrl.setPtHighLimit;
   286         }
   287     }
   288 
   289     pidCtrl.setPoint = setPoint;
   290     pidCtrl.prevError = setPoint - currInput;
   291     pidCtrl.totalError = 0.0;
   292     pidCtrl.startSettling = nPgmTime;
   293 
   294     TExit();
   295     return;
   296 }   //PIDCtrlSetTarget
   297 
   298 /**
   299  *  This function determines if we are on target by checking if the previous
   300  *  error is within target tolerance and remain within tolerance for at least
   301  *  the settling period.
   302  *
   303  *  @param pidCtrl Points to the PID Control structure.
   304  *
   305  *  @returns Returns true if we are on target, false otherwise.
   306  */
   307 bool
   308 PIDCtrlIsOnTarget(
   309     PIDCTRL &pidCtrl
   310     )
   311 {
   312     bool fOnTarget = false;
   313 
   314     TFuncName("PIDCtrlIsOnTarget");
   315     TLevel(HIFREQ);
   316     TEnter();
   317 
   318     if (pidCtrl.options & PIDCTRLO_NO_OSCILLATE)
   319     {
   320         //
   321         // Do not allow oscillation. If the error is within tolerance, stop.
   322         //
   323         if (abs(pidCtrl.prevError) <= pidCtrl.tolerance)
   324         {
   325             fOnTarget = true;
   326         }
   327     }
   328     else if (abs(pidCtrl.prevError) > pidCtrl.tolerance)
   329     {
   330         pidCtrl.startSettling = nPgmTime;
   331     }
   332     else if (nPgmTime - pidCtrl.startSettling >= pidCtrl.settlingTime)
   333     {
   334         //
   335         // We consider OnTarget only if the error is within tolerance for
   336         // at least the settling time.
   337         //
   338         fOnTarget = true;
   339     }
   340 
   341     TExitMsg(("=%x", fOnTarget));
   342     return fOnTarget;
   343 }   //PIDCtrlIsOnTarget
   344 
   345 /**
   346  *  This function calculates the output based on the current input.
   347  *
   348  *  @param pidCtrl Points to the PID structure.
   349  *  @param currInput Specifies the current input value.
   350  *
   351  *  @return Returns the calculate output value.
   352  */
   353 float
   354 PIDCtrlOutput(
   355     PIDCTRL &pidCtrl,
   356     float currInput
   357     )
   358 {
   359     float output;
   360     float error;
   361     float adjTotalError;
   362 
   363     TFuncName("PIDCtrlOutput");
   364     TLevel(API);
   365     TEnter();
   366 
   367     error = pidCtrl.setPoint - currInput;
   368     if (pidCtrl.options & PIDCTRLO_INVERSE)
   369     {
   370         error = -error;
   371     }
   372     adjTotalError = pidCtrl.Ki*(pidCtrl.totalError + error);
   373     if ((adjTotalError >= pidCtrl.minOutput) &&
   374         (adjTotalError <= pidCtrl.maxOutput))
   375     {
   376         pidCtrl.totalError += error;
   377     }
   378 
   379     output = pidCtrl.Kp*error +
   380              pidCtrl.Ki*pidCtrl.totalError +
   381              pidCtrl.Kd*(error - pidCtrl.prevError);
   382 
   383     if (pidCtrl.options & PIDCTRLO_INTEGRATE)
   384     {
   385         output += pidCtrl.prevOutput;
   386     }
   387 
   388     if (output < pidCtrl.minOutput)
   389     {
   390         output = pidCtrl.minOutput;
   391     }
   392     else if (output > pidCtrl.maxOutput)
   393     {
   394         output = pidCtrl.maxOutput;
   395     }
   396 
   397     pidCtrl.prevError = error;
   398     pidCtrl.prevOutput = output;
   399 
   400     TExitMsg(("=%f", output));
   401     return output;
   402 }   //PIDCtrlOutput
   403 
   404 #endif  //ifndef _PIDCTRL_H