Dmitry Kovalev
/
LG2
fork
Fork of LG by
Diff: vibro.c
- Revision:
- 85:0466ee8cdfc8
- Parent:
- 48:8697dfe679b9
- Child:
- 87:7e575d26d6d0
--- a/vibro.c Mon Mar 21 13:29:41 2016 +0000 +++ b/vibro.c Wed Mar 23 11:10:55 2016 +0000 @@ -7,36 +7,53 @@ ///////////////////////////////////////////////////////////////////////////// /////////////////////////инициализация вибропривода////////////////////////// ///////////////////////////////////////////////////////////////////////////// + +void Discharg () +{ + if (Gyro.Discharg) + { + //Проверка поджига + Gyro.Discharg = Gyro.Discharg >> 1; + if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD; + } +} +void BackLight () +{ + if (Gyro.BackLight) + { //Проверка подсветки + Gyro.BackLight = Gyro.BackLight >> 1; + if (Gyro.BackLight & 0x01) Gyro.PinReg |= PinRegBitL; else Gyro.PinReg &= ~PinRegBitL; + } +} + + void init_Vibro () { - Gyro.FrqRate=40; - Gyro.FrqMin=380; + + Gyro.FrqHZ=449; Gyro.Frq = Gyro.FrqHZ<<16; Gyro.AmpPer=20; Gyro.AmpPerDel=10; Gyro.Amp = 10*65535; - Gyro.AmpL = 20*650; - /* - Vib.AmpH=50*650; - // Vib.AmpT=600; - // Output.Str.T_Vibro=40; - //LPC_GPIO0->FIOSET |= (1<<5); - */ + + Gyro.AmpMin =10;// минимальное значение AmpT; + Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления) + } -void VibroOut(void) +void VibroOut(void) // выставка ног вибро { - if(CountV31>=16) { + if(CountV31>=16) { + // 50 75 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { - Gyro.PinReg &= ~PinRegBit_1V;/*LoopOn*///установить в регистре PinReg бит "вибро 1" в "0" - LedOFF + Gyro.PinReg &= ~PinRegBit_1V;/*LoopOn*///установить в регистре PinReg бит "вибро 1" в "0" } else { Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1" - LedON } + } else { if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0" @@ -51,50 +68,50 @@ void CalcAmpN(void) { - static int PeriodCount = 0,Tnoise; - - - + static int PeriodCount = 0; unsigned int Nmax=0; + //расчет амплитуды относительно центральной точки - if(PeriodCount>= Tnoise) { //если количество заходов в прерывание больше либо равно частоте ошумления. - if (Flag==0) { //сейчас малая амплитуда? + if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. + PeriodCount=0; - if(Gyro.AmpPer>90) { - Gyro.AmpPer=90; + if (Flag==0) { //сейчас малая амплитуда? + if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { + Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды } - Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400); - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); + //250 100000 / 399 + Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); // + // 50 (( 250 * (100-20)) /400) // + Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400); //левая граница амплитуды + // 75 ((250 /2)-50) + Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды Flag=1; } else { if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { - Gyro.AmpPer=90-Gyro.AmpPerDel; + Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды } - + //250 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400); - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); + //56 ((250*(100-20+10))/400) + Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400);//левая граница амплитуды + //69 ((250/2)-56 + Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды Flag=0; } - Gyro.AmpMin =10; - Gyro.AmpTD =30; + //Tnoise=55; - srand(Global_Time); + srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. + Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - Tnoise=Gyro.AmpT; //изменение флага предидущей амплитуды - PeriodCount=0; + } else { PeriodCount++; } - - - } ////////////////////////////////////////////////////////////////////////////// @@ -103,105 +120,37 @@ 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) { + 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/(Gyro.Frq>>11)); - Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); - // LoopOn - // LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq); + CalcAmpN();//расчет ошумления и амплитуды задание интервалов выставки ножек вибро. + //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; //прием частоты из аск_глд старый протокол + LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));//запись в таймер 1 значение частоты вибро + Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); //запись частоты для выдачи в аск_глд старый протокол Time_vibro=0; break; case 10: - -// if (MaxAmp>(65535+400)) -// { -// Vib.Amp-=1000;// -// } -// else -// { -// Vib.Amp+=1000; -// } - - - - Gyro.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// if(Gyro.Amp>2500000) { Gyro.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - Gyro.AmpPer = Gyro.Amp>>16; MaxAmp=0; - FrecTemp=Gyro.AmpPer;//для вывода в терминал break; + case 16: - - //SetV2 - // LoopOff Time_vibro=0; - - - - - // расчет Фазы с учетор разници TempFaza = -4; for (CountFaza = 0; CountFaza < 8; CountFaza++ ) { if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++; } // расчет Фазы с учетор разници Gyro.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