forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

Revision:
220:04c54405b82d
Parent:
218:b4067cac75c0
Child:
221:53b256368ca1
--- a/SPI.c	Wed Sep 27 13:09:24 2017 +0000
+++ b/SPI.c	Fri Oct 20 13:16:32 2017 +0000
@@ -8,6 +8,7 @@
 int AD_Regul = 0;
 int temp9,tempADC5;
 int AD_MAX=0;
+unsigned int SPIlog;
 int k=0,l=0,r=0,n=0;//счетчики для регулировки периметра
 int flagmod=0,Bdelta;
 int start=10;
@@ -28,9 +29,10 @@
 unsigned int Buff_ADC_2		[32];
 unsigned int Buff_ADC_3	   	[32];
 unsigned int Buff_ADC_4		[32];
-
+int Buff_OUT1	[64];
+int Buff_OUT	[64];
 int RegulADC,DeltaRegul,tempDeltaRegul;
-
+int count10v=0,AMPSUM,AMPSUMP,AMPSUMM,AMPSUM_DESP,AMPSUMPout,AMPSUMMout,SumDelta,PLC_EROR,PLC_Flag,HFO_Flag,HFOdelta,HFOregul,HFOSumDelta;
 
 
 //int BuffADC_32Point	[64];		
@@ -48,7 +50,7 @@
 
 //unsigned int Buff_AMP		[256];
 //unsigned int Buff_AMP64P	[256];
-unsigned int TypeMod=4;
+unsigned int TypeMod=0;
 unsigned int ModArraySin	  [64] = {50,55,59,64,68,73,77,81,85,88,91,94,96,98,99,99,100,99,99,98,96,94,91,88,85,81,77,73,68,64,59,55,50,45,41,36,32,27,23,19,16,12,9,7,4,2,1,1,0,1,1,2,4,7,9,12,16,19,23,27,32,36,41,45};
 unsigned int ModArraySin32	  [32] = {50,59,68,77,85,91,96,99,100,99,96,91,85,77,68,59,50,41,32,23,16,9,4,1,0,1,4,9,16,23,32,41};
 unsigned int ModArrayTriangle [64];
