controller.c

MC56F802BLDC 可以使用的算法 就是电机启动有点慢

/****************************************************************************************************
*
*  Motorola Inc.
*  (c) Copyright 2000 Motorola Inc.
*  ALL RIGHTS RESERVED.
*
*****************************************************************************************************
*
*
*  FILE NAME: controller.c
*  
*  DESCRIPTION: This source file contains the modules that calculate the standard PI and PID controllers.
*
*  MODULES INCLUDED: dspfunc.h
*                    mc.h
*                    controller.h
*                    prototype.h
*
*****************************************************************************************************/

//#include "prototype.h"
//#include "dspfunc.h"
//#include "mc.h"
//#include "controller.h"

#include "types.h"
#include "mc.h"
#include "controller.h"




/****************************************************************************************************
*
*  MODULE: controllerPIDtype1()
*
*  DESCRIPTION: This module calculates the PID algorithm according to the equation in the SDK documentation.
*
*  RETURNS: The function returns the controller output that is limited by PositivePIDLimit and 
*           NegativePIDLimit values.                     
*
*  ARGUMENTS: The function has three arguments: in     - DesiredValue  - 16bit range
*                                               in     - MeasuredValue - 16bit range
*                                               in/out - pointer pParams to mc_tPIDparams structure that
*                                                        contains the controller parameters, the input 
*                                                        error in step k-1 and the integral portion 
*                                                        in step k-1.
*
*  RANGE ISSUES: The function returns 16bit signed value. NegativePIDLimit <= controller output <= PositivePIDLimit
*                The controller parameters (gains) - 16bit positive signed values.
*                The scale values: 0 <= scale values <= 30. 
*
*  SPECIAL ISSUES: The function calculates correct results if the saturaion mode is set or not .
*
*  TEST METHOD: The testing file is debug_mcauxControllerPIDtype1.c 
*
****************************************************************************************************/

Word16 controllerPIDtype1(Word16 DesiredValue, Word16 MeasuredValue, mc_sPIDparams *pParams)
{
   Word32 ProportionalPortion, IntegralPortion, PIDoutput;
   Word16 InputError;

/*-------------------------------------------------------------------------------------------------*/
/* Saturation mode must be set */
/*   InputError = sub(DesiredValue, MeasuredValue); */  /* input error */ 
/*-------------------------------------------------------------------------------------------------*/
/* input error calculation - 16bit range, with and without saturation mode  */

   PIDoutput = L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue)); /* input error - 32bit range */
   if(PIDoutput > MAX_16)   /* inpur error is greater than 0x00007fff = 32767  - 32bit range */
       InputError = MAX_16;     /* input error = max. positive 16 bit signed value */
   else if(PIDoutput < MIN_16)  /* input error is less than 0xffff7fff = -32768    - 32bit range */
       InputError = MIN_16;        /* input error = min. negative 16 bit signed value */
   else
       InputError = extract_l(PIDoutput);  /* input error - 16bit range */
/*-------------------------------------------------------------------------------------------------*/
/* proportional portion calculation */
   
   ProportionalPortion = L_mult((pParams -> ProportionalGain), InputError) >> (pParams -> ProportionalGainScale + 1);
/*-------------------------------------------------------------------------------------------------*/
/* integral portion calculation */ 

   IntegralPortion = L_mult((pParams -> IntegralGain), InputError) >> (pParams -> IntegralGainScale + 1);
 /* integral portion in step k + integral portion in step k-1 */
   IntegralPortion = L_add(IntegralPortion, L_deposit_l(pParams -> IntegralPortionK_1));   

   /* integral portion limitation */
   if(IntegralPortion > pParams -> PositivePIDLimit)
      pParams -> IntegralPortionK_1 = pParams -> PositivePIDLimit;
   else if(IntegralPortion < pParams -> NegativePIDLimit)
      pParams -> IntegralPortionK_1 =  pParams -> NegativePIDLimit;
   else
      pParams -> IntegralPortionK_1 =  extract_l(IntegralPortion);
/*-------------------------------------------------------------------------------------------------*/
/* derivative portion calculation */      
 
   PIDoutput = L_sub(L_deposit_l(InputError),L_deposit_l(pParams -> InputErrorK_1)); /* [e(k) - e(k-1)] - 32bit range */
   pParams -> InputErrorK_1 = InputError;                 /* e(k-1) = e(k) */
   if(PIDoutput > MAX_16)       /* [e(k) - e(k-1)] is greater than 0x00007fff = 32767  - 32bit range */
       InputError = MAX_16;         /* [e(k) - e(k-1)] = max. positive 16 bit signed value - 16 bit range */
   else if(PIDoutput < MIN_16)  /* [e(k) - e(k-1)] is less than 0xffff7fff = -32768    - 32bit range */
       InputError = MIN_16;         /* [e(k) - e(k-1)] = min. negative 16 bit signed value - 16 bit range */
   else
       InputError = extract_l(PIDoutput);  /* [e(k) - e(k-1)]    - 16bit range */

   /* drivative portion in step k - integer */
   PIDoutput = L_mult((pParams -> DerivativeGain), InputError) >> (pParams -> DerivativeGainScale + 1);

