Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@183:7e200f4d9b16, 2016-07-15 (annotated)

Committer:
Kovalev_D
Date:
Fri Jul 15 14:03:47 2016 +0000
Revision:
183:7e200f4d9b16
Parent:
180:375dcd9c0cb6
Child:
187:6bfb02c2831c
123456

Who changed what in which revision?

UserRevisionLine numberNew 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 Gyro.Discharg = Gyro.Discharg >> 1;
Kovalev_D 89:a0d344db227e 14 //если последний бит 1 то установить бит поджига в еденицу, иначе в 0;
Kovalev_D 85:0466ee8cdfc8 15 if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD;
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 85:0466ee8cdfc8 23 Gyro.BackLight = Gyro.BackLight >> 1;
Kovalev_D 89:a0d344db227e 24 //если последний бит в битовом поле 1 то установить бит подсветки в еденицу, иначе в 0;
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 114:5cc38a53d8a7 31 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 32 {
Kovalev_D 89:a0d344db227e 33 if(CountV31>=16) {//первая нога вибро
Kovalev_D 89:a0d344db227e 34 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
igor_v 30:17c84ed091b3 35 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) {
Kovalev_D 115:e5a230e5af52 36 Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "0"
igor_v 21:bc8c1cec3da6 37 } else {
Kovalev_D 40:8a6494f61326 38 Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 39 }
Kovalev_D 85:0466ee8cdfc8 40
Kovalev_D 89:a0d344db227e 41 } else {//вторая нога вибро
igor_v 30:17c84ed091b3 42 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) {
Kovalev_D 169:140743e3bb96 43 Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0"
igor_v 21:bc8c1cec3da6 44 } else {
Kovalev_D 40:8a6494f61326 45 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 46 }
igor_v 21:bc8c1cec3da6 47 }
igor_v 0:8ad47e2b6f00 48 }
igor_v 0:8ad47e2b6f00 49
igor_v 0:8ad47e2b6f00 50 void CalcAmpN(void)
igor_v 0:8ad47e2b6f00 51 {
Kovalev_D 112:4a96133a1311 52
Kovalev_D 85:0466ee8cdfc8 53 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 54 unsigned int Nmax=0;
igor_v 21:bc8c1cec3da6 55
Kovalev_D 85:0466ee8cdfc8 56 //расчет амплитуды относительно центральной точки
Kovalev_D 85:0466ee8cdfc8 57 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 112:4a96133a1311 58 PeriodCount=0;//сбрасываем таймер
Kovalev_D 112:4a96133a1311 59 if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
Kovalev_D 85:0466ee8cdfc8 60 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 112:4a96133a1311 61 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 183:7e200f4d9b16 62 }
Kovalev_D 183:7e200f4d9b16 63 else if((Gyro.AmpPer+Gyro.AmpPerDel)<18) {
Kovalev_D 183:7e200f4d9b16 64 Gyro.AmpPer=18+Gyro.AmpPerDel; //проверка нижней граници амплитуды
Kovalev_D 183:7e200f4d9b16 65 }
Kovalev_D 169:140743e3bb96 66 // 239
Kovalev_D 169:140743e3bb96 67 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 169:140743e3bb96 68 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 169:140743e3bb96 69 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 169:140743e3bb96 70 Cheng_AMP_Flag=1;
igor_v 21:bc8c1cec3da6 71 }
igor_v 21:bc8c1cec3da6 72
igor_v 21:bc8c1cec3da6 73 else {
igor_v 30:17c84ed091b3 74 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 112:4a96133a1311 75 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
igor_v 21:bc8c1cec3da6 76 }
Kovalev_D 183:7e200f4d9b16 77 else if((Gyro.AmpPer+Gyro.AmpPerDel)<18) {
Kovalev_D 183:7e200f4d9b16 78 Gyro.AmpPer=18+Gyro.AmpPerDel; //проверка нижней граници амплитуды
Kovalev_D 183:7e200f4d9b16 79 }
Kovalev_D 169:140743e3bb96 80 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 169:140743e3bb96 81 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16));//левая граница амплитуды
Kovalev_D 180:375dcd9c0cb6 82 // Gyro.AmpN1=(unsigned int)((Nmax*(100-(Gyro.AmpPer-Gyro.AmpPerDel)))/(Gyro.Frq>>16));//левая граница амплитуды
Kovalev_D 169:140743e3bb96 83 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 169:140743e3bb96 84 Cheng_AMP_Flag=0;
igor_v 21:bc8c1cec3da6 85 }
Kovalev_D 85:0466ee8cdfc8 86 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
igor_v 30:17c84ed091b3 87 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 112:4a96133a1311 88
igor_v 21:bc8c1cec3da6 89 } else {
Kovalev_D 112:4a96133a1311 90 PeriodCount++;//таймер амплитуды
igor_v 21:bc8c1cec3da6 91 }
Kovalev_D 169:140743e3bb96 92
Kovalev_D 140:1fbf117fc120 93 switch( Gyro.StrayHZ_flag) {
Kovalev_D 169:140743e3bb96 94 case 0: //8046
Kovalev_D 169:140743e3bb96 95 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 139:1716152517aa 96 break;
Kovalev_D 139:1716152517aa 97
Kovalev_D 139:1716152517aa 98 case 1:
Kovalev_D 169:140743e3bb96 99 LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
Kovalev_D 139:1716152517aa 100 break;
Kovalev_D 139:1716152517aa 101 }
Kovalev_D 112:4a96133a1311 102 }
Kovalev_D 112:4a96133a1311 103
Kovalev_D 112:4a96133a1311 104 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 112:4a96133a1311 105 {
Kovalev_D 112:4a96133a1311 106 Gyro.CaunPlus = CaunAddPlus;
Kovalev_D 112:4a96133a1311 107 CaunAddPlus = 0;
Kovalev_D 112:4a96133a1311 108 Gyro.CaunMin = CaunAddMin;
Kovalev_D 112:4a96133a1311 109 CaunAddMin = 0;
Kovalev_D 112:4a96133a1311 110
Kovalev_D 112:4a96133a1311 111 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;//расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 183:7e200f4d9b16 112 Gyro.Amp -= (Gyro.MaxAmp - 4000) * 1; // расчет амплитуды ВП с учетом разници
Kovalev_D 179:2b4e6bc277df 113
Kovalev_D 112:4a96133a1311 114 if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 179:2b4e6bc277df 115
Kovalev_D 171:d227a6045305 116 if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 112:4a96133a1311 117 }
Kovalev_D 112:4a96133a1311 118
Kovalev_D 112:4a96133a1311 119 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 112:4a96133a1311 120 {
Kovalev_D 112:4a96133a1311 121 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 122 TempFaza = -4;
Kovalev_D 112:4a96133a1311 123 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;}
Kovalev_D 171:d227a6045305 124 if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*1000;}
igor_v 0:8ad47e2b6f00 125 }
igor_v 0:8ad47e2b6f00 126
igor_v 0:8ad47e2b6f00 127 //////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 128 /////////////////////////функция работы вибропривода//////////////////////////
igor_v 0:8ad47e2b6f00 129 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 130 void cheng(void)
Kovalev_D 99:3d8f206ceac2 131 {
Kovalev_D 124:9ae09249f842 132
Kovalev_D 124:9ae09249f842 133 /*
Kovalev_D 124:9ae09249f842 134 if (CountV255 == 0) Gyro.Rate2_Event=1;
Kovalev_D 124:9ae09249f842 135 if (CountV255 == 127) Gyro.Rate2_Event=1;
Kovalev_D 124:9ae09249f842 136 */
Kovalev_D 107:4d178bcc9d8a 137 switch(CountV31) {
Kovalev_D 112:4a96133a1311 138 case 0:
Kovalev_D 117:eefe61968528 139
Kovalev_D 112:4a96133a1311 140 Time_vibro=0;
Kovalev_D 112:4a96133a1311 141 Gyro.VibroNoiseF=1;
Kovalev_D 112:4a96133a1311 142 break;
Kovalev_D 112:4a96133a1311 143
Kovalev_D 112:4a96133a1311 144 case 16:
Kovalev_D 112:4a96133a1311 145 Time_vibro=0;
Kovalev_D 112:4a96133a1311 146 Gyro.VibroFrqRegulF=1;
Kovalev_D 124:9ae09249f842 147 Gyro.Rate2_Event=1;
Kovalev_D 112:4a96133a1311 148 break;
Kovalev_D 108:030cdde08314 149
Kovalev_D 112:4a96133a1311 150 case 24:
Kovalev_D 112:4a96133a1311 151 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 152 break;
igor_v 21:bc8c1cec3da6 153 }
igor_v 21:bc8c1cec3da6 154 }