fork

Dependencies:   mbed

Fork of LG by igor Apu

PLC_reg.c

Committer:
igor_v
Date:
2016-01-30
Revision:
1:f2adcae3d304
Parent:
0:8ad47e2b6f00
Child:
21:bc8c1cec3da6

File content as of revision 1:f2adcae3d304:

/****************************************Copyright (c)****************************************************
**--------------File Info---------------------------------------------------------------------------------
** File name:           PLC_reg.c
** Last modified Date:  2011-09-26
** Last Version:        V1.00
** Descriptions:        Routines for system of perimeter regulating unit
**
**--------------------------------------------------------------------------------------------------------
** Created by:          Electrooptica Incorp.
** Created date:        2011-09-26
** Version:             V1.00
** Descriptions:        
**
**--------------------------------------------------------------------------------------------------------       
*********************************************************************************************************/
#include "Global.h"



#define CONFIG_HFO_REG	//r. �������� ����������� �������� ������� ��� �� ������������ �� ����� ���������
#define WP_TRANSITION_ENA //
 
	//e.--- constants for the CPLC regulator ------------------------------------------------------- //r.--- ��������� ��� ������� ��� -------------------------------------------------------

#define  PLC_SHIFT				(6) 	
#define	 PLC_PHASE_DET_SHIFT	(18)	//e. 18 - for analog output //r. 18 - ��� ����������� 

#define  PLC_RESET_THRESHOLD 	(-3276) //e. correspond to the voltage +1.2 Volts //r. ������������� ���������� +1.2 ������
#define	 WP_REG32MAX_SATURATION (32767 << PLC_SHIFT)
#define	 WP_REG32MIN_NEW_SATURATION (PLC_RESET_THRESHOLD << PLC_SHIFT)
#define  WP_TMP_THRESHOLD		(7) //e. temperature threshold, defining heats up or cool down the device //r. ������������� �����, ������������ ����������� ��� ����������� ������


#define debugPLC

	   int WP_reg32;
	   int WP_Phase_Det; 		//e. output of the phase detector of the CPLC (in a digital kind)//r. ����� �������� ��������� ��� (� �������� ����)
	   int WP_reset_heating;	//e. voltage of reset at heating //r. ���������� ������ ��� ����������
	   int WP_reset_cooling;	//e. voltage of reset at cooling //r. ���������� ������ ��� ����������
	   int MaxDelayPLC;
 	   int sin_func[100];

int phase_Digital;

int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime); 

/******************************************************************************
** Function name:		init_PLC
**
** Descriptions: Initialization procedure for PLC regulator 		
**
** Parameters:		None		
** Returned value:	None	
** 
******************************************************************************/
void init_PLC(void)
{
	int i;
	                           //( 1,2 ������)
	if (Device_blk.Str.WP_reset < PLC_RESET_THRESHOLD) //e. ���������� ����� ������ �� ����������� �� ������ ��������� 1,2 ������. 
		//(�������� �������� ���������� ��� (����� ������)) < (-3276).
	{
		Device_blk.Str.WP_reset = PLC_RESET_THRESHOLD + 1;//(-3275)
	}
//���������� �� ��� =  (���. �������� �� ����������� + ���. �������� �� �����������)/2
	Output.Str.WP_reg = (Device_blk.Str.WP_rup + Device_blk.Str.WP_rdw) >> 1; //e. WP_reg start voltage is (WP_rup - WP_rdw)/2	

	//        ���������� �� ��� <<   6 
	WP_reg32 = Output.Str.WP_reg<<PLC_SHIFT;
		   
 	if ((Device_blk.Str.PI_b3>100)||(Device_blk.Str.PI_b3<10))	//e. ���� ��������� ������� ���������� ��� ������ 1kHz ��� ������ 100Hz
	      Device_blk.Str.PI_b3 = 40;			                			//e. ���������� ������� � 250Hz (������� �����������) 
  
  for (i = 0; i<Device_blk.Str.PI_b3; i++)			   //e. ������������ ��� �������
   {
     float temp = sin((float)i*2.0*PI/(float)Device_blk.Str.PI_b3); /// ���������� �������� ������
                                                                    /// ��� ������� ���������� ��� (PI_b3),
     sin_func[i] = (int)(temp*32767);                               /// � ���������� ���� �������� ��� ���.
      if (sin_func[i] < 0)
       sin_func[i] += 65536;  
    }
	 
      //e. calculation of filter coefficients for PLC		            
//                                250 Hz                                	10 KHz
		init_BandPass( 1.0/(float)Device_blk.Str.PI_b3, 10.0/(float)(DEVICE_SAMPLE_RATE_HZ), PLC);	//��������� ������ ��� ��������� ������� ��������� ����������
		                                                                                            //� ����������� ������������(aPLC[0-2] � bPLC[0-2]) 
		                                                                                            //(����������� ��� ��� ����������� ����� ��� ��������� �������� ������� ����������.)

	Device_blk.Str.WP_scl <<=  1; //e. during fist 10 seconds after start we state	Device_blk.Str.WP_scl = 2*Device_blk.Str.WP_scl
                                // ������ 10 ������ �������� � ������������ �������� * 2
		
	MaxDelayPLC = Device_blk.Str.PI_b3>>1;	//e. max expected delay for phase detector output
} // init_PLC
					
