pcm_code.c

PCM

	
	/**********************************************************************************
	* FILE : PTMV2.C *
	* DESCRIPTION *
	* Revision 2.0.0
	*
	* Main program to control 4 plate polarisation transformer using TMS320VC6711 DSP
	* implementing endless control, in 0, 45, -45, 0 configuration.
	* 
	* A/D is THS1206, D/A is TLV5639I
	* CREATED 2001*
		
	*******************************************************************************/
	#include	"math.h"   
    #include    "paramter.h" 
    #include	"Variable.h"
  	/***************************  MAIN     *****************************************/
	int test=0;
	void main(void)
	{
	
	     		 	   
	Ini() ;
	
	Recovery();

	 while(1)   
	{		
	    vout=Read_Vout(0) ;	
	 
	 	
	 
	 	asm(" NOP");
	
	 	Wei(&th1,&V1,1);
	   	Wei(&th2p,&V2p,2);
	  	Wei(&sig1,&V3,1);
	    Wei(&th2m,&V2m,2);
	
	  /*	 test++;
		 if (test > 0)
		 {
		    test=0;
		     
	    }
      */  
	}
}	  

/************************************************/	
void Wei (volatile  float *ptth,volatile  float *ptv,volatile int PtDAng )
{
 volatile  float arg1;
  volatile  int jj;
 Ptth=ptth;
 PtV=ptv;

  Control_Period =Control_Period_PS ; 
/********************************/ 
    if( (th1) > 1.4*pi )
 	{
 	asm(" NOP");
 	Rewind();
 //	Rewind();
 //	Rewind();
 
 	asm(" NOP");
 	}
   
   if( (sig1) > 1.4*pi )
 	{
 	asm(" NOP");
 	Rewind();
 //	Rewind();
 //	Rewind();
 
 	asm(" NOP");
 	}
 	 if( (th2p) > 1.9*pi )
 	{
 	asm(" NOP");
 	Rewind();
 //	Rewind();
 //	Rewind();
 
 	asm(" NOP");
 	}
 	 if( (th2m) > 1.9*pi )
 	{
 	asm(" NOP");
 	Rewind();
 //	Rewind();
 //	Rewind();
 
 	asm(" NOP");
 	}
 /*  if( th2m >=1.9*pi)
   th2m=0;
  
 if( th2p >=1.9*pi)
   th2p=0;
*/
/*
if( th1 >=1.9*pi)
   th1=0;
   if( sig1 >=1.9*pi)
   sig1=0;
*/	 

/**************************************/
  for(jj=0;jj<100;jj++);
   vout=Read_Vout(0) ;			/* Read_Vout(0) Do not check the timer  */
	vout_old=vout;
	
	
    if(vout > Omega1)
		   	{
		   	flag=0;
		   	}
 switch (flag)
 {
 case 0: asm(" NOP");
 		break;	
 case 1:
 		 
		 flag=1;
		 if(vout < Omega3)
			 {
	//	 Recovery();
			 }
			 
		 if(vout > Omega1)
		   	{
		   	break;
		   	}
		  
		  if(PtDAng==1)
		  arg1=DAng1;
		  else
		  arg1=DAng2;
			 
			 Perturbation(Ptth,arg1);
    		 Write_Output(V1,V2m,V2p,V3);
			 vout=Read_Vout(1) ;
			if(vout > Omega1)
		   	{
		   	break;
		   	}
			if( (vout - vout_old)>=  VoutDiff)
			      vout_old=vout;
			else
					{
					if(PtDAng==1)
					  arg1=DAng1;
					  else
					  arg1=DAng2;
			   		vout_old=vout;
			   		direction=-direction;
			   		Perturbation(Ptth,arg1);
					Write_Output(V1,V2m,V2p,V3);
					vout=Read_Vout(1);
					vout_old=vout;
				    }
			  
			 
			 if(vout > Omega1)
		   			{
		   				break;
		   			}
			    	   	
			flag=1;
			
			while(flag>0)
				{
				  
					    if(PtDAng==1)
						  arg1=DAng1;
						  else
						  arg1=DAng2;
					   Perturbation(Ptth,arg1);
					 	Write_Output(V1,V2m,V2p,V3);
						vout=Read_Vout(1);
						 if(vout > Omega1)
		   					{
		   						break;
		   					}
						if( (vout - vout_old)<=  VoutDiff )
							{
							flag=-1;
		     				direction=-direction;
		 			        
							     
								  if(PtDAng==1)
								   arg1=DAng1;
								   else
								   arg1=DAng2;
									Perturbation(Ptth,arg1);
									Write_Output(V1,V2m,V2p,V3);
									vout=Read_Vout(1);
					 	       
							} 
					vout_old=vout;	
					if( (*Ptth >=tpi))
					 	{
					 	flag=-1;
					 	}
						
				}

	}
	flag=1;	  
}
/*********************** Perturbation  *************************/	
 	void Perturbation(volatile  float *ptth,volatile  float Angle_change  )
 	{ 
 	    volatile int  Angle_index=0;
 	    Ptth=ptth;
 	   if(Ptth== &th1)
 	    {
 	    Angle_index=1;
 	    PtV=&V1;
 	    }
 	    if(Ptth== &th2p)
 	    {
 	    Angle_index=2;
 	     PtV=&V2p;
 	    }
 	    if(Ptth== &th2m)
 	    {
 	    Angle_index=3;
 	    PtV=&V2m;
 	    }
 	    if(Ptth== &sig1)
 	    {
 	    Angle_index=4;
 	    PtV=&V3;
 	    }
 
 	  switch(Angle_index)
 	 {       
	case 1:	     //  Change in th1
		   	PtV=&V1;
		   	Ptth=&th1;
		       if((direction < 0)&&(th1 < Angle_change))
			  {
			 		if( sig1 >= Angle_change)
			 	   {
			 	   		Ptth=&sig1;
			 	   		PtV=&V3;
			 		}
			 		else
			 		{
			 		if((th1==0)&&(sig1==0))
			 		direction=-direction;
			 		else
			 		{
			 		flag=-1;
			 		*Ptth=Angle_change;
			 	    }
			 	    }			 	
			 	
			  }	
	break;
			 /***********************************/
	case 2:		   //  Change in Th2p
			  			  
			if( (direction<0)&&(th2p < Angle_change))
			{ 
    			
    			    direction=-direction;
    			    Ptth=&th2m;	
    			    PtV=&V2m;
    	    }
    	
    break;		
 /************************************************/
 
	case 3:		 
		   //  Change in Th2m
			  			  
			if( (direction<0)&&(th2m < Angle_change))
			{ 
    			
    			    direction=-direction;
    			    Ptth=&th2p;
    			    PtV=&V2p;	
    	    }
    	
    break;			
 /************************************************/
 	case 4:	 
		    if((direction < 0)&&(sig1 < Angle_change))
			  {
			 		if( th1 >= Angle_change)
			 		
			 		{	
			 			Ptth=&th1;
			 			PtV=&V1 ;
			 		}
			 			
			 		else
			 		{
			 		if((th1==0)&&(sig1==0))
			 		direction=-direction;
			 		else
			 		{
			 		flag=-1;
			 		*Ptth=Angle_change;
			 	    }
			 	    }			 	
			 	
			  }				 
	 break;
	 }
	/***********************************/ 	
    *Ptth +=direction*Angle_change; 
    *PtV=a2vss(*Ptth);
 }
 /*********************** Rewind **********************/
 
 void 	Rewind(void)
 {  int per_direction=1;
 LED45_on;
/*************************/
		per_direction=1;
      if( (th1+sig1)<.2*pi)
        per_direction=-1;
		
			 uin=  SOPin(th1,th2p-th2m,sig1,&vin,&win);
			    th1 -=per_direction*dtheta1_reset;
				th1=th1 >0 ? th1:0;        					   
				th2=reset_step(th1,uin,vin,win,&sig1);          
				if (th2 >= 0)
				{
				th2p=th2;
				th2m=0; 
				}
				else
				{
				th2m=-th2;
				th2p=0;
				}
			
				V1=a2vss(th1);
				V2m=a2vss(th2m);
				V2p=a2vss(th2p);
				V3=a2vss(sig1);
				Write_Output(V1,V2m,V2p,V3);
/*****************************/	

	LED45_off;
 }


  



	/********************************************************************************/
	