@@ -58,78 +60,36 @@
 
 void InitMOD(void)
 {
-    /* for (int i = 0; i < 64; i++ ) 
+    for (int i = 0; i < 64; i++ ) 
  	 { 
-   	 if(i<32)   { ModArrayTriangle[i]=Mod; Mod+=3;}
-   	 else       { ModArrayTriangle[i]=Mod; Mod-=3;}
+   	 if(i<32)   { ModArrayTriangle[i]=Mod; Mod=100;}
+   	 else       { ModArrayTriangle[i]=Mod; Mod=0;}
      }
     
      for (int i = 0; i < 16; i++ ) 
  	 { 
       ModArraySaw[i]=Mod;
       Mod+=1;
-     }*/
+     }
 }
 void Modulator(void)
 {
 	switch(TypeMod) 
 		{
-	    case 0: LPC_DAC->DACR = (ModArraySin		[(CountV64)&0x3f]*Gyro.ModAmp);     break;
+	  
+	 //   case 0: LPC_DAC->DACR = (ModArraySin		[(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000; break;    
+	 case 0: LPC_DAC->DACR = (ModArraySin		[(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000; break;
+	 
 	    case 1: LPC_DAC->DACR = (ModArraySaw		[CountV64]*Gyro.ModAmp); 			break;
 	    case 2: LPC_DAC->DACR = (ModArrayTriangle	[(CountV64-2)&0x3f]*Gyro.ModAmp);  	break;
 	    case 3: LPC_DAC->DACR = (ModArrayTriangle	[(CountV64-2)&0x3f]*Gyro.ModAmp);  	break;
-   	    case 4: LPC_DAC->DACR = ((ModArraySin32		[(CountV31)&0x1f]+150)*Gyro.ModAmp);      		 break;
+ 
+   	    //case 4: LPC_DAC->DACR = ((ModArraySin32		[(CountV31)&0x1f]+150)*Gyro.ModAmp);      		 break;ShiftMod
+     	case 4: LPC_DAC->DACR = ((ModArraySin32		[(CountV31+8)&0x1f])*Gyro.ModAmp + Gyro.ShiftMod-32000); break;
    	    case 5:    		 break;
 	    }
 }
-void  PLCRegul(void)
 
-{
-static unsigned int Flag_64=0, count;	    
-static int  CountFaza,Sin;
-
-        		for (CountFaza = 0;  CountFaza <32; CountFaza++)   
-        		{ 
-        			Sin=(ModArraySin32[CountFaza] - 50);
-        			if(Sin < 0) znak = -1;
-        			else znak = 1;
-        			BuffADC_znak[CountFaza] =  znak;
-        	    }
-
-       		for (CountFaza = 0;  CountFaza <16; CountFaza++)   
-        		{ 
-       			SinPls+=  ((Buff_Restored_Mod[(CountFaza - Gyro.PLC_Phase) & 0x1f])*BuffADC_znak[CountFaza]);
-        		}
-        	for (CountFaza = 16;  CountFaza <32; CountFaza++)   
-        		{
-       			SinMns-=   ((Buff_Restored_Mod[(CountFaza - Gyro.PLC_Phase) & 0x1f])*BuffADC_znak[CountFaza]);
-        		}   			    
-      			    if(count>128)
-      			    {
-      			    count=0;
-      			    Gyro.PLC_Eror_count = (SinMns+SinPls)>>7; 
-       		 	    SinPls=0;
-       		 	    SinMns=0;
-       		 	    	 if(Gyro.RgConA&0x8)
-							{
-								if( DeltaRegul<8) Spi.DAC_B+=(Gyro.PLC_Eror_count>>3)*Gyro.PLC_Gain;
-       		 	 			}
-       		 	    }
-       		 	    else count++;
-
-  if(Gyro.LG_Type==1)
- 	{
-   		if	 	( Spi.DAC_B < Gyro.HighTreshold ) Spi.DAC_B = (Gyro.ResetLevelHeat);       
-    	else if	( Spi.DAC_B > Gyro.DownTreshold ) Spi.DAC_B = (Gyro.ResetLevelCool);
-    }
-  else
-    {
-    	if      ( Spi.DAC_B < 10300 )	Spi.DAC_B = 32000;            //проверка на переваливание за границу.  
-      	else if ( Spi.DAC_B > 57000 )	Spi.DAC_B = 32000;
-    }     
-}
-
- 
 void  PLCRegul250(void)
 {	
     unsigned int temp;
@@ -160,48 +120,37 @@
    		SinPls=0;
    		SinMns=0; 
 }
-void  HFORegul(void)
+          
+                
+ 
+ void  HFORegul2(void)
 {
 static unsigned int countHFO;
-	 if(countHFO<32)
-	 {
-	 RegulADC=(32767-Spi.ADC5);
- 	 DeltaRegul+=((Gyro.HFO_ref-RegulADC));
-	 countHFO++;
- 	 }
- 	 else
-	 {
+	
 	 countHFO=0;
 	 if(Gyro.RgConA&0x2) 
 		 {
-		 	
-		 	DeltaRegul=DeltaRegul>>14;
-		 	tempDeltaRegul=DeltaRegul;
-
-       		
+		 	if(HFO_Flag)
+		 	{
+ 	        DeltaRegul+=(Gyro.HFO_ref-(AMPSUM>>8));
+	        
+	        sprintf((Time),"%d  %d  %d\r\n", AMPSUM>>8, DeltaRegul, Gyro.HFO_ref);
+    	 	WriteCon(Time);
+    	 	HFO_Flag=0;
+    	 	AMPSUM=0;
+       		}
             switch(DeltaRegul) {
             case  1: DeltaRegul=0; break;
-            case  2: DeltaRegul=0; break;
-            case  3: DeltaRegul=0; break;
-            //case  4: DeltaRegul=0; break;
             case -1: DeltaRegul=0; break;
-            case -2: DeltaRegul=0; break;
-            case -3: DeltaRegul=0; break;
-            //case -4: DeltaRegul=0; break;
-     		}
-     		/*sprintf((Time)," %d  %d \r\n",DeltaRegul,tempDeltaRegul);
-       		WriteCon(Time);*/
-		 	
-		 	Spi.DAC_A-=DeltaRegul*Gyro.HFO_Gain;
-		 	timer=10;
+           }
+        	Spi.DAC_A-=(DeltaRegul>>2)*Gyro.HFO_Gain;
+        	DeltaRegul=0;
 		 }
  	 else DeltaRegul=0;
- 	 }
     if(Spi.DAC_A>Gyro.HFO_Min-1)	  Spi.DAC_A=Gyro.HFO_Min-2;
  	else if(Spi.DAC_A<Gyro.HFO_Max+1) Spi.DAC_A=Gyro.HFO_Max+2;
-}               
-                
-         
+ 	
+}          
            
 // Spi.DAC_B-=ADCDIF>>6;
         
@@ -248,10 +197,124 @@
         Gyro.ADS_NewData = 1;
 	}
 }
