Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
vibro.c@189:8a16378724c4, 2016-08-22 (annotated)
- Committer:
- Kovalev_D
- Date:
- Mon Aug 22 10:39:08 2016 +0000
- Revision:
- 189:8a16378724c4
- Parent:
- 188:4c523cc373cc
- Child:
- 190:289514f730ee
?????? ?? ??????????? ??????? ? ????????? ?????
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; |
Kovalev_D | 129:406995a91322 | 3 | GyroParam GyroP; |
Kovalev_D | 89:a0d344db227e | 4 | |
Kovalev_D | 112:4a96133a1311 | 5 | volatile unsigned int Cheng_AMP_Flag=0; |
Kovalev_D | 117:eefe61968528 | 6 | int reper=0; |
Kovalev_D | 122:fbacb932a30b | 7 | int Rate2VibFlag; |
Kovalev_D | 89:a0d344db227e | 8 | void Discharg ()//проверка битового поля поджига и установка значения бита поджига |
Kovalev_D | 85:0466ee8cdfc8 | 9 | { |
Kovalev_D | 85:0466ee8cdfc8 | 10 | if (Gyro.Discharg) |
Kovalev_D | 85:0466ee8cdfc8 | 11 | { |
Kovalev_D | 89:a0d344db227e | 12 | //Проверка здвигового поля(последовательности) поджига |
Kovalev_D | 89:a0d344db227e | 13 | //если последний бит 1 то установить бит поджига в еденицу, иначе в 0; |
Kovalev_D | 85:0466ee8cdfc8 | 14 | if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD; |
Kovalev_D | 188:4c523cc373cc | 15 | Gyro.Discharg = Gyro.Discharg >> 1; |
Kovalev_D | 85:0466ee8cdfc8 | 16 | } |
Kovalev_D | 85:0466ee8cdfc8 | 17 | } |
Kovalev_D | 85:0466ee8cdfc8 | 18 | void BackLight () |
Kovalev_D | 85:0466ee8cdfc8 | 19 | { |
Kovalev_D | 85:0466ee8cdfc8 | 20 | if (Gyro.BackLight) |
Kovalev_D | 89:a0d344db227e | 21 | { |
Kovalev_D | 89:a0d344db227e | 22 | //Проверка здвигового поля(последовательности) подсветки |
Kovalev_D | 89:a0d344db227e | 23 | //если последний бит в битовом поле 1 то установить бит подсветки в еденицу, иначе в 0; |
Kovalev_D | 85:0466ee8cdfc8 | 24 | if (Gyro.BackLight & 0x01) Gyro.PinReg |= PinRegBitL; else Gyro.PinReg &= ~PinRegBitL; |
Kovalev_D | 188:4c523cc373cc | 25 | Gyro.BackLight = Gyro.BackLight >> 1; |
Kovalev_D | 85:0466ee8cdfc8 | 26 | } |
Kovalev_D | 85:0466ee8cdfc8 | 27 | } |
igor_v | 114:5cc38a53d8a7 | 28 | void VibroOut(void) // выставка ног вибро |
igor_v | 0:8ad47e2b6f00 | 29 | { |
Kovalev_D | 89:a0d344db227e | 30 | if(CountV31>=16) {//первая нога вибро |
Kovalev_D | 89:a0d344db227e | 31 | // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1 |
igor_v | 30:17c84ed091b3 | 32 | if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { |
Kovalev_D | 115:e5a230e5af52 | 33 | Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "0" |
igor_v | 21:bc8c1cec3da6 | 34 | } else { |
Kovalev_D | 40:8a6494f61326 | 35 | Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1" |
igor_v | 21:bc8c1cec3da6 | 36 | } |
Kovalev_D | 85:0466ee8cdfc8 | 37 | |
Kovalev_D | 89:a0d344db227e | 38 | } else {//вторая нога вибро |
igor_v | 30:17c84ed091b3 | 39 | if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { |
Kovalev_D | 169:140743e3bb96 | 40 | Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0" |
igor_v | 21:bc8c1cec3da6 | 41 | } else { |
Kovalev_D | 40:8a6494f61326 | 42 | Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1" |
igor_v | 21:bc8c1cec3da6 | 43 | } |
igor_v | 21:bc8c1cec3da6 | 44 | } |
igor_v | 0:8ad47e2b6f00 | 45 | } |
igor_v | 0:8ad47e2b6f00 | 46 | |
igor_v | 0:8ad47e2b6f00 | 47 | void CalcAmpN(void) |
igor_v | 0:8ad47e2b6f00 | 48 | { |
Kovalev_D | 112:4a96133a1311 | 49 | |
Kovalev_D | 85:0466ee8cdfc8 | 50 | static int PeriodCount = 0; |
igor_v | 21:bc8c1cec3da6 | 51 | unsigned int Nmax=0; |
igor_v | 21:bc8c1cec3da6 | 52 | |
Kovalev_D | 85:0466ee8cdfc8 | 53 | //расчет амплитуды относительно центральной точки |
Kovalev_D | 85:0466ee8cdfc8 | 54 | if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. |
Kovalev_D | 189:8a16378724c4 | 55 | PeriodCount=0;//сбрасываем таймер |
Kovalev_D | 189:8a16378724c4 | 56 | |
Kovalev_D | 112:4a96133a1311 | 57 | if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? |
Kovalev_D | 189:8a16378724c4 | 58 | if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды |
Kovalev_D | 189:8a16378724c4 | 59 | else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды |
Kovalev_D | 189:8a16378724c4 | 60 | |
Kovalev_D | 189:8a16378724c4 | 61 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 169:140743e3bb96 | 62 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды |
Kovalev_D | 189:8a16378724c4 | 63 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 189:8a16378724c4 | 64 | |
Kovalev_D | 169:140743e3bb96 | 65 | Cheng_AMP_Flag=1; |
igor_v | 21:bc8c1cec3da6 | 66 | } |
igor_v | 21:bc8c1cec3da6 | 67 | |
igor_v | 21:bc8c1cec3da6 | 68 | else { |
Kovalev_D | 189:8a16378724c4 | 69 | if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды |
Kovalev_D | 189:8a16378724c4 | 70 | else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды |
Kovalev_D | 189:8a16378724c4 | 71 | |
Kovalev_D | 189:8a16378724c4 | 72 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 169:140743e3bb96 | 73 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16));//левая граница амплитуды |
Kovalev_D | 169:140743e3bb96 | 74 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 189:8a16378724c4 | 75 | |
Kovalev_D | 169:140743e3bb96 | 76 | Cheng_AMP_Flag=0; |
igor_v | 21:bc8c1cec3da6 | 77 | } |
Kovalev_D | 85:0466ee8cdfc8 | 78 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
igor_v | 30:17c84ed091b3 | 79 | Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 189:8a16378724c4 | 80 | } |
Kovalev_D | 189:8a16378724c4 | 81 | else {PeriodCount++;} |
Kovalev_D | 169:140743e3bb96 | 82 | |
Kovalev_D | 140:1fbf117fc120 | 83 | switch( Gyro.StrayHZ_flag) { |
Kovalev_D | 169:140743e3bb96 | 84 | case 0: //8046 |
Kovalev_D | 169:140743e3bb96 | 85 | LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 139:1716152517aa | 86 | break; |
Kovalev_D | 139:1716152517aa | 87 | |
Kovalev_D | 139:1716152517aa | 88 | case 1: |
Kovalev_D | 169:140743e3bb96 | 89 | LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка. |
Kovalev_D | 139:1716152517aa | 90 | break; |
Kovalev_D | 139:1716152517aa | 91 | } |
Kovalev_D | 112:4a96133a1311 | 92 | } |
Kovalev_D | 112:4a96133a1311 | 93 | |
Kovalev_D | 112:4a96133a1311 | 94 | void VibroAMPRegul(void) //подстройка амплитуды ВП |
Kovalev_D | 112:4a96133a1311 | 95 | { |
Kovalev_D | 189:8a16378724c4 | 96 | Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin |
Kovalev_D | 112:4a96133a1311 | 97 | CaunAddPlus = 0; |
Kovalev_D | 189:8a16378724c4 | 98 | Gyro.CaunMin = CaunAddMin;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin |
Kovalev_D | 112:4a96133a1311 | 99 | CaunAddMin = 0; |
Kovalev_D | 112:4a96133a1311 | 100 | |
Kovalev_D | 112:4a96133a1311 | 101 | Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;//расчет максимальной амплитуды из востановленного синуса р-р. |
Kovalev_D | 189:8a16378724c4 | 102 | Gyro.Amp -= (Gyro.MaxAmp - 3000) * Gyro.AmpSpeed; // расчет амплитуды ВП с учетом разници |
Kovalev_D | 189:8a16378724c4 | 103 | /* |
Kovalev_D | 189:8a16378724c4 | 104 | if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// |
Kovalev_D | 189:8a16378724c4 | 105 | if((Gyro.Amp>>16) > Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);}*/ // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////*/ |
Kovalev_D | 189:8a16378724c4 | 106 | if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100% |
Kovalev_D | 179:2b4e6bc277df | 107 | |
Kovalev_D | 112:4a96133a1311 | 108 | } |
Kovalev_D | 112:4a96133a1311 | 109 | |
Kovalev_D | 112:4a96133a1311 | 110 | void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) |
Kovalev_D | 112:4a96133a1311 | 111 | { |
Kovalev_D | 112:4a96133a1311 | 112 | static int TempFaza, CountFaza; |
Kovalev_D | 112:4a96133a1311 | 113 | TempFaza = -4; |
Kovalev_D | 112:4a96133a1311 | 114 | for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} |
Kovalev_D | 171:d227a6045305 | 115 | if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*1000;} |
Kovalev_D | 189:8a16378724c4 | 116 | |
Kovalev_D | 189:8a16378724c4 | 117 | if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты |
Kovalev_D | 189:8a16378724c4 | 118 | else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты |
igor_v | 0:8ad47e2b6f00 | 119 | } |
igor_v | 0:8ad47e2b6f00 | 120 | |
igor_v | 0:8ad47e2b6f00 | 121 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 122 | /////////////////////////функция работы вибропривода////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 123 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 124 | void cheng(void) |
Kovalev_D | 99:3d8f206ceac2 | 125 | { |
Kovalev_D | 107:4d178bcc9d8a | 126 | switch(CountV31) { |
Kovalev_D | 112:4a96133a1311 | 127 | case 0: |
Kovalev_D | 117:eefe61968528 | 128 | |
Kovalev_D | 112:4a96133a1311 | 129 | Time_vibro=0; |
Kovalev_D | 112:4a96133a1311 | 130 | Gyro.VibroNoiseF=1; |
Kovalev_D | 112:4a96133a1311 | 131 | break; |
Kovalev_D | 112:4a96133a1311 | 132 | |
Kovalev_D | 112:4a96133a1311 | 133 | case 16: |
Kovalev_D | 112:4a96133a1311 | 134 | Time_vibro=0; |
Kovalev_D | 112:4a96133a1311 | 135 | Gyro.VibroFrqRegulF=1; |
Kovalev_D | 124:9ae09249f842 | 136 | Gyro.Rate2_Event=1; |
Kovalev_D | 112:4a96133a1311 | 137 | break; |
Kovalev_D | 108:030cdde08314 | 138 | |
Kovalev_D | 112:4a96133a1311 | 139 | case 24: |
Kovalev_D | 112:4a96133a1311 | 140 | Gyro.VibroAMPRegulF=1; |
Kovalev_D | 112:4a96133a1311 | 141 | break; |
igor_v | 21:bc8c1cec3da6 | 142 | } |
igor_v | 21:bc8c1cec3da6 | 143 | } |