/************  Read Vout*******************************************/
	float Read_Vout(volatile int check_time)
	{
	volatile int NAverage,h,var1,var2; 
	volatile float	vout_temp;
   if ( check_time >0)
   {
		t=TIMER1_COUNTER;
		while( t < Control_Period )  
	    		{
	   	t=TIMER1_COUNTER;
	    		}
	    /*	t=TIMER1_COUNTER;    */     /* Check the time since last write  */
		 
		 TIMER1_DISABLE ;  
		 TIMER1_PERIOD ;
		 TIMER1_ENABLE  ;
		 TIMER1_DISABLE	;
	}
	  VL1=*pADC1;
	 VL1 &=Mask;
	 var2=*pADC1;
	 var2  &=Mask;
	 VL1 +=var2;
	 var2=*pADC1;
	 var2  &=Mask;
	 VL1 +=var2;
	 VL1 /=3.0;
	 VL1 -=BiosADC1;


	 
    VL2=*pADC2;
	VL2 &=Mask;
	var2=*pADC2;
	var2 &=Mask;
   	VL2+=var2;
   	var2=*pADC2;
	var2 &=Mask;
   	VL2+=var2;
   	var2=*pADC2;
	var2 &=Mask;
   	VL2+=var2;
   	var2=*pADC2;
	var2 &=Mask;
   	VL2+=var2;
   	VL2 /=5.0;
	
     VL2 = VL2 > BiosADC2 ? (VL2-BiosADC2): 0;  
	 VL2= VL2>0 ? VL2:0;
	 VL2= VL2 > VL1 ? VL1:VL2;

	 vout_temp=(VL1-2.0*VL2)/ VL1;
	 vout_temp= vout_temp >1 ? 1:vout_temp;
	 vout_temp=vout_temp < -1 ? -1:vout_temp;
	
	return  vout_temp;
	}	
	/*******************************************************************************/
	void Write_Output(volatile float v1,volatile float v2m,volatile float v2p,volatile float v3)
	{   
					 DAC1_data = (v1+DAC1_shift)*DAC1_scal; 
		 		     DAC2_data = (v2m+DAC2_shift)*DAC2_scal; 
		 		     DAC3_data = (v2p+DAC3_shift)*DAC3_scal; 
			 	     DAC4_data = (v3+DAC4_shift)*DAC4_scal;              
				     DAC_FIRE  ; 
				     /* Enable the timer to control the read /write period  */
				     TIMER1_DISABLE	;
		  			 TIMER1_PERIOD ;
	 	  			 TIMER1_ENABLE  ;
				   
				   	
	}
	/*****************************************************************/
	void 	Ini(void) 
	{ volatile int j,var1=0;
		 
		  
		  *(int *)EMIF_GCTL = 0x00003310; 
	 	  *(int *)EMIF_CE3 = 0x20B22224;  /* 0x20B22224;   */
	      V1=V2p=V2m=V3=Vmin;
	       LEDs_on  ;    
     	   for (j=1;j<1000000;j++);   
           LEDs_off;      
           for (j=1;j<1000000;j++);   
           LEDs_on ;  
           for (j=1;j<1000000;j++);   
           LEDs_off; 
       	
	 /**** Initialize DACs**************/
	 		DAC1_CREG ; 
	 		DAC2_CREG ;
	 		DAC3_CREG ;
	 		DAC4_CREG ;
	 /************DACs are ready to work******/   
	
	/************** Set the output of the DAC to zero **********/
	 DAC1_data = 0; 
	 DAC2_data = 0; 
	 DAC3_data =0; 
	 DAC4_data = 0;              
	 DAC_FIRE  ; 
	/************** Set the output of the DAC to zero **********/
	*pADC1=0x401;
	 *pADC1=0x400;   
	 *pADC1=ADC1_CR0 ;
	 *pADC1=ADC1_CR1 ;
	 *pADC2=0x401;
	 *pADC2=0x400; 
	 *pADC2=ADC2_CR0; 
	 *pADC2=ADC2_CR1 ;
	
/**********Time Initialization*******************/

		  TIMER1_DISABLE ;	
		  TIMER1_PERIOD ;