mc_control.c

DSP实现三相异步电机矢量控制的C语言算法

/**
* @file mc_control.c
*
* Copyright (c) 2005 Atmel.
*
* @brief This module provide services to control speed for AT90PWM3 Only
* Type of control : PID means proportionnal, integral and derivative.
*
* @version 1.0 (CVS revision : $Revision: 1.2 $)
* @date $Date: 2005/11/16 17:18:43 $
* @author $Author: raubree $
*****************************************************************************/
#include "config.h"
#include "inavr.h"

/* Speed control variables */
S16 speed_error=0;      //!<Error calculation
S16 speed_integral = 0;
S16 speed_integ = 0;
S16 speed_proportional = 0;
//extern  U16 erreur ;

/**************************************************************************************/
/*                          Speed Control                                             */
/**************************************************************************************/
/**
* @brief  speed controller
* @return value of speed (duty cycle on 16 bits)
*         speed_measure has 10 bits resolution
*/
S16 mc_control_speed_16b(S16 speed_ref , S16 speed_measure)
{
  S16 Duty = 0;
  S16 increment = 0;

  // Error calculation
  speed_error = speed_ref - speed_measure ;

  // proportional term calculation : Kp= 7/64=0.1
  speed_proportional = ( speed_error/8 - speed_error/64 );

  // integral term calculation
  speed_integral = speed_integral + speed_error;

  // speed integral saturation
  if(speed_integral >  32000) speed_integral =  32000;
  if(speed_integral < -32000) speed_integral = -32000;

  // speed_integ = Ki_speed*speed_integral, with Ki_speed = 29/8192=3e-3
  speed_integ = (speed_integral - speed_integral/8 + speed_integral/32) / 256 ;

  // Duty Cycle calculation
  increment = speed_proportional + speed_integ;

  increment = (increment/2 + increment/4) ; // PI output normalization

  // saturation of the PI output
  if( increment > (S16)(0) ) {
    if  (increment <= (S16)(192)) Duty = (S16)increment ;
     else Duty = 192 ;
  }
  else {
    if  (increment < (S16)(-192)) Duty = -192 ;
     else Duty = (S16)increment ;
  }

  // return Duty Cycle
  return Duty;
}