Diff: SPI.c

Revision:

213:9953db9543d6

Parent:

212:8d6784491bae

Child:

214:4c70e452c491

--- a/SPI.c	Mon Jul 03 08:32:27 2017 +0000
+++ b/SPI.c	Wed Jul 26 13:24:39 2017 +0000
@@ -22,13 +22,14 @@
 int ADC5Old,ADCDIF=0;
 int DACModReg;
 int SinPls=0,SinMns=0;
+ int timer=750;
 unsigned int ADC5New;
 unsigned int Buff_ADC_1		[32];
 unsigned int Buff_ADC_2		[32];
 unsigned int Buff_ADC_3	   	[32];
 unsigned int Buff_ADC_4		[32];
 
-int RegulADC,DeltaRegul;
+int RegulADC,DeltaRegul,tempDeltaRegul;
 unsigned int Buff_ADC_5		   [512];
 unsigned int BuffADC_16Point	[64];
 unsigned int BuffADC_32Point	[64];		
@@ -144,16 +145,31 @@
    		SinMns=0; 
 }
 void  HFORegul(void)
-{	
+{
+
  RegulADC=32767-Spi.ADC5;
- DeltaRegul=((Gyro.HFO_ref-RegulADC)>>11);
+ DeltaRegul+=((Gyro.HFO_ref-RegulADC));
+ //tempDeltaRegul+=((Gyro.HFO_ref-RegulADC));
+ //if(DeltaRegul>50)DeltaRegul=50;
+
+ if(timer) timer--;
+ else
+ {
+	/*sprintf((Time),"%d  %d  %d  %d  %d  %d \r\n", Gyro.HFO_Min , Spi.DAC_A,Gyro.HFO_Max,tempDeltaRegul>>10,   Spi.DAC_B, Gyro.CuruAngle);
+    WriteCon(Time);
+    Gyro.CuruAngle=0;*/
+    //tempDeltaRegul=0;
  if(Gyro.RgConA&0x2) 
  {
+ 	DeltaRegul=DeltaRegul>>10;
+ 	tempDeltaRegul=DeltaRegul>>10;
  	Spi.DAC_A-=DeltaRegul*Gyro.HFO_Gain;
- 	
- 	if(Spi.DAC_A>Gyro.HFO_Min)		Spi.DAC_A-=0x7ff;
- 	else if(Spi.DAC_A<Gyro.HFO_Max) Spi.DAC_A+=0x7ff;
- 	}
+ 	timer=10;
+ }
+ 	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;
  
 }               
                 
@@ -271,16 +287,16 @@
 {
 	if(dispersion>3)
     {
-	unsigned int step = 50, ENDMOD=32767;
+	unsigned int step = 50, ENDMOD=65400;
 	sprintf((Time),"%d %d %d %d     %d   %d   %d\r\n", Gyro.CuruAngle, Spi.DAC_B, Gyro.AD_Slow, Spi.ADC5, 0xfFFf-Spi.ADC1, Spi.ADC1, Gyro.Termo);
     Gyro.CuruAngle=0;
     WriteCon(Time);
-	Spi.DAC_B-=step;
-	if(Spi.DAC_B<ENDMOD)
+	Spi.DAC_B+=step;
+	if(Spi.DAC_B>ENDMOD)
 		{
 		 Gyro.LogMod=0;
 		 PlcON
-		 Spi.DAC_B = 32000;
+		 Spi.DAC_B = 48000;
 		}
 	dispersion=0;
 	}