+void ModFilt(void)
+{
+	       static int SumBuff,Delta, OldDelta;
+	      
+        
+        
+         if (count10v>=64*8)
+ 		  {
+ 	        count10v=0;
+ 	        
+ 	     /*  for(int q=0; q<5; q++)
+      	       	{ 			
+      	    	AMPSUMP+=BuffADC_10v[q];
+       	     	}*/
+
+ 	 /*     for(int q=0; q<32; q++)
+      	   	 	{
+      	    	AMPSUMP += BuffADC_10v[q];
+	      	    	
+	       	   }
+       	   for(int q=32; q<64; q++)
+      	    	{
+      	    	AMPSUMM += BuffADC_10v[q];
+       	   		}
+       	    */
+       	    /////////////////////////////
+       	    
+          /*  for(int q=32; q<32+5; q++)
+      	    	{
+      	    	AMPSUMM+=BuffADC_10v[q];
+       	    	}*/
+ 	       /* for(int q=32+12; q<32+20; q++)
+      	    	{
+      	    	AMPSUMM+=BuffADC_10v[q];
+       	    	}*/
+      	   /* for(int q=32+28; q<32+32; q++)
+      	   	 	{
+      	    	AMPSUMM+=BuffADC_10v[q];
+       	    	} */
+       	 
+           
+          /*  sprintf((Time),"%d  %d    %d   %d \r\n",  AMPSUM, AMPSUMP, AMPSUMM,AMPSUMP  - AMPSUMM);
+            WriteCon(Time);*/
+             	
+    
+ 	        
+ 	        for(int q=0; q<32; q++)
+      	    	{
+	  	    	SumBuff += BuffADC_10v[q];
+	         	SumBuff -= BuffADC_10v_OLD[q+32];      	       	
+	  	       	BuffADC_10v_F [q] = SumBuff;
+          		}
+  	        for(int q=32; q<64; q++)
+      	    	{
+     	    	 SumBuff += BuffADC_10v[q];
+         	   	 SumBuff -= BuffADC_10v[q-32];      	       	
+  	       		 BuffADC_10v_F [q] = SumBuff;
+          		}         	
+           
+ 	      for(int q=0; q<32; q++)
+      	   	 	{
+      	    	AMPSUMP +=BuffADC_10v_F [q];
+	      	    	
+	       	   }
+       	   for(int q=32; q<64; q++)
+      	    	{
+      	    	AMPSUMM += BuffADC_10v_F [q];
+       	   		}
+       	   		
+         
+    	            
+       
+    	        for(int q=0; q<64; q++)
+      	    	{
+      	    	 	AMPSUM+=BuffADC_10v[q];
+      	    	 	//Buff_OUT[q]=BuffADC_1Point[q];
+      	    	 	Buff_OUT[q]=BuffADC_10v_F [q];
+      	    	 	//Buff_OUT1[q]=
+   	       	 		BuffADC_10v_OLD[q]= BuffADC_10v[q];
+    	   		 	BuffADC_10v[q] =0;
+           		}
+            sprintf((Time),"%d  \r\n",  Spi.DAC_A);
+            WriteCon(Time);
+          	   
+       	    AMPSUMPout=AMPSUMP;
+       	    AMPSUMMout=AMPSUMM;
+       	    
+            Delta= (AMPSUMPout - AMPSUMMout);
+            SumDelta+=Delta;
+            Gyro.PLC_Eror_count=SumDelta/Gyro.PLC_Gain;
+            SumDelta-=Gyro.PLC_Gain*Gyro.PLC_Eror_count;
+          	 
+          	AMPSUM=AMPSUM>>6;
+          	AMPSUM=AMPSUM>>3; 
+        
+           	AMPSUM_DESP=AMPSUM;
+           	HFOdelta=(int)((Gyro.HFO_ref)-AMPSUM);
+           	PLC_EROR=Gyro.PLC_Eror_count;
+           	
+           	HFOSumDelta+=HFOdelta;
+           	HFOregul=HFOSumDelta/(int)(Gyro.HFO_Gain);
+           	
+           	HFOSumDelta-=Gyro.HFO_Gain*HFOregul;
+	        tempDeltaRegul=HFOregul;
+	        
+            HFO_Flag=1;
+            PLC_Flag=1;
+    	 	AMPSUMM=0;
+    	 	AMPSUMP=0;
+    	    AMPSUM=0;
+          }
+}
+
 void SPI_Exchange(void)  // новая функция чтения, в нецй не должно быть ничего лишнего
 {
-   
+  
     unsigned int DummySPI;
+    
     //unsigned int ADC5Dif;
     ADC5New = LPC_SSP0->DR;// Чтение АЦП
     //Spi.ADC5_Accum += LPC_SSP0->DR;
@@ -283,38 +346,92 @@
        }
         if(Time1Hz>6)
         {    
-        BuffADC_1Point[CountV255]  =(0x7fff-ADC5New)&0x7fff;
+        BuffADC_1Point[CountV255] = (0x7fff-ADC5New)&0x7fff;
         BuffADC_1Point_64[CountV31]=(0x7fff-ADC5New)&0x7fff;
 
+       /* ADC_128Point += BuffADC_1Point[CountV255];
+        ADC_128Point -= BuffADC_1Point[(CountV255 - 128) & 0xff];						// заполнение буфера накопленых приращений за 64 тактов
+        BuffADC_128Point[CountV64] = ADC_128Point;*/
+
+		
+     //   Buff_Restored_Mod[CountV31] =(int)(BuffADC_1Point_64[CountV31] - ((BuffADC_128Point[CountV64])>>7));       
+   //     Buff_Restored_Mod[CountV64] =(int)(BuffADC_32PointD[CountV64]*2 -  BuffADC_64Point[CountV64]);
+        
+   
+        count10v++;
+        BuffADC_10v[CountV64] += BuffADC_1Point[CountV255];
+        
+        ModFilt();
+  
+       }
+            
+}
+
+void  HFORegul(void)
+{
+static unsigned int countHFO;
+	
+	 countHFO=0;
+	
+	 if(Gyro.RgConA&0x2) 
+		 {
+		   	if(HFO_Flag)
+		   	{
+		   	switch(HFOregul){
+		   
+		  // 	case  1:  HFOregul=0; break;
+		//   	case -1:  HFOregul=0; break;
+//		  	case  2:  HFOregul=1; break;
+//		   	case -2:  HFOregul=1; break; 	
+		   	
+		   	
+		   	}
+		    Spi.DAC_A -= HFOregul;
+		   	HFO_Flag=0;
+        	DeltaRegul=0;
+        	}
+		 }
+		
+ 	 else DeltaRegul=0;
+ 
+    if(Spi.DAC_A>Gyro.HFO_Min-1)	  Spi.DAC_A=Gyro.HFO_Min-2;
+ 	else if(Spi.DAC_A<Gyro.HFO_Max+1) Spi.DAC_A=Gyro.HFO_Max+2;
+ 	
+}     
+
 