/******************************************************************************
** Function name:		PLC_MeanderDelay
**
** Descriptions: Outgoing of the delayed meander signal for the PLC regulator 		
**
** parameters:		Input value		
** Returned value:	Delayed value	
** 
******************************************************************************/
int PLC_MeanderDelay(int flag)
{
	static int poz_counter = 0, neg_counter = 0, flg_delay;

	if (Device_blk.Str.WP_ref == 0)
	{
		return (flag);
	}
	
	//e. check whether delay exceeds the greatest possible value //r. �������� �� ����������� �� �������� ����������� ���������
	if (Device_blk.Str.WP_ref > MaxDelayPLC) { Device_blk.Str.WP_ref = MaxDelayPLC; }
	
	if (flag) //e. outgoing poz_sin_flag flag, which delayed by the WP_ref //r. ������������ ������������ �� �������� WP_ref ����� poz_sin_flag
	{
  		neg_counter = 0;
  		poz_counter++;
	}
	else 
	{
  		poz_counter = 0;
  		neg_counter++;
	}
	if (poz_counter == Device_blk.Str.WP_ref) { flg_delay = 0; }
	if (neg_counter == Device_blk.Str.WP_ref) { flg_delay = 1; }
	return (flg_delay);
} 
/******************************************************************************
** Function name:		clc_PLC
**
** Descriptions: Procedure of initial processing for the CPLC regulator 		
**
** parameters:	None		
** Returned value:	None	
** 
******************************************************************************/
void clc_PLC(void)
{
	static int is_zeroing = 0;
	static int zero_delay = 0;
//	static int WP_DelaySin_Array[21] = {0};
// int phase_Digital;
	int poz_sin_flag;   	 	   
	int poz_sin_flag_delayed;

			
	static int plc_reset32;
	static enum 
	{		//r. ��������� ��������� �������� ��� ��������� ���
		FINISHED, 		  //r. �������� ������� ��������
	  TRANS_HEATING,  //r. ������� ����������� ��� ����������
		TRANS_COOLING	  //r. ������� ����������� ��� ����������
	} plc_transiton = FINISHED; 

//	int i;

	if (Output.Str.WP_sin >= 32768) 
		{
		  poz_sin_flag = 0;
	  } 
	  	else 
				   {
		         poz_sin_flag = 1;
           }

					 //r. ��������� ������ ��� ������� ���
	WP_Phase_Det = PLC_PhaseDetFilt(/*Output.Str.WP_sin*/Input.StrIn.WP_sel);

	
	if (WP_Phase_Det >0) 
		{ //r. WP_sel>0
  		phase_Digital = 1;
	  }
		else 
			  {
  		    phase_Digital = -1;
	      }
	// from this WP_Phase_Det - modulated signal like LIM_DIG
 
	poz_sin_flag_delayed = PLC_MeanderDelay(poz_sin_flag);

	if(poz_sin_flag_delayed)
	{
		WP_Phase_Det = -WP_Phase_Det; 
		phase_Digital = -phase_Digital;
	}  
	// from this WP_Phase_Det - demodulated signal like LIDEM_DIG
  
	if (!is_zeroing)  //r. �� ���� ��������� ���������
	{				//r. ��� ���������
			if ((WP_reg32 > (Device_blk.Str.WP_rup << PLC_SHIFT)) && IsHeating) //r. ���������� ����������
			{
	  			is_zeroing = 1;
				 //r. ���������� ������ ��� ����������
				WP_reset_heating = CPL_reset_calc(Device_blk.Str.WP_reset, Device_blk.Str.K_WP_rst_heating, Temp_Aver, Device_blk.Str.TemperNormal);
  			plc_transiton = TRANS_HEATING;
  			plc_reset32 = WP_reset_heating << PLC_SHIFT;;

				Device_blk.Str.HF_scl = Device_blk.Str.HF_scl_2; //r. �������� ����������� �������� ������� ��� �� ������������ �� ����� ���������	
	  	} 			
  			else if ((WP_reg32 < (Device_blk.Str.WP_rdw << PLC_SHIFT)) && !IsHeating)	//r. ����������
			{
	  		is_zeroing = 1;
				//r. ���������� ������ ��� ����������
				WP_reset_cooling = CPL_reset_calc(Device_blk.Str.WP_reset2, Device_blk.Str.K_WP_rst_cooling, Temp_Aver, Device_blk.Str.TemperNormal);

	  		plc_transiton = TRANS_COOLING;
	  		plc_reset32 = WP_reset_cooling << PLC_SHIFT;

  			Device_blk.Str.HF_scl = Device_blk.Str.HF_scl_2; //r. �������� ����������� �������� ������� ��� �� ������������ �� ����� ���������
			}
			else  //r. ������ �� ���������, ������� ������ �������
				WP_reg32 = L_mac(WP_reg32, phase_Digital, Device_blk.Str.WP_scl ); // WP_reg32 += phase_Digital * Device_blk.Str.WP_scl;

	}
	else  //r. ���� ���������� (1) - ����� ���������
	{ 

		if (plc_transiton != FINISHED)
		{
			if (plc_transiton == TRANS_HEATING)
			{
				
				WP_reg32 = L_sub(WP_reg32, Device_blk.Str.WP_transition_step); // WP_reg32 -= Device_blk.Str.WP_transition_step;
				if (WP_reg32 < plc_reset32)
				{
		  			zero_delay = 0;
		  			plc_transiton = FINISHED; //r.false;
	  				WP_reg32 = plc_reset32;
				}
			}
			else // plc_transiton == TRANS_COOLING
			{
				WP_reg32 = L_add(WP_reg32, Device_blk.Str.WP_transition_step); // WP_reg32 += Device_blk.Str.WP_transition_step;
				if (WP_reg32 > plc_reset32)
				{
		  			zero_delay = 0;
		  			plc_transiton = FINISHED; //r.false;
	  				WP_reg32 = plc_reset32;
				}
			}
		}
		else 

		if (zero_delay < Device_blk.Str.WP_mdy)
  		{
  			zero_delay++;
  		}
  		else //e. resetting was completed //r. ��������� �����������
  		{
  			is_zeroing = 0;
  			//e. save the temperature for further comparison //r. ���������� ����������� ��� ����������� ���������
		//	TempOfReset = Temp_Aver; //r.x. Temp5_Aver; //r. Tmp_Out[TSENS_NUMB]; // T4;
			//r.x	Zero_Numb_dbg++; // ��� ����� ������������ ����� ���������

		//	DithFreqRangeCalc(); //e. calculation of range of the division factor for the dither drive frequency, depending on current temperature //r. ������ ������ ������������ ������� ��� ������� ������������, ��������� �� ������� �����������
  		}
	}

		Saturation(WP_reg32, WP_REG32MAX_SATURATION, WP_REG32MIN_NEW_SATURATION); //e. the minimum corresponds to a small negative number, appropriate to PLC_RESET_THRESHOLD //r. ������� ������������� ���������� �������������� �����, �����-�� PLC_RESET_THRESHOLD

	
	if ( loop_is_closed(WP_REG_ON) )	//e. the regulator loop is closed //r. ������ �������
	{
		Output.Str.WP_reg = (int)(WP_reg32 >> PLC_SHIFT); //e. we use as controlling - voltages of the integrator //r. ���������� ��� ����������� - ���������� �����������

	}
	else 					//e. the regulator loop is open //r. ������ ���������
	{
		WP_reg32 = Output.Str.WP_reg << PLC_SHIFT; 	//e. set the previous value of the WP_reg  //r. ����������� ���������� �������� WP_reg 

	}

	//e. integartion of output of the PD of the CPLC regulator for the technological output on the Rate command //r. �������������� ������ �� ������� ��� ��� ���������������� ������ �� ������� Rate

	Output.Str.WP_pll = WP_PhaseDetectorRate( WP_Phase_Det, time_1_Sec); 
	
} // clc_PLC

