Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2016-03-23
- Revision:
- 85:0466ee8cdfc8
- Parent:
- 48:8697dfe679b9
- Child:
- 87:7e575d26d6d0
File content as of revision 85:0466ee8cdfc8:
#include "Global.h" GyroT Gyro; //volatile int V1 = 0 ; //volatile int Temp = 0 ; volatile unsigned int Flag=0; unsigned int FrecTemp=0; ///////////////////////////////////////////////////////////////////////////// /////////////////////////инициализация вибропривода////////////////////////// ///////////////////////////////////////////////////////////////////////////// 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.FrqHZ=449; Gyro.Frq = Gyro.FrqHZ<<16; Gyro.AmpPer=20; Gyro.AmpPerDel=10; Gyro.Amp = 10*65535; Gyro.AmpMin =10;// минимальное значение AmpT; Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления) } void VibroOut(void) // выставка ног вибро { if(CountV31>=16) { // 50 75 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; //проверка верхней граници мплитуды } //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; //проверка верхней граници мплитуды } //250 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); //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; } //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 [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));//запись в таймер 1 значение частоты вибро Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); //запись частоты для выдачи в аск_глд старый протокол Time_vibro=0; break; case 10: Gyro.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// if(Gyro.Amp>2500000) { Gyro.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Gyro.AmpPer = Gyro.Amp>>16; MaxAmp=0; break; case 16: 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; break; } }