-        ADC_64Point += BuffADC_1Point[CountV255];
-        ADC_64Point -= BuffADC_1Point[(CountV255 - 128) & 0xff];						// заполнение буфера накопленых приращений за 64 тактов
-        BuffADC_64Point[CountV64] = ADC_64Point;
+void  PLCRegul(void)
 
-           
-           
-        /*ADC_32PointD += BuffADC_1Point[CountV255];  
-        ADC_32PointD -= BuffADC_1Point[(CountV255 - 32) & 0xff]; 							// заполнение буфера накопленых приращений за 32 тактов
-        ADC_32PointD += BuffADC_1Point[(CountV255 - 64) & 0xff];  
-        ADC_32PointD -= BuffADC_1Point[(CountV255 - 96) & 0xff];*/ 
-      //  BuffADC_32PointD[CountV64] =  ADC_32PointD;
-
-       
-      	/* 
-        ADC_16Point += BuffADC_1Point[CountV255];  
-        ADC_16Point -= BuffADC_1Point[(CountV255 - 16) & 0xff];
-        BuffADC_16Point	[CountV64] =  ADC_16Point; 
-		*/
-            
-   //     Buff_Restored_Mod[CountV64] =(int)(BuffADC_32PointD[CountV64]*2 -  BuffADC_64Point[CountV64]);
-        Buff_Restored_Mod[CountV31] =(int)(BuffADC_1Point_64[CountV31] - ((BuffADC_64Point[CountV64])>>7));
+{
+static unsigned int Flag_64=0, count;	    
+static int  CountFaza,Sin;
 
 
+ 	 if(Gyro.RgConA&0x8)
+		{
+		if(PLC_Flag)
+			{  
+	  	  	 	PLC_Flag=0;
+	  	  		Spi.DAC_B+=(Gyro.PLC_Eror_count);
+	  	  		
+	  	  		
+			}
+   		 }   
+   		Gyro.PLC_Eror_count=0;  		 	
+  if(Gyro.LG_Type==1)
+ 	{
+   		if	 	( Spi.DAC_B < Gyro.HighTreshold ) Spi.DAC_B = (Gyro.ResetLevelHeat);       
+    	else if	( Spi.DAC_B > Gyro.DownTreshold ) Spi.DAC_B = (Gyro.ResetLevelCool);
+    }
+  else
+    {
+    	if      ( Spi.DAC_B < 10300 )	Spi.DAC_B = 32000;            //проверка на переваливание за границу.  
+      	else if ( Spi.DAC_B > 57000 )	Spi.DAC_B = 32000;
+    }     
+}
 
