pid.c

用ＴＭＳ３２０ＬＦ２４０７Ａ开发电机控制的源代码

/************************************************************************
Project name: 	motorTest.pjt
File name: 		pid.c
Author: 		武洪恩，胡天亮. 山东大学数控技术研究中心
				Hong'en Wu. Tianliang Hu, CNC Research Center, Shandong University
Description:	Pid function
History:		(1)
*************************************************************************/

//include header files
#include "f2407_c.h"
#include "pid.h"
#include "motor.h"

//extern Variable definition
extern int motorAPos;

extern long motorBPos;
extern int  QepBCycle;

extern long motorCPos;
extern long motorDPos;
extern double motorCmd[4];


//Variable definition

//Variable for test
long MaxCode = 0;

///////////////////////////////
//PID Structure for every Axis
PIDDataStruct pidDataAxisA;
PIDDataStruct pidDataAxisB;
PIDDataStruct pidDataAxisC;
PIDDataStruct pidDataAxisD;

double MaxSpeed = 3.0;		//Speed limit
int motorCtrlData[4] = {0, 0, 0, 0};			//spi data to transfer


/*--------------------------------------
Name: 			motorA_PID_Init()
Description:	PID parameter initialization for Motor A
Input par:		none
Output par:		none
---------------------------------------*/
void inline motorA_PID_Init()
{	
	pidDataAxisA.Signals.uc  = motorAPos/10000.0;
	pidDataAxisA.States.deta_u = 0;
	pidDataAxisA.States.u_1 = 0;
	pidDataAxisA.States.u_2 = 0;
	pidDataAxisA.States.y_1 = 0;
	pidDataAxisA.States.y_2 = 0;
	pidDataAxisA.States.kp = 6.0;
	pidDataAxisA.States.ki = 0.00015;
	pidDataAxisA.States.kd = 0.45;
	pidDataAxisA.States.error_1 = 0;
	pidDataAxisA.States.error_2 = 0;
	pidDataAxisA.Par.x1 = 0;
	pidDataAxisA.Par.x2 = 0;
	pidDataAxisA.Par.x3 = 0;
}

/*--------------------------------------
Name: 			motorB_PID_Init()
Description:	PID parameter initialization for Motor B
Input par:		none
Output par:		none
---------------------------------------*/
void inline motorB_PID_Init()
{	
	pidDataAxisB.Signals.uc  = motorBPos/10000.0;
	pidDataAxisB.States.deta_u = 0;
	pidDataAxisB.States.u_1 = 0;
	pidDataAxisB.States.u_2 = 0;
	pidDataAxisB.States.y_1 = 0;
	pidDataAxisB.States.y_2 = 0;
	pidDataAxisB.States.kp = 6.0;
	pidDataAxisB.States.ki = 0.00015;
	pidDataAxisB.States.kd = 0.45;
	pidDataAxisB.States.error_1 = 0;
	pidDataAxisB.States.error_2 = 0;
	pidDataAxisB.Par.x1 = 0;
	pidDataAxisB.Par.x2 = 0;
	pidDataAxisB.Par.x3 = 0;
}

/*--------------------------------------
Name: 			motorC_PID_Init()
Description:	PID parameter initialization for Motor C
Input par:		none
Output par:		none
---------------------------------------*/
void inline motorC_PID_Init()
{	
	pidDataAxisC.Signals.uc  = motorCPos/10000.0;
	pidDataAxisC.States.deta_u = 0;
	pidDataAxisC.States.u_1 = 0;
	pidDataAxisC.States.u_2 = 0;
	pidDataAxisC.States.y_1 = 0;
	pidDataAxisC.States.y_2 = 0;
	pidDataAxisC.States.kp = 6.0;
	pidDataAxisC.States.ki = 0.00015;
	pidDataAxisC.States.kd = 0.45;
	pidDataAxisC.States.error_1 = 0;
	pidDataAxisC.States.error_2 = 0;
	pidDataAxisC.Par.x1 = 0;
	pidDataAxisC.Par.x2 = 0;
	pidDataAxisC.Par.x3 = 0;
}

