towerexclusion.c

包括常规PID、NOTCH、LAG2 等函数的实现方法

#include "../data.h"

extern struct DATA data;

void TowerExclusion(int aciStatus, float rWg, float *rW1, float *rW2, float *rQopt, float rQdem0)
{
	static int iMode, iCount, iMaxCount[2];
	static float rSpdStep;
	
	if (aciStatus == 0)
	{
		iMaxCount[0] = (int)(data.TowerExcTime[0]/data.TorqueStep);
		iMaxCount[1] = (int)(data.TowerExcTime[1]/data.TorqueStep);
		rSpdStep = data.TowerExcSpdRate*data.TorqueStep;
		if (rWg > 0.5*(data.TowerExcSpeed[0]+data.TowerExcSpeed[1]))
		{
			iMode = 2;
			iCount = iMaxCount[1];
		}
		else
		{
			iMode = -2;
			iCount = iMaxCount[0];
		}
	}
	
	switch(iMode)
	{
		case -2:	//Operating below exclusion zone
			*rW1 = data.Wmin;
			*rW2 = data.TowerExcSpeed[0];
			if (rQdem0 < data.TowerExcTorque[0] || data.Wmax <= data.TowerExcSpeed[1]) iCount = iMaxCount[0];
			else iCount--;
			if (iCount == 0)
			{
				iMode = 1;	//Change mode
				if (!data.SmoothSwitch) *rW1 = *rW2;
				*rQopt = 0;	//Allow torque to drop, to accelerate rotor
			}
			break;
		case 2: 	//Operating above exclusion zone
			*rW1 = data.TowerExcSpeed[1];
			*rW2 = data.Wmax;
			if (rQdem0 > data.TowerExcTorque[1] || data.Wmin >= data.TowerExcSpeed[0]) iCount = iMaxCount[1];
			else iCount--;
			if (iCount == 0)
			{
				iMode = -1;	//Change mode
				if (data.SmoothSwitch)
				{
					*rQopt = data.SetTorque;	//Allow torque to rise to decelerate rotor
				}
				else
				{
					*rW2 = *rW1;
					*rQopt = 0;	//Probably unnecessary
				}
			}
			break;
		case -1:		//Ramping down through zone
			*rW1 -= rSpdStep;
			if (*rW1 <=  data.TowerExcSpeed[0])
			{
				//Got there
				iMode = -2;
				iCount = iMaxCount[0];
				if (data.SmoothSwitch)
				{
					data.iSpeedMode = 1;
					*rW1 = data.Wmin;
					*rW2 = data.TowerExcSpeed[0];
				}
				else
				{
					*rW1 =  data.TowerExcSpeed[0];
					*rW2 = *rW1;
					*rQopt = 0;
				}
			}
			else if (data.SmoothSwitch)
			{
				*rQopt = data.SetTorque;	//Allow torque to rise to decelerate rotor
			}
			else
			{
				*rQopt = 0;	//Probably unnecessary
				*rW2 = *rW1;
			}
			break;
		case 1:		//Ramping up through zone
			*rW2 += rSpdStep;
			if (*rW2 >=  data.TowerExcSpeed[1])
			{
				//Got there
				iMode = 2;
				iCount = iMaxCount[1];
				if (data.SmoothSwitch) 
				{
					data.iSpeedMode = 0;
					*rW1 = data.TowerExcSpeed[1];
					*rW2 = data.Wmax;
				}
				else
				{
					*rW2 =  data.TowerExcSpeed[1];
					*rQopt = 0;
				}
			}
			else *rQopt = 0;
			if (!data.SmoothSwitch) *rW1 = *rW2;
			break;
	}
	
}