-     // SinPls +=  (Buff_Restored_Mod[ (CountV64 - Gyro.PLC_Phase)  & 0x3f]/*znak*/)>>5;
-    //  SinMns +=  (Buff_Restored_Mod[ (CountV64 - Gyro.PLC_Phase)  & 0x3f]/*znak*/)>>5;
-        }       
-}
+ 
+
 
 void ShowMod(void)//технологическая функция для просмотра в ориджине мод на всем диапазпне цап
 {
@@ -362,7 +479,7 @@
 }
 void DAC_OutPut(void)//выдача в цапы
 {
-  	if(Gyro.RgConA&0x10)	Modulator();
+  	/*if(Gyro.RgConA&0x10)*/	Modulator();
     LPC_SSP0->DR=0x5555;
     LPC_SSP0->DR=0x5555;
     LPC_SSP0->DR=0x5555;
@@ -370,12 +487,12 @@
     if (CountV31 & 1) 
     {                      	//если нечетный такт то
         LPC_SSP0->DR =  WRITE_DAC0;   			//e.команда для ЦАП_0 передавать.
-        LPC_SSP0->DR = (Spi.DAC_A)+ Gyro.Debag2;         	//e. передача 12 бит
+        LPC_SSP0->DR = (Spi.DAC_A);         	//e. передача 12 бит
     } 
     else 
     { //если такт четный.
         LPC_SSP0->DR = WRITE_DAC1 ; //e.команда для ЦАП_1 передавать.
-        LPC_SSP0->DR = (Spi.DAC_B) + Gyro.Debag;
+       LPC_SSP0->DR = (Spi.DAC_B) ;
     }
 }