Diff: host/Source/App/vibro.c

Revision:

23:12e6183f04d4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/host/Source/App/vibro.c	Wed Feb 03 10:44:42 2016 +0300
@@ -0,0 +1,221 @@
+#include "vibro.h"
+#include "stdlib.h"
+#include "lpc17xx.h"
+#include "ctime"
+#include "InputOutput.h"
+#include "Parameters.h"
+#include "CntrlGLD.h"
+#include "CyclesSync.h"
+#include "SIP.h"
+//extern OUTPUT Output;
+#include "QEI.h"
+#include "Global.h"
+#include "MTimer.h"
+
+struct MAIN Main;
+
+
+volatile int V1 = 0 ;
+volatile int Temp = 0 ;
+volatile unsigned int Flag=0;
+
+unsigned int FrecTemp=0;
+/////////////////////////////////////////////////////////////////////////////
+/////////////////////////èíèöèàëèçàöèÿ âèáðîïðèâîäà//////////////////////////
+/////////////////////////////////////////////////////////////////////////////
+	unsigned int init_Vibro ()
+		{
+			Main.FrqRate=40;
+			Main.FrqMin=380;
+			Main.FrqHZ=449;
+			Main.Frq = Main.FrqHZ<<16;
+		
+	   	Main.AmpPer=20;
+	    Main.AmpPerDel=10;
+     	Main.Amp  = 10*65535;
+			Main.AmpL = 20*650;
+/*
+			  Vib.AmpH=50*650;
+		//	Vib.AmpT=600;
+		//	Output.Str.T_Vibro=40;
+		//LPC_GPIO0->FIOSET   |= (1<<5);
+*/
+			return 0;
+		}
+		
+
+void VibroOut(void)
+{
+
+	
+	if(CountV31>=16)
+	{
+	if((Time_vibro>Main.AmpN1) && (Time_vibro<Main.AmpN2))	{ClrV2 /*LoopOn*/}
+	  else{ SetV2 	}
+	}
+	else
+		{
+			if((Time_vibro>Main.AmpN1)&&(Time_vibro<Main.AmpN2))	{ClrV1 /*LoopOff*/}
+      else{ SetV1	}
+	  }
+
+
+
+}
+
+void CalcAmpN(void)
+{
+	static int PeriodCount = 0,Tnoise;
+	    
+	    
+	 
+	    unsigned int Nmax=0;
+	   
+    	
+	     if(PeriodCount>= Tnoise)//åñëè êîëè÷åñòâî çàõîäîâ â ïðåðûâàíèå áîëüøå ëèáî ðàâíî ÷àñòîòå îøóìëåíèÿ.
+				{
+					if (Flag==0)//ñåé÷àñ ìàëàÿ àìïëèòóäà?
+							{
+								
+								if(Main.AmpPer>90){Main.AmpPer=90;}
+						  	Nmax =(unsigned int)((100000/(Main.Frq>>16))-1);
+		          	Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer))/400);
+			          Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1);
+						  	Flag=1;	
+								
+							}
+	
+					else
+						{
+							if((Main.AmpPer+Main.AmpPerDel)>90){Main.AmpPer=90-Main.AmpPerDel;}
+							
+							Nmax =(unsigned int)((100000/(Main.Frq>>16))-1);
+		        	Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer+Main.AmpPerDel))/400);
+			        Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1);
+							Flag=0;
+						}  
+      Main.AmpMin =10;
+		  Main.AmpTD =30;
+		  //Tnoise=55;	
+		  srand(Global_Time);
+			Main.AmpT = (rand() % Main.AmpTD+Main.AmpMin);// ÎØÓÌËÅÍÈÅ amp
+	   	Tnoise=Main.AmpT;							
+							//èçìåíåíèå ôëàãà ïðåäèäóùåé àìïëèòóäû
+							PeriodCount=0;
+				}
+			else{PeriodCount++;}
+	
+	
+	   
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/////////////////////////ôóíêöèÿ ðàáîòû âèáðîïðèâîäà//////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+void cheng(void)
+{  
+  	static int TempFaza, CountFaza;
+
+
+		//Noise();
+
+		//Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
+/*
+//	CountV++;
+//	CountV &= 0x1f;
+*/	
+    	if (Buff_Restored_sin [CountV255] > MaxAmp)
+			{
+				MaxAmp=Buff_Restored_sin [CountV255];
+			}
+	
+
+	
+	
+	
+		switch(CountV31)
+		{ 
+			case 0:	
+			CalcAmpN();
+			//Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
+			LPC_TIM1->MR0 =(unsigned int)(100000000/(Main.Frq>>11));
+			Output.Str.T_Vibro=(unsigned int)((7680000*16/Main.Frq)*4096);
+			VibroPush();
+  //	LoopOn
+  //	LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq);	
+			Time_vibro=0;
+			break;
+      case 10:
+			
+//			if (MaxAmp>(65535+400))
+//			{
+//				Vib.Amp-=1000;//
+//			}
+//			else
+//			{
+//				Vib.Amp+=1000;
+//			}
+			
+			
+			
+		
+			Main.Amp -= (MaxAmp - 65536 - 300)*4;  // ðàñ÷åò àìïëèòóäâ ñ ó÷åòîì ðàçíèöè 
+			///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			if(Main.Amp>2500000){Main.Amp=1200000;}// âðåìåííîå îãðàíè÷åíèå ðîñòà àìïëèòóäû â ñëó÷àå íåïîäîæåííîãî ãèðîñêîïà//////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////		
+
+			
+			Main.AmpPer = Main.Amp>>16;
+			MaxAmp=0;
+			FrecTemp=Main.AmpPer;//äëÿ âûâîäà â òåðìèíàë
+			break;
+			case 16:
+			VibroPull();
+			//SetV2
+	   // LoopOff
+			Time_vibro=0;
+
+
+			
+			
+			
+
+// ðàñ÷åò Ôàçû ñ ó÷åòîð ðàçíèöè 
+			TempFaza = -4;
+			for (CountFaza = 0; CountFaza < 8; CountFaza++ ) 		
+			{	
+			  if (Buff_Restored_sin [(CountV255- 12  + CountFaza) & 0xff] > 65535) TempFaza++;
+			} // ðàñ÷åò Ôàçû ñ ó÷åòîð ðàçíèöè 
+  	  Main.Frq -=TempFaza*1000;
+
+			
+			
+			
+//			if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535)
+//			{
+//			Vib.Frq=Vib.Frq-10000;
+//			}
+
+//			else
+//			{
+//				Vib.Frq=Vib.Frq+1000;
+//			}
+			break;
+		}
+		
+
+
+
+		/*
+		if((CountV & 0x0f) == V1)
+			{
+				ClrV1
+				ClrV2	
+			}*/
+}
\ No newline at end of file