/*--------------------------------------
Name: 			motorC_PID_Init()
Description:	PID parameter initialization for Motor D
Input par:		none
Output par:		none
---------------------------------------*/
void inline motorD_PID_Init()
{	
	pidDataAxisD.Signals.uc  = motorDPos/10000.0;
	pidDataAxisD.States.deta_u = 0;
	pidDataAxisD.States.u_1 = 0;
	pidDataAxisD.States.u_2 = 0;
	pidDataAxisD.States.y_1 = 0;
	pidDataAxisD.States.y_2 = 0;
	pidDataAxisD.States.kp = 6.0;
	pidDataAxisD.States.ki = 0.00015;
	pidDataAxisD.States.kd = 0.45;
	pidDataAxisD.States.error_1 = 0;
	pidDataAxisD.States.error_2 = 0;
	pidDataAxisD.Par.x1 = 0;
	pidDataAxisD.Par.x2 = 0;
	pidDataAxisD.Par.x3 = 0;
}


/*---------------------------------------------------------------------------
Name: 			PID_Calculate(int motorID)
Description:	PID caclulating, result is saving in global variable pidValue
Input par:		motorID, to select motor
Output par:		none
---------------------------------------------------------------------------*/
void PID_Calculate(int motorID)
{
	PIDDataStruct* pPidStruct;
	int voltAjust = 0;
	switch(motorID)
	{
		//Serve on signal of motor A and B is logistic inverted in CPLD 
		case MOTORA_ID:
			pPidStruct = (&pidDataAxisA);
			pPidStruct->Signals.y  = motorAPos/10000.0;
			pPidStruct->Signals.uc += motorCmd[0];
			voltAjust = DA_ZERO_OFFSETA;
			break;

		case MOTORB_ID:
			pPidStruct = (&pidDataAxisB);
			pPidStruct->Signals.y = motorBPos/10000.0;
			pPidStruct->Signals.uc += motorCmd[1];
			voltAjust = DA_ZERO_OFFSETB;
			break;

		case MOTORC_ID:
			pPidStruct = (&pidDataAxisC);
			pPidStruct->Signals.y = motorCPos/10000.0;
			pPidStruct->Signals.uc += motorCmd[2];
			voltAjust = DA_ZERO_OFFSETC;
			break;

		case MOTORD_ID:
			pPidStruct = (&pidDataAxisD);
			pPidStruct->Signals.y = motorDPos/10000.0;
			pPidStruct->Signals.uc += motorCmd[3];
			voltAjust = DA_ZERO_OFFSETD;
			break;

		default:
			disable();
			while(1);
	}

	//For test only
	if(MaxCode<motorAPos)
	{
		MaxCode = motorAPos;
	}

	pPidStruct->States.error = pPidStruct->Signals.uc - pPidStruct->Signals.y;
	pPidStruct->Par.x1 = pPidStruct->States.error;

	if(fabs(pPidStruct->States.error)<0.005)
	{
		pPidStruct->Par.x2 += pPidStruct->States.error;	
	}

	pPidStruct->Par.x3 = pPidStruct->States.error - pPidStruct->States.error_1;
	

	pPidStruct->States.error_1 = pPidStruct->States.error;

	//控制器输出增量值
	pPidStruct->States.deta_u = 
		pPidStruct->States.kp * pPidStruct->Par.x1 + 
		pPidStruct->States.ki * pPidStruct->Par.x2 + pPidStruct->States.kd * pPidStruct->Par.x3;
	//计算控制器输出
	pPidStruct->States.u = pPidStruct->States.deta_u;
	//判断实际控制器输出
	if(pPidStruct->States.u > MaxSpeed)
	{
		pPidStruct->States.u = MaxSpeed;
	}
	else if(pPidStruct->States.u < -MaxSpeed)
	{
		pPidStruct->States.u = -MaxSpeed;
	}
	else
	{
		pPidStruct->States.u = pPidStruct->States.u;
	}
	//变换电压(-19943/4)
	motorCtrlData[motorID] = -(pPidStruct->States.u) * 4860 + voltAjust;
}