/*-------------------------------------------------------------------------------------------------*/
/* controller output calculation */   

   PIDoutput = L_add(PIDoutput, ProportionalPortion); /* derivative portion + proportional portion */
   PIDoutput = L_add(PIDoutput, L_deposit_l(pParams -> IntegralPortionK_1)); /* + integral portion = controller output */
   
   /* controller output limitation */
   if(PIDoutput > pParams -> PositivePIDLimit)
      PIDoutput = pParams -> PositivePIDLimit;
   else if(PIDoutput < pParams -> NegativePIDLimit)
      PIDoutput =  pParams -> NegativePIDLimit;
/*-------------------------------------------------------------------------------------------------*/

      
   return (extract_l(PIDoutput));  /* controller output with limitation */
}






/****************************************************************************************************
*
*  MODULE: controllerPItype1()
*
*  DESCRIPTION: This module calculates the PI algorithm according to the equation in the SDK documentation.
*
*  RETURNS: The function returns the controller output that is limited by PositivePILimit and 
*           NegativePILimit values.                     
*
*  ARGUMENTS: The function has three arguments: in     - DesiredValue  - 16bit range
*                                               in     - MeasuredValue - 16bit range
*                                               in/out - pointer pParams to mc_tPIparams structure that
*                                                        contains the controller parameters 
*                                                        and the integral portion in step k-1.
*                                                        
*
*  RANGE ISSUES: The function returns 16bit signed value. NegativePILimit <= controller output <= PositivePILimit
*                The controller parameters (gains) - 16bit positive signed values.
*                The scale values: 0 <= scale values <= 30. 
*
*  SPECIAL ISSUES: The function calculates correct results if the saturaion mode is set or not .
*
*  TEST METHOD: The testing file is debug_mcauxControllerPItype1.c
*
****************************************************************************************************/

Word16 controllerPItype1(Word16 DesiredValue, Word16 MeasuredValue, mc_sPIparams *pParams)
{
   Word32 ProportionalPortion, IntegralPortion, PIoutput;
   Word16 InputError;

/*-------------------------------------------------------------------------------------------------*/
/* Saturation mode must be set */
/*   InputError = sub(DesiredValue, MeasuredValue); */  /* input error */ 
/*-------------------------------------------------------------------------------------------------*/
/* input error calculation - 16bit range, with and without saturation mode  */

   PIoutput = L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue)); /* input error - 32bit range */
   if(PIoutput > MAX_16)   /* inpur error is greater than 0x00007fff = 32767  - 32bit range */
       InputError = MAX_16;     /* input error = max. positive 16 bit signed value */
   else if(PIoutput < MIN_16)  /* input error is less than 0xffff7fff = -32768    - 32bit range */
       InputError = MIN_16;        /* input error = min. negative 16 bit signed value */
   else
       InputError = extract_l(PIoutput);  /* input error - 16bit range */
/*-------------------------------------------------------------------------------------------------*/
/* proportional portion calculation */
   
   ProportionalPortion = L_mult((pParams -> ProportionalGain), InputError) >> (pParams -> ProportionalGainScale + 1);
/*-------------------------------------------------------------------------------------------------*/
/* integral portion calculation */ 

   IntegralPortion = L_mult((pParams -> IntegralGain), InputError) >> (pParams -> IntegralGainScale + 1);
 /* integral portion in step k + integral portion in step k-1 */
   IntegralPortion = L_add(IntegralPortion, L_deposit_l(pParams -> IntegralPortionK_1));   

   /* integral portion limitation */
   if(IntegralPortion > pParams -> PositivePILimit)
      pParams -> IntegralPortionK_1 = pParams -> PositivePILimit;
   else if(IntegralPortion < pParams -> NegativePILimit)
      pParams -> IntegralPortionK_1 =  pParams -> NegativePILimit;
   else
      pParams -> IntegralPortionK_1 =  extract_l(IntegralPortion);
/*-------------------------------------------------------------------------------------------------*/
/* controller output calculation */   

   /* proportional portion + integral portion */
   PIoutput = L_add(ProportionalPortion, L_deposit_l(pParams -> IntegralPortionK_1)); /* controller output */

   /* controller output limitation */
   if(PIoutput > pParams -> PositivePILimit)
      PIoutput = pParams -> PositivePILimit;
   else if(PIoutput < pParams -> NegativePILimit)
      PIoutput =  pParams -> NegativePILimit;

      
   return (extract_l(PIoutput));  /* controller output with limitation */
}