Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@115:e5a230e5af52, 2016-04-05 (annotated)

Committer:
Kovalev_D
Date:
Tue Apr 05 10:31:28 2016 +0000
Revision:
115:e5a230e5af52
Parent:
114:5cc38a53d8a7
Child:
117:eefe61968528
56

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 85:0466ee8cdfc8 3
Kovalev_D 89:a0d344db227e 4
Kovalev_D 112:4a96133a1311 5 volatile unsigned int Cheng_AMP_Flag=0;
Kovalev_D 89:a0d344db227e 6 void Discharg ()//проверка битового поля поджига и установка значения бита поджига
Kovalev_D 85:0466ee8cdfc8 7 {
Kovalev_D 85:0466ee8cdfc8 8 if (Gyro.Discharg)
Kovalev_D 85:0466ee8cdfc8 9 {
Kovalev_D 89:a0d344db227e 10 //Проверка здвигового поля(последовательности) поджига
Kovalev_D 89:a0d344db227e 11 Gyro.Discharg = Gyro.Discharg >> 1;
Kovalev_D 89:a0d344db227e 12 //если последний бит 1 то установить бит поджига в еденицу, иначе в 0;
Kovalev_D 85:0466ee8cdfc8 13 if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD;
Kovalev_D 85:0466ee8cdfc8 14 }
Kovalev_D 85:0466ee8cdfc8 15 }
Kovalev_D 85:0466ee8cdfc8 16 void BackLight ()
Kovalev_D 85:0466ee8cdfc8 17 {
Kovalev_D 85:0466ee8cdfc8 18 if (Gyro.BackLight)
Kovalev_D 89:a0d344db227e 19 {
Kovalev_D 89:a0d344db227e 20 //Проверка здвигового поля(последовательности) подсветки
Kovalev_D 85:0466ee8cdfc8 21 Gyro.BackLight = Gyro.BackLight >> 1;
Kovalev_D 89:a0d344db227e 22 //если последний бит в битовом поле 1 то установить бит подсветки в еденицу, иначе в 0;
Kovalev_D 85:0466ee8cdfc8 23 if (Gyro.BackLight & 0x01) Gyro.PinReg |= PinRegBitL; else Gyro.PinReg &= ~PinRegBitL;
Kovalev_D 85:0466ee8cdfc8 24 }
Kovalev_D 85:0466ee8cdfc8 25 }
Kovalev_D 85:0466ee8cdfc8 26
Kovalev_D 85:0466ee8cdfc8 27
igor_v 21:bc8c1cec3da6 28
igor_v 0:8ad47e2b6f00 29
igor_v 114:5cc38a53d8a7 30 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 31 {
Kovalev_D 89:a0d344db227e 32 if(CountV31>=16) {//первая нога вибро
Kovalev_D 89:a0d344db227e 33 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
igor_v 30:17c84ed091b3 34 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) {
Kovalev_D 115:e5a230e5af52 35 Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "0"
igor_v 21:bc8c1cec3da6 36 } else {
Kovalev_D 40:8a6494f61326 37 Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 38 }
Kovalev_D 85:0466ee8cdfc8 39
Kovalev_D 89:a0d344db227e 40 } else {//вторая нога вибро
igor_v 30:17c84ed091b3 41 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) {
Kovalev_D 40:8a6494f61326 42 Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0"
igor_v 21:bc8c1cec3da6 43 } else {
Kovalev_D 40:8a6494f61326 44 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 45 }
igor_v 21:bc8c1cec3da6 46 }
igor_v 0:8ad47e2b6f00 47 }
igor_v 0:8ad47e2b6f00 48
igor_v 0:8ad47e2b6f00 49 void CalcAmpN(void)
igor_v 0:8ad47e2b6f00 50 {
Kovalev_D 112:4a96133a1311 51
Kovalev_D 85:0466ee8cdfc8 52 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 53 unsigned int Nmax=0;
igor_v 21:bc8c1cec3da6 54
Kovalev_D 85:0466ee8cdfc8 55 //расчет амплитуды относительно центральной точки
Kovalev_D 85:0466ee8cdfc8 56 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 112:4a96133a1311 57 PeriodCount=0;//сбрасываем таймер
igor_v 21:bc8c1cec3da6 58
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; //проверка верхней граници амплитуды
igor_v 21:bc8c1cec3da6 62 }
Kovalev_D 85:0466ee8cdfc8 63 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); //
Kovalev_D 85:0466ee8cdfc8 64 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400); //левая граница амплитуды
Kovalev_D 85:0466ee8cdfc8 65 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 112:4a96133a1311 66 Cheng_AMP_Flag=1;
Kovalev_D 107:4d178bcc9d8a 67
igor_v 21:bc8c1cec3da6 68 }
igor_v 21:bc8c1cec3da6 69
igor_v 21:bc8c1cec3da6 70 else {
igor_v 30:17c84ed091b3 71 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 112:4a96133a1311 72 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
igor_v 21:bc8c1cec3da6 73 }
Kovalev_D 89:a0d344db227e 74
igor_v 30:17c84ed091b3 75 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 85:0466ee8cdfc8 76 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400);//левая граница амплитуды
Kovalev_D 85:0466ee8cdfc8 77 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 112:4a96133a1311 78 Cheng_AMP_Flag=0;
Kovalev_D 107:4d178bcc9d8a 79
igor_v 21:bc8c1cec3da6 80 }
Kovalev_D 85:0466ee8cdfc8 81 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
igor_v 30:17c84ed091b3 82 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 112:4a96133a1311 83
igor_v 21:bc8c1cec3da6 84 } else {
Kovalev_D 112:4a96133a1311 85 PeriodCount++;//таймер амплитуды
igor_v 21:bc8c1cec3da6 86 }
Kovalev_D 113:8be429494918 87 LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));//запись в таймер 1 значение частоты вибро
Kovalev_D 112:4a96133a1311 88 }
Kovalev_D 112:4a96133a1311 89
Kovalev_D 112:4a96133a1311 90 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 112:4a96133a1311 91 {
Kovalev_D 112:4a96133a1311 92 Gyro.CaunPlus = CaunAddPlus;
Kovalev_D 112:4a96133a1311 93 CaunAddPlus = 0;
Kovalev_D 112:4a96133a1311 94 Gyro.CaunMin = CaunAddMin;
Kovalev_D 112:4a96133a1311 95 CaunAddMin = 0;
Kovalev_D 112:4a96133a1311 96
Kovalev_D 112:4a96133a1311 97 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;//расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 112:4a96133a1311 98 Gyro.Amp -= (Gyro.MaxAmp - 5000) * 1; // расчет амплитуды ВП с учетом разници
Kovalev_D 112:4a96133a1311 99 if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 112:4a96133a1311 100 Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 112:4a96133a1311 101 }
Kovalev_D 112:4a96133a1311 102
Kovalev_D 112:4a96133a1311 103 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 112:4a96133a1311 104 {
Kovalev_D 112:4a96133a1311 105 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 106 TempFaza = -4;
Kovalev_D 112:4a96133a1311 107 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;}
Kovalev_D 112:4a96133a1311 108 if(Gyro.RgConA&0x1) {Gyro.Frq += TempFaza*1000;} /// перепутан вибро 1 вибро 2
igor_v 0:8ad47e2b6f00 109 }
igor_v 0:8ad47e2b6f00 110
igor_v 0:8ad47e2b6f00 111 //////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 112 /////////////////////////функция работы вибропривода//////////////////////////
igor_v 0:8ad47e2b6f00 113 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 114 void cheng(void)
Kovalev_D 99:3d8f206ceac2 115 {
Kovalev_D 107:4d178bcc9d8a 116 switch(CountV31) {
Kovalev_D 112:4a96133a1311 117 case 0:
Kovalev_D 112:4a96133a1311 118 Time_vibro=0;
Kovalev_D 112:4a96133a1311 119 Gyro.VibroNoiseF=1;
Kovalev_D 113:8be429494918 120
Kovalev_D 108:030cdde08314 121
Kovalev_D 112:4a96133a1311 122 break;
Kovalev_D 112:4a96133a1311 123
Kovalev_D 112:4a96133a1311 124 case 16:
Kovalev_D 112:4a96133a1311 125 Time_vibro=0;
Kovalev_D 112:4a96133a1311 126 Gyro.VibroFrqRegulF=1;
Kovalev_D 112:4a96133a1311 127 break;
Kovalev_D 108:030cdde08314 128
Kovalev_D 112:4a96133a1311 129 case 24:
Kovalev_D 112:4a96133a1311 130 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 131 break;
igor_v 21:bc8c1cec3da6 132 }
igor_v 21:bc8c1cec3da6 133 }