Dmitry Kovalev
n
Diff: vibro.c
- Revision:
- 21:bc8c1cec3da6
- Parent:
- 0:8ad47e2b6f00
- Child:
- 28:1c9acd3b224d
--- a/vibro.c Tue Feb 02 17:14:25 2016 +0000
+++ b/vibro.c Wed Feb 03 07:19:30 2016 +0000
@@ -1,16 +1,5 @@
-#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;
@@ -21,43 +10,46 @@
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;
- }
-
+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 }
- }
+
+ 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
+ }
+ }
@@ -65,88 +57,88 @@
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++;}
-
-
-
+ 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;
+{
+ static int TempFaza, CountFaza;
- //Noise();
+ //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];
- }
-
+ //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:
-
+
+ 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;//
@@ -155,48 +147,49 @@
// {
// 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;
+ 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;
+
-
-
-
+// расчет Фазы с учетор разници
+ 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;
@@ -206,16 +199,16 @@
// {
// Vib.Frq=Vib.Frq+1000;
// }
- break;
- }
-
+ break;
+ }
+
- /*
- if((CountV & 0x0f) == V1)
- {
- ClrV1
- ClrV2
- }*/
-}
+ /*
+ if((CountV & 0x0f) == V1)
+ {
+ ClrV1
+ ClrV2
+ }*/
+}
\ No newline at end of file