Index: PIControllers.c =================================================================== diff -u -rf352febbad947e7ae4afbd9a5eaa87e434648f37 -r8306f93dda4cd495f2738212398f2c5076935165 --- PIControllers.c (.../PIControllers.c) (revision f352febbad947e7ae4afbd9a5eaa87e434648f37) +++ PIControllers.c (.../PIControllers.c) (revision 8306f93dda4cd495f2738212398f2c5076935165) @@ -55,22 +55,24 @@ F32 controlSignal; ///< Actual control signal. F32 maxErrorSumStep; ///< Maximum change in I (error sum) for a single control interval. PI_CONTROLLER_DIRECTIONS_T direction; ///< PI controller control direction. + BOOL isFeedForwardEnabled; ///< Eanble or disable Feed forward term + F32 feedForward; ///< Feed forward term } PI_CONTROLLER_T; // ********** private data ********** /// PI Controllers - initial configurations. static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] = -{ // Kp Ki uMax uMin ref meas err esw esum ctrl Ilimit controller type +{ // Kp Ki uMax uMin ref meas err esw esum ctrl Ilimit controller type, feedfwdEnabled, feedfwdValue #ifdef _DD_ - { 0.0, 0.0, 2700, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D12_PUMP - { 0.0, 0.0, 2700, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D48_PUMP - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D5_HEAT - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_D45_HEAT + { 0.0, 0.0, 2650, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D12_PUMP + { 0.0, 0.0, 2650, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D48_PUMP + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 15.0, CONTROLLER_BIDIRECTIONAL, TRUE, 0.0 }, // PI_CONTROLLER_ID_D5_HEAT + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D45_HEAT #endif #ifdef _RO_ - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP_MAX_PRES + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_RO_PUMP + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_RO_PUMP_MAX_PRES #endif }; @@ -88,10 +90,13 @@ * @param kI Coefficient for integral * @param controlMin Minimum control output * @param controlMax Maximum control output + * @param isFeedForwardEnabled Feed forward enabled if true, otherwise not. + * @param feedFowardSignal feedforward value to be applied to the control output * @return none *************************************************************************/ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, - F32 kP, F32 kI, F32 controlMin, F32 controlMax ) + F32 kP, F32 kI, F32 controlMin, F32 controlMax, + BOOL isFeedForwardEnabled, F32 feedFowardSignal ) { if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { @@ -110,7 +115,8 @@ } controller->uMin = controlMin; controller->uMax = controlMax; - resetPIController( controllerID, initialControlSignal ); + controller->isFeedForwardEnabled = isFeedForwardEnabled; + resetPIController( controllerID, initialControlSignal, feedFowardSignal ); } else { @@ -133,9 +139,10 @@ * Id is passed. * @param controllerID ID filter number * @param initialControlSignal Value of the output on the first iteration + * @param feedFowardSignal Value of feed froward calculation if exists. * @return none *************************************************************************/ -void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal ) +void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 feedFowardSignal ) { PI_CONTROLLER_T *controller; @@ -148,6 +155,7 @@ controller->errorSum = controller->controlSignal / controller->Ki; controller->errorSumBeforeWindUp = controller->errorSum; controller->measuredSignal = 0.0; + controller->feedForward = feedFowardSignal; } else { @@ -218,17 +226,33 @@ // Calculate control signal from i term controller->errorSumBeforeWindUp = controller->errorSum; controlSignalBeforeWindup = ( controller->Ki * controller->errorSum ); - controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + if ( FALSE == controller->isFeedForwardEnabled ) + { + controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + } + else + { + controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, ( controller->uMax - controller->feedForward ) ); + } + // Handle anti-windup for i term windupError = controlSignalBeforeWindup - controller->controlSignal; if ( fabs( windupError ) > NEARLY_ZERO ) { controller->errorSum -= ( windupError / controller->Ki ); } - // Add p term to control signal and re-apply range limits + // Add p term to control signal controller->controlSignal += ( controller->Kp * controller->errorSignal ); + + // Add feed forward term to control signal + if ( FALSE != controller->isFeedForwardEnabled ) + { + controller->controlSignal = controller->controlSignal + controller->feedForward; + } + + //Re-apply range limits controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax ); result = controller->controlSignal; @@ -298,6 +322,10 @@ output = controller->Ki * controller->errorSum; break; + case CONTROLLER_SIGNAL_FEEDFORWARD_OUTPUT: + output = controller->feedForward; + break; + case CONTROLLER_SIGNAL_CONTROL: output = controller->controlSignal; break;