Dmitry Kovalev
/
LG
n
Fork of LG by
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