Dmitry Kovalev
/
LG2
fork
Fork of LG by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2016-04-01
- Revision:
- 107:4d178bcc9d8a
- Parent:
- 106:250ddd8629c6
- Child:
- 108:030cdde08314
File content as of revision 107:4d178bcc9d8a:
#include "Global.h" GyroT Gyro; //volatile int V1 = 0 ; //volatile int Temp = 0 ; volatile unsigned int Flag=0; unsigned int FrecTemp=0; unsigned int FrecTp=0,amp=0; void Discharg ()//проверка битового поля поджига и установка значения бита поджига { if (Gyro.Discharg) { //Проверка здвигового поля(последовательности) поджига Gyro.Discharg = Gyro.Discharg >> 1; //если последний бит 1 то установить бит поджига в еденицу, иначе в 0; if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD; } } void BackLight () { if (Gyro.BackLight) { //Проверка здвигового поля(последовательности) подсветки Gyro.BackLight = Gyro.BackLight >> 1; //если последний бит в битовом поле 1 то установить бит подсветки в еденицу, иначе в 0; if (Gyro.BackLight & 0x01) Gyro.PinReg |= PinRegBitL; else Gyro.PinReg &= ~PinRegBitL; } } void VibroOut(void) // выставка ног вибро { if(CountV31>=16) {//первая нога вибро // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { Gyro.PinReg &= ~PinRegBit_1V;/*LoopOn*///установить в регистре PinReg бит "вибро 1" в "0" } else { Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1" } } else {//вторая нога вибро if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0" } else { Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1" } } } void CalcAmpN(void) { static int PeriodCount = 0; unsigned int Nmax=0; //расчет амплитуды относительно центральной точки if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. PeriodCount=0; 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); //правая граница амплитуды Flag=1; } else { 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+Gyro.AmpPerDel))/400);//левая граница амплитуды Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды Flag=0; } //Tnoise=55; srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp //изменение флага предидущей амплитуды } else { PeriodCount++; } } ////////////////////////////////////////////////////////////////////////////// /////////////////////////функция работы вибропривода////////////////////////// ////////////////////////////////////////////////////////////////////////////// void cheng(void) { static int TempFaza, CountFaza; if (Buff_Restored_sin [CountV31] > MaxAmp) {MaxAmp = Buff_Restored_sin [CountV31];} // подумать со знаком*/ 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));//запись в таймер 1 значение частоты вибро // Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); //запись частоты для выдачи в аск_глд старый протокол Time_vibro=0; break; case 24: Gyro.Amp -= (MaxAmp - 350)*50; // расчет амплитудв с учетом разници //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16); // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Gyro.AmpPer = Gyro.Amp>>16; amp=MaxAmp; MaxAmp=0; break; case 16: Time_vibro=0; // расчет Фазы с учетор разници TempFaza = -4; for (CountFaza = 0; CountFaza <8; CountFaza++ ) { if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++; } // FrecTp=Buff_Restored_sin [(CountV31 - 12 + CountFaza) & 0xff]; if(Gyro.RgConA&0x1){ Gyro.Frq += TempFaza*1000;/// перепутан вибро 1 вибро 2 } break; } }