Dmitry Kovalev
/
LG2
fork
Fork of LG by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2016-04-12
- Revision:
- 129:406995a91322
- Parent:
- 124:9ae09249f842
- Child:
- 139:1716152517aa
File content as of revision 129:406995a91322:
#include "Global.h" GyroT Gyro; GyroParam GyroP; volatile unsigned int Cheng_AMP_Flag=0; int reper=0; int Rate2VibFlag; 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;//установить в регистре 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 (Cheng_AMP_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); //правая граница амплитуды Cheng_AMP_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); //правая граница амплитуды Cheng_AMP_Flag=0; } srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp } else { PeriodCount++;//таймер амплитуды } LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));//запись в таймер 1 значение частоты вибро } void VibroAMPRegul(void) //подстройка амплитуды ВП { Gyro.CaunPlus = CaunAddPlus; CaunAddPlus = 0; Gyro.CaunMin = CaunAddMin; CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;//расчет максимальной амплитуды из востановленного синуса р-р. Gyro.Amp -= (Gyro.MaxAmp - 5000) * 1; // расчет амплитуды ВП с учетом разници if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100% } void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) { static int TempFaza, CountFaza; TempFaza = -4; for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} if(Gyro.RgConA&0x1) {Gyro.Frq += TempFaza*1000;} /// перепутан вибро 1 вибро 2 } ////////////////////////////////////////////////////////////////////////////// /////////////////////////функция работы вибропривода////////////////////////// ////////////////////////////////////////////////////////////////////////////// void cheng(void) { /* if (CountV255 == 0) Gyro.Rate2_Event=1; if (CountV255 == 127) Gyro.Rate2_Event=1; */ switch(CountV31) { case 0: Time_vibro=0; Gyro.VibroNoiseF=1; break; case 16: Time_vibro=0; Gyro.VibroFrqRegulF=1; Gyro.Rate2_Event=1; break; case 24: Gyro.VibroAMPRegulF=1; break; } }