Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
vibro.c@87:7e575d26d6d0, 2016-03-24 (annotated)
- Committer:
- Kovalev_D
- Date:
- Thu Mar 24 09:16:55 2016 +0000
- Revision:
- 87:7e575d26d6d0
- Parent:
- 85:0466ee8cdfc8
- Child:
- 89:a0d344db227e
GLD_Init
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 0:8ad47e2b6f00 | 1 | #include "Global.h" |
igor_v | 30:17c84ed091b3 | 2 | GyroT Gyro; |
igor_v | 48:8697dfe679b9 | 3 | //volatile int V1 = 0 ; |
igor_v | 48:8697dfe679b9 | 4 | //volatile int Temp = 0 ; |
igor_v | 0:8ad47e2b6f00 | 5 | volatile unsigned int Flag=0; |
igor_v | 0:8ad47e2b6f00 | 6 | unsigned int FrecTemp=0; |
igor_v | 0:8ad47e2b6f00 | 7 | ///////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 8 | /////////////////////////инициализация вибропривода////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 9 | ///////////////////////////////////////////////////////////////////////////// |
Kovalev_D | 85:0466ee8cdfc8 | 10 | |
Kovalev_D | 85:0466ee8cdfc8 | 11 | void Discharg () |
Kovalev_D | 85:0466ee8cdfc8 | 12 | { |
Kovalev_D | 85:0466ee8cdfc8 | 13 | if (Gyro.Discharg) |
Kovalev_D | 85:0466ee8cdfc8 | 14 | { |
Kovalev_D | 85:0466ee8cdfc8 | 15 | //Проверка поджига |
Kovalev_D | 85:0466ee8cdfc8 | 16 | Gyro.Discharg = Gyro.Discharg >> 1; |
Kovalev_D | 85:0466ee8cdfc8 | 17 | if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD; |
Kovalev_D | 85:0466ee8cdfc8 | 18 | } |
Kovalev_D | 85:0466ee8cdfc8 | 19 | } |
Kovalev_D | 85:0466ee8cdfc8 | 20 | void BackLight () |
Kovalev_D | 85:0466ee8cdfc8 | 21 | { |
Kovalev_D | 85:0466ee8cdfc8 | 22 | if (Gyro.BackLight) |
Kovalev_D | 85:0466ee8cdfc8 | 23 | { //Проверка подсветки |
Kovalev_D | 85:0466ee8cdfc8 | 24 | Gyro.BackLight = Gyro.BackLight >> 1; |
Kovalev_D | 85:0466ee8cdfc8 | 25 | if (Gyro.BackLight & 0x01) Gyro.PinReg |= PinRegBitL; else Gyro.PinReg &= ~PinRegBitL; |
Kovalev_D | 85:0466ee8cdfc8 | 26 | } |
Kovalev_D | 85:0466ee8cdfc8 | 27 | } |
Kovalev_D | 85:0466ee8cdfc8 | 28 | |
Kovalev_D | 85:0466ee8cdfc8 | 29 | |
igor_v | 21:bc8c1cec3da6 | 30 | |
igor_v | 0:8ad47e2b6f00 | 31 | |
Kovalev_D | 85:0466ee8cdfc8 | 32 | void VibroOut(void) // выставка ног вибро |
igor_v | 0:8ad47e2b6f00 | 33 | { |
Kovalev_D | 85:0466ee8cdfc8 | 34 | if(CountV31>=16) { |
Kovalev_D | 85:0466ee8cdfc8 | 35 | // 50 75 |
igor_v | 30:17c84ed091b3 | 36 | if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { |
Kovalev_D | 85:0466ee8cdfc8 | 37 | Gyro.PinReg &= ~PinRegBit_1V;/*LoopOn*///установить в регистре PinReg бит "вибро 1" в "0" |
igor_v | 21:bc8c1cec3da6 | 38 | } else { |
Kovalev_D | 40:8a6494f61326 | 39 | Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1" |
igor_v | 21:bc8c1cec3da6 | 40 | } |
Kovalev_D | 85:0466ee8cdfc8 | 41 | |
igor_v | 21:bc8c1cec3da6 | 42 | } else { |
igor_v | 30:17c84ed091b3 | 43 | if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { |
Kovalev_D | 40:8a6494f61326 | 44 | Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0" |
igor_v | 21:bc8c1cec3da6 | 45 | } else { |
Kovalev_D | 40:8a6494f61326 | 46 | Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1" |
igor_v | 21:bc8c1cec3da6 | 47 | } |
igor_v | 21:bc8c1cec3da6 | 48 | } |
igor_v | 0:8ad47e2b6f00 | 49 | |
igor_v | 0:8ad47e2b6f00 | 50 | |
igor_v | 0:8ad47e2b6f00 | 51 | |
igor_v | 0:8ad47e2b6f00 | 52 | } |
igor_v | 0:8ad47e2b6f00 | 53 | |
igor_v | 0:8ad47e2b6f00 | 54 | void CalcAmpN(void) |
igor_v | 0:8ad47e2b6f00 | 55 | { |
Kovalev_D | 85:0466ee8cdfc8 | 56 | static int PeriodCount = 0; |
igor_v | 21:bc8c1cec3da6 | 57 | unsigned int Nmax=0; |
igor_v | 21:bc8c1cec3da6 | 58 | |
Kovalev_D | 85:0466ee8cdfc8 | 59 | //расчет амплитуды относительно центральной точки |
igor_v | 21:bc8c1cec3da6 | 60 | |
Kovalev_D | 85:0466ee8cdfc8 | 61 | if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. |
Kovalev_D | 85:0466ee8cdfc8 | 62 | PeriodCount=0; |
igor_v | 21:bc8c1cec3da6 | 63 | |
Kovalev_D | 85:0466ee8cdfc8 | 64 | if (Flag==0) { //сейчас малая амплитуда? |
Kovalev_D | 85:0466ee8cdfc8 | 65 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { |
Kovalev_D | 85:0466ee8cdfc8 | 66 | Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды |
igor_v | 21:bc8c1cec3da6 | 67 | } |
Kovalev_D | 85:0466ee8cdfc8 | 68 | //250 100000 / 399 |
Kovalev_D | 85:0466ee8cdfc8 | 69 | Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); // |
Kovalev_D | 85:0466ee8cdfc8 | 70 | // 50 (( 250 * (100-20)) /400) // |
Kovalev_D | 85:0466ee8cdfc8 | 71 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400); //левая граница амплитуды |
Kovalev_D | 85:0466ee8cdfc8 | 72 | // 75 ((250 /2)-50) |
Kovalev_D | 85:0466ee8cdfc8 | 73 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
igor_v | 21:bc8c1cec3da6 | 74 | Flag=1; |
igor_v | 21:bc8c1cec3da6 | 75 | |
igor_v | 21:bc8c1cec3da6 | 76 | } |
igor_v | 21:bc8c1cec3da6 | 77 | |
igor_v | 21:bc8c1cec3da6 | 78 | else { |
igor_v | 30:17c84ed091b3 | 79 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { |
Kovalev_D | 85:0466ee8cdfc8 | 80 | Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды |
igor_v | 21:bc8c1cec3da6 | 81 | } |
Kovalev_D | 85:0466ee8cdfc8 | 82 | //250 |
igor_v | 30:17c84ed091b3 | 83 | Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 85:0466ee8cdfc8 | 84 | //56 ((250*(100-20+10))/400) |
Kovalev_D | 85:0466ee8cdfc8 | 85 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400);//левая граница амплитуды |
Kovalev_D | 85:0466ee8cdfc8 | 86 | //69 ((250/2)-56 |
Kovalev_D | 85:0466ee8cdfc8 | 87 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
igor_v | 21:bc8c1cec3da6 | 88 | Flag=0; |
igor_v | 21:bc8c1cec3da6 | 89 | } |
Kovalev_D | 85:0466ee8cdfc8 | 90 | |
igor_v | 21:bc8c1cec3da6 | 91 | //Tnoise=55; |
Kovalev_D | 85:0466ee8cdfc8 | 92 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 85:0466ee8cdfc8 | 93 | |
igor_v | 30:17c84ed091b3 | 94 | Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
igor_v | 21:bc8c1cec3da6 | 95 | //изменение флага предидущей амплитуды |
Kovalev_D | 85:0466ee8cdfc8 | 96 | |
igor_v | 21:bc8c1cec3da6 | 97 | } else { |
igor_v | 21:bc8c1cec3da6 | 98 | PeriodCount++; |
igor_v | 21:bc8c1cec3da6 | 99 | } |
igor_v | 0:8ad47e2b6f00 | 100 | } |
igor_v | 0:8ad47e2b6f00 | 101 | |
igor_v | 0:8ad47e2b6f00 | 102 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 103 | /////////////////////////функция работы вибропривода////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 104 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 105 | void cheng(void) |
igor_v | 21:bc8c1cec3da6 | 106 | { |
igor_v | 21:bc8c1cec3da6 | 107 | static int TempFaza, CountFaza; |
Kovalev_D | 85:0466ee8cdfc8 | 108 | if (Buff_Restored_sin [CountV255] > MaxAmp) {MaxAmp=Buff_Restored_sin [CountV255];} |
Kovalev_D | 85:0466ee8cdfc8 | 109 | switch(CountV31) { |
igor_v | 21:bc8c1cec3da6 | 110 | case 0: |
Kovalev_D | 85:0466ee8cdfc8 | 111 | CalcAmpN();//расчет ошумления и амплитуды задание интервалов выставки ножек вибро. |
Kovalev_D | 85:0466ee8cdfc8 | 112 | //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; //прием частоты из аск_глд старый протокол |
Kovalev_D | 85:0466ee8cdfc8 | 113 | LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));//запись в таймер 1 значение частоты вибро |
Kovalev_D | 85:0466ee8cdfc8 | 114 | Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); //запись частоты для выдачи в аск_глд старый протокол |
igor_v | 21:bc8c1cec3da6 | 115 | Time_vibro=0; |
igor_v | 21:bc8c1cec3da6 | 116 | break; |
igor_v | 21:bc8c1cec3da6 | 117 | case 10: |
igor_v | 30:17c84ed091b3 | 118 | Gyro.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници |
igor_v | 21:bc8c1cec3da6 | 119 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 30:17c84ed091b3 | 120 | if(Gyro.Amp>2500000) { |
igor_v | 30:17c84ed091b3 | 121 | Gyro.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// |
igor_v | 21:bc8c1cec3da6 | 122 | } |
igor_v | 21:bc8c1cec3da6 | 123 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 124 | |
igor_v | 30:17c84ed091b3 | 125 | Gyro.AmpPer = Gyro.Amp>>16; |
igor_v | 21:bc8c1cec3da6 | 126 | MaxAmp=0; |
igor_v | 21:bc8c1cec3da6 | 127 | break; |
Kovalev_D | 85:0466ee8cdfc8 | 128 | |
igor_v | 21:bc8c1cec3da6 | 129 | case 16: |
igor_v | 21:bc8c1cec3da6 | 130 | Time_vibro=0; |
igor_v | 21:bc8c1cec3da6 | 131 | |
igor_v | 21:bc8c1cec3da6 | 132 | // расчет Фазы с учетор разници |
igor_v | 21:bc8c1cec3da6 | 133 | TempFaza = -4; |
igor_v | 21:bc8c1cec3da6 | 134 | for (CountFaza = 0; CountFaza < 8; CountFaza++ ) { |
igor_v | 21:bc8c1cec3da6 | 135 | if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++; |
igor_v | 21:bc8c1cec3da6 | 136 | } // расчет Фазы с учетор разници |
igor_v | 30:17c84ed091b3 | 137 | Gyro.Frq -=TempFaza*1000; |
igor_v | 21:bc8c1cec3da6 | 138 | break; |
igor_v | 21:bc8c1cec3da6 | 139 | } |
igor_v | 21:bc8c1cec3da6 | 140 | |
igor_v | 21:bc8c1cec3da6 | 141 | } |