/******************** (C) COPYRIGHT 2008 STMicroelectronics ********************
* File Name          : MC_PID_regulators.c
* Author             : IMS Systems Lab 
* Date First Issued  : 21/11/07
* Description        : This file contains the software implementation for the
                       PI(D) regulators.
********************************************************************************
* History:
* 21/11/07 v1.0
* 29/05/08 v2.0
********************************************************************************
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* THIS SOURCE CODE IS PROTECTED BY A LICENSE.
* FOR MORE INFORMATION PLEASE CAREFULLY READ THE LICENSE AGREEMENT FILE LOCATED
* IN THE ROOT DIRECTORY OF THIS FIRMWARE PACKAGE.
*******************************************************************************/

/* Standard include ----------------------------------------------------------*/
#include "MC_PID_regulators.h"

PID_Struct_t 	PID_Flux_InitStructure; // d轴电流环闭环

PID_Struct_t 	PID_Torque_InitStructure; // q轴电流环闭环

PID_Struct_t 	PID_Weak_InitStructure; // 输出电压弱磁闭环

PID_Struct_t 	PID_IMax;	// 母线电流闭环
	
PID_Struct_t  PID_MotorSpd; // 电机速度闭环

PID_Struct_t 	PID_ConstantPower; // 恒定功率


/*******************************************************************************
* Function Name  : PID_Init
* Description    : Initialize PID coefficients for torque, flux and speed loop: 
                   Kp_Gain: proportional coeffcient 
                   Ki_Gain: integral coeffcient 
                   Kd_Gain: differential coeffcient 
* Input          : Pointer 1 to Torque PI structure,  
                   Pointer 2 to Flux PI structure,  
                   Pointer 3 to Speed PI structure  
* Output         : None
* Return         : None
*******************************************************************************/
void PID_Init (void)
{
	//q轴电流环
	PID_Torque_InitStructure.hKp_Gain    = 20000;
  PID_Torque_InitStructure.hKp_Divisor = 1024;
	PID_Torque_InitStructure.hKi_Gain = 3;
  PID_Torque_InitStructure.hKi_Divisor = 1024;
  PID_Torque_InitStructure.hKd_Gain = 0;
  PID_Torque_InitStructure.hKd_Divisor = 1024;
  PID_Torque_InitStructure.wPreviousError = 0;
  PID_Torque_InitStructure.hLower_Limit_Output = -32768;
  PID_Torque_InitStructure.hUpper_Limit_Output= 32767;
  PID_Torque_InitStructure.wLower_Limit_Integral = -32768 * 1024;
  PID_Torque_InitStructure.wUpper_Limit_Integral = 10000 * 1024;
  PID_Torque_InitStructure.wIntegral = 0;
	
	//d轴电流环
  PID_Flux_InitStructure.hKp_Gain    = 20000;
  PID_Flux_InitStructure.hKp_Divisor = 1024;  
  PID_Flux_InitStructure.hKi_Gain = 3;
  PID_Flux_InitStructure.hKi_Divisor = 1024;
  PID_Flux_InitStructure.hKd_Gain = 0;
  PID_Flux_InitStructure.hKd_Divisor = 1024;
  PID_Flux_InitStructure.wPreviousError = 0;
  PID_Flux_InitStructure.hLower_Limit_Output= -32768;
  PID_Flux_InitStructure.hUpper_Limit_Output= 32767;
  PID_Flux_InitStructure.wLower_Limit_Integral = -32768 * 1024;
  PID_Flux_InitStructure.wUpper_Limit_Integral = 32767 * 1024;
  PID_Flux_InitStructure.wIntegral = 0;
  
	//输出电压弱磁闭环
	PID_Weak_InitStructure.hKp_Gain    = 64;
  PID_Weak_InitStructure.hKp_Divisor = 1024;
	PID_Weak_InitStructure.hKi_Gain = 3;
  PID_Weak_InitStructure.hKi_Divisor = 1024;
  PID_Weak_InitStructure.hKd_Gain = 0;
  PID_Weak_InitStructure.hKd_Divisor = 1024;
  PID_Weak_InitStructure.wPreviousError = 0;
  PID_Weak_InitStructure.hLower_Limit_Output= -262144;
  PID_Weak_InitStructure.hUpper_Limit_Output= 0;
  PID_Weak_InitStructure.wLower_Limit_Integral = -262144 * 1024;
  PID_Weak_InitStructure.wUpper_Limit_Integral = 0;
  PID_Weak_InitStructure.wIntegral = 0;
	
	//输出功率环(目前用于推行模式母线电流环)
	PID_ConstantPower.hKp_Gain    = 30000;
  PID_ConstantPower.hKp_Divisor = 256;
	PID_ConstantPower.hKi_Gain = 300;
  PID_ConstantPower.hKi_Divisor = 1024;
  PID_ConstantPower.hKd_Gain = 0;
  PID_ConstantPower.hKd_Divisor = 1024;
  PID_ConstantPower.wPreviousError = 0;
  PID_ConstantPower.hLower_Limit_Output= -2100;
  PID_ConstantPower.hUpper_Limit_Output= 0;
  PID_ConstantPower.wLower_Limit_Integral = -2150400;
  PID_ConstantPower.wUpper_Limit_Integral = 0;
  PID_ConstantPower.wIntegral = 0;

	//电机速度环
	PID_MotorSpd.hKp_Gain    = 4096;
  PID_MotorSpd.hKp_Divisor = 1024;
	PID_MotorSpd.hKi_Gain = 1;
  PID_MotorSpd.hKi_Divisor = 1024;
  PID_MotorSpd.hKd_Gain = 0;
  PID_MotorSpd.hKd_Divisor = 1024;
  PID_MotorSpd.wPreviousError = 0;
  PID_MotorSpd.hLower_Limit_Output= -1050;
  PID_MotorSpd.hUpper_Limit_Output= 1050;
  PID_MotorSpd.wLower_Limit_Integral = -358400;
  PID_MotorSpd.wUpper_Limit_Integral = 358400;
  PID_MotorSpd.wIntegral = 0;

  //母线电流环(目前用于力矩模式母线电流环)
  PID_IMax.hKp_Gain    = 30000;
  PID_IMax.hKp_Divisor = 256;
	PID_IMax.hKi_Gain = 300;
  PID_IMax.hKi_Divisor = 1024;
  PID_IMax.hKd_Gain = 0;
  PID_IMax.hKd_Divisor = 1024;
  PID_IMax.wPreviousError = 0;
  PID_IMax.hLower_Limit_Output= -1050;
  PID_IMax.hUpper_Limit_Output= 0;
  PID_IMax.wLower_Limit_Integral = -1075200;
  PID_IMax.wUpper_Limit_Integral = 0;
  PID_IMax.wIntegral = 0;	
}