/******************************************************************************
** Function name:		Signal_2_Oscill
**
** Descriptions: Procedure of analog worm output 		
**
** parameters:	Type of output		
** Returned value:	code to DAC	
** 
******************************************************************************/
int Signal_2_Oscill() //e. the signal for the control by scope on DAC output  (was DS) //r. ������ ��� �������� ������������� �� ������ ��� (������ ���)
{
	// Scope_Mode var not used now, reserved for future applications
	return (-WP_Phase_Det << 2);
} // Signal_2_Oscill

/******************************************************************************
** Function name:		clc_WP_sin
**
** Descriptions: Procedure of scan signal generating 		
**
** parameters:	None		
** Returned value:	Current code for scan signal DAC of PLC
** 
******************************************************************************/
int clc_WP_sin(void)
{
	static int index = 0;     
	index++;

	if (index >= 40/*Device_blk.Str.PI_b3*/)
		index = 0;
/*  if (index > 20) 
		LPC_GPIO0->FIOSET = (1<<26);
	else 
		LPC_GPIO0->FIOCLR = (1<<26);	*/
		DAC_Output(sin_func[index]); //output to DAC
		 
	return (sin_func[index]);
} // clc_WP_sin

/******************************************************************************
** Function name:		WP_PhaseDetectorRate
**
** Descriptions:     Integartion of output of the PD of the CPLC regulator 
                     for the technological output on the Rate command 		
**
** Parameters:	   Current PD magnitude, period of integration		
** Returned value:	Integrated magnitude of PD 	
** 
******************************************************************************/
int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) 
{

	static int SampleAndHoldOut = 0;
	static int WP_PhasDet_integr = 0;//, WP_PhasDetector = 0;
	
	if (IntegrateTime == DEVICE_SAMPLE_RATE_uks)
	{
		SampleAndHoldOut = (int)(WP_PhasDet_integr >> PLC_PHASE_DET_SHIFT);  
		WP_PhasDet_integr = 0;
	}
	else
	{	
		WP_PhasDet_integr += PhaseDetInput;
	}
	return (SampleAndHoldOut);
} // WP_PhaseDetectorRate