Dmitry Kovalev / Mbed 2 deprecated LGstaandart

forkd

Dependencies:   mbed

Fork of LG2 by Dmitry Kovalev

vibro.c@190:289514f730ee, 2016-08-23 (annotated)

Committer:
Kovalev_D
Date:
Tue Aug 23 14:03:16 2016 +0000
Revision:
190:289514f730ee
Parent:
189:8a16378724c4
Child:
191:40028201ddad
PLC

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