/*******************************************************************************
* Function Name  : PID_Regulator
* Description    : Compute the PI(D) output for a PI(D) regulation.
* Input          : Pointer to the PID settings (*PID_Flux)
                   Speed in int16_t format
* Output         : int16_t
* Return         : None
*******************************************************************************/

/******************************PID 调节函数的说明******************************/
int16_t PID_Regulator(int16_t hReference, int16_t hPresentFeedback,PID_Struct_t *PID_Struct)
{
  int16_t wError;
	int32_t	wProportional_Term,wIntegral_Term, houtput_32;
  int32_t dwAux; 
 
  // error computation
	wError= hReference - hPresentFeedback;		  //取得需要误差量	delta_e  <Q16

  // Proportional term computation
  wProportional_Term = PID_Struct->hKp_Gain * wError;	 // wP = Kp * delta_e  <Q32
														 // wP 为比例总调节量
  // Integral term computation
	if(PID_Struct->hKi_Gain == 0)
	{
		PID_Struct->wIntegral = 0;
	}
	else
	{ 
		wIntegral_Term = PID_Struct->hKi_Gain * wError;		   // wI = Ki * delta_e	，本次积分项
		dwAux = PID_Struct->wIntegral + wIntegral_Term;	// 积分累积的调节量 = 以前的积分累积量 + 本次的积分项

		if(dwAux > PID_Struct->wUpper_Limit_Integral)		   //dwAux为当前积分累积项，下面测试积分饱和度
		{
			PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral;	// 超上限
		}
		else if(dwAux < PID_Struct->wLower_Limit_Integral)				//超下限
		{ 
			PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral;
		}
		else
		{
			PID_Struct->wIntegral = dwAux;		  //不超限, 更新积分累积项为dwAux  <Q32
		}
		
	}
  // Differential term computation
	houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor) + 	//不含微分环节的总调节量  <Q16
								(PID_Struct->wIntegral/PID_Struct->hKi_Divisor);  

	if((houtput_32<0)&&(houtput_32<PID_Struct->hLower_Limit_Output)) //超下限
	{
		return(PID_Struct->hLower_Limit_Output);
	}
	if((houtput_32>0)&&(houtput_32>=PID_Struct->hUpper_Limit_Output))	   //测试输出是否饱和，超上限
	{
		return(PID_Struct->hUpper_Limit_Output);		  			 	
	}
	else 
	{
		return houtput_32;  //不超限。输出结果 houtput_32  <Q16
	}
}

/******************** (C) COPYRIGHT 2008 STMicroelectronics *******************/