Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

vibro.c@232:130a2b5003e6, 2018-01-31 (annotated)

Committer:
Kovalev_D
Date:
Wed Jan 31 13:41:23 2018 +0000
Revision:
232:130a2b5003e6
Parent:
231:079835d508ef 
static termocorr

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 231:079835d508ef 4
Kovalev_D 231:079835d508ef 5 TermCorS TermoCorrStatic;
Kovalev_D 231:079835d508ef 6 TermCorD TermoCorrDynamic;
Kovalev_D 231:079835d508ef 7
Kovalev_D 231:079835d508ef 8
Kovalev_D 112:4a96133a1311 9 volatile unsigned int Cheng_AMP_Flag=0;
Kovalev_D 190:289514f730ee 10 //int reper=0;
Kovalev_D 193:a0fe8bfc97e4 11 int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo;
Kovalev_D 209:224e7331a061 12 unsigned int OldMaxAmp=0,countFras=0;
Kovalev_D 207:d1ce992f5d17 13 int z=25;
Kovalev_D 208:19150d2b528f 14 int i=16,tempi=0,klk=0;
Kovalev_D 226:4a4d5bd5fcd7 15
Kovalev_D 226:4a4d5bd5fcd7 16
Kovalev_D 193:a0fe8bfc97e4 17 __irq void EINT3_IRQHandler()
Kovalev_D 208:19150d2b528f 18 {
Kovalev_D 232:130a2b5003e6 19 if( Gyro.EXT_Latch){}
Kovalev_D 232:130a2b5003e6 20 else Gyro.EXT_Latch=1;
Kovalev_D 232:130a2b5003e6 21 LPC_GPIOINT->IO0IntClr |= (1<<1);
Kovalev_D 232:130a2b5003e6 22 //NVIC_DisableIRQ(EINT3_IRQn);
Kovalev_D 226:4a4d5bd5fcd7 23
Kovalev_D 193:a0fe8bfc97e4 24 }
Kovalev_D 193:a0fe8bfc97e4 25
Kovalev_D 193:a0fe8bfc97e4 26
igor_v 114:5cc38a53d8a7 27 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 28 {
Kovalev_D 190:289514f730ee 29 if(CountV31>=16)
Kovalev_D 190:289514f730ee 30 {//первая нога вибро
Kovalev_D 89:a0d344db227e 31 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
Kovalev_D 190:289514f730ee 32 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 33 {
Kovalev_D 203:3a6615de9581 34 SetV1//установить в регистре PinReg бит "вибро 1" в "0"
Kovalev_D 190:289514f730ee 35 }
Kovalev_D 190:289514f730ee 36 else
Kovalev_D 190:289514f730ee 37 {
Kovalev_D 207:d1ce992f5d17 38 ClrV1 //установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 39 }
Kovalev_D 85:0466ee8cdfc8 40
Kovalev_D 190:289514f730ee 41 }
Kovalev_D 190:289514f730ee 42 else {//вторая нога вибро
Kovalev_D 190:289514f730ee 43 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 44 {
Kovalev_D 207:d1ce992f5d17 45 SetV2 //установить в регистре PinReg бит "вибро 2" в "0"
Kovalev_D 190:289514f730ee 46 }
Kovalev_D 190:289514f730ee 47 else
Kovalev_D 190:289514f730ee 48 {
Kovalev_D 203:3a6615de9581 49 ClrV2//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 50 }
igor_v 21:bc8c1cec3da6 51 }
igor_v 0:8ad47e2b6f00 52 }
Kovalev_D 214:4c70e452c491 53 /*
Kovalev_D 193:a0fe8bfc97e4 54
Kovalev_D 193:a0fe8bfc97e4 55 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 207:d1ce992f5d17 56 {
Kovalev_D 207:d1ce992f5d17 57 static int PeriodCount = 0,Period=0;
Kovalev_D 210:b02fa166315d 58 unsigned int Nmax=0, lowper=1;
Kovalev_D 209:224e7331a061 59 Gyro.FrqHZ=Gyro.Frq>>16;
Kovalev_D 209:224e7331a061 60 if(PeriodCount>= Gyro.AmpT)
Kovalev_D 209:224e7331a061 61 { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 209:224e7331a061 62 PeriodCount=0;//сбрасываем таймер
Kovalev_D 214:4c70e452c491 63 sprintf((Time),"%d %d %d\r\n", Gyro.AmpN1, Gyro.AmpN2-Gyro.AmpN1, Gyro.AmpPer);
Kovalev_D 214:4c70e452c491 64 WriteCon(Time);
Kovalev_D 211:ac8251b067d2 65 //Gyro.AmpPerDel=(Gyro.AmpPer*100)/Gyro.AmpPerDel;
Kovalev_D 209:224e7331a061 66 if(Cheng_AMP_Flag==0)
Kovalev_D 209:224e7331a061 67 {
Kovalev_D 214:4c70e452c491 68 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 210:b02fa166315d 69 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);//256 //
Kovalev_D 210:b02fa166315d 70 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды 63
Kovalev_D 210:b02fa166315d 71 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 65
Kovalev_D 210:b02fa166315d 72 Gyro.L_vibro=Gyro.AmpPer*208;
Kovalev_D 214:4c70e452c491 73 sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag);
Kovalev_D 214:4c70e452c491 74 WriteCon(Time);
Kovalev_D 209:224e7331a061 75 Cheng_AMP_Flag=1;
Kovalev_D 209:224e7331a061 76 }
Kovalev_D 210:b02fa166315d 77 else
Kovalev_D 210:b02fa166315d 78 {
Kovalev_D 210:b02fa166315d 79 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 210:b02fa166315d 80 if((Gyro.RgConA&0x20))
Kovalev_D 210:b02fa166315d 81 {
Kovalev_D 210:b02fa166315d 82 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);//256
Kovalev_D 210:b02fa166315d 83 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/Gyro.FrqHZ); //левая граница амплитуды 61
Kovalev_D 210:b02fa166315d 84 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 67
Kovalev_D 210:b02fa166315d 85 Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*208;
Kovalev_D 214:4c70e452c491 86 sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag);
Kovalev_D 214:4c70e452c491 87 WriteCon(Time);
Kovalev_D 210:b02fa166315d 88 }
Kovalev_D 210:b02fa166315d 89 Cheng_AMP_Flag=0;
Kovalev_D 210:b02fa166315d 90 }
Kovalev_D 210:b02fa166315d 91 if(Gyro.AmpN2<(Gyro.AmpN1+2))Gyro.AmpN2=Gyro.AmpN1+2;
Kovalev_D 210:b02fa166315d 92
Kovalev_D 207:d1ce992f5d17 93 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 210:b02fa166315d 94 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 95 } else {
Kovalev_D 193:a0fe8bfc97e4 96 PeriodCount++;//таймер амплитуды
Kovalev_D 193:a0fe8bfc97e4 97 }
Kovalev_D 214:4c70e452c491 98 }*/
Kovalev_D 214:4c70e452c491 99 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 214:4c70e452c491 100 {
Kovalev_D 214:4c70e452c491 101 static int PeriodCount = 0,Period=0;
Kovalev_D 214:4c70e452c491 102 unsigned int Nmax=0, lowper=1;
Kovalev_D 214:4c70e452c491 103 Gyro.FrqHZ=Gyro.Frq>>16;
Kovalev_D 214:4c70e452c491 104 if(PeriodCount>= Gyro.AmpT)
Kovalev_D 214:4c70e452c491 105 {
Kovalev_D 214:4c70e452c491 106 PeriodCount=0;//сбрасываем таймер
Kovalev_D 214:4c70e452c491 107 if(Cheng_AMP_Flag==0)
Kovalev_D 214:4c70e452c491 108 {
Kovalev_D 214:4c70e452c491 109 //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды
Kovalev_D 214:4c70e452c491 110 T_vib_1 = Gyro.AmpPer * T_vibP;
Kovalev_D 214:4c70e452c491 111 T_vib_2 = T_vibP * (10000-Gyro.AmpPer);
Kovalev_D 214:4c70e452c491 112 Gyro.L_vibro=Gyro.AmpPer*3;
Kovalev_D 214:4c70e452c491 113 Cheng_AMP_Flag=1;
Kovalev_D 214:4c70e452c491 114 }
Kovalev_D 214:4c70e452c491 115 else
Kovalev_D 214:4c70e452c491 116 {
Kovalev_D 214:4c70e452c491 117 //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды
Kovalev_D 214:4c70e452c491 118 if((Gyro.RgConA&0x20))
Kovalev_D 214:4c70e452c491 119 {
Kovalev_D 214:4c70e452c491 120 T_vib_1 = T_vibP * (Gyro.AmpPer +(Gyro.AmpPerDel*100));
Kovalev_D 214:4c70e452c491 121 T_vib_2 = T_vibP * (10000 -(Gyro.AmpPer+(Gyro.AmpPerDel*100)));
Kovalev_D 214:4c70e452c491 122 Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*3;
Kovalev_D 214:4c70e452c491 123 }
Kovalev_D 214:4c70e452c491 124 Cheng_AMP_Flag=0;
Kovalev_D 214:4c70e452c491 125 }
Kovalev_D 214:4c70e452c491 126
Kovalev_D 214:4c70e452c491 127 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 214:4c70e452c491 128 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 214:4c70e452c491 129 }
Kovalev_D 214:4c70e452c491 130 else {PeriodCount++;}
Kovalev_D 208:19150d2b528f 131
Kovalev_D 192:d32c8cf7bcd9 132 }
Kovalev_D 207:d1ce992f5d17 133
Kovalev_D 112:4a96133a1311 134 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 208:19150d2b528f 135 {
Kovalev_D 209:224e7331a061 136
Kovalev_D 208:19150d2b528f 137 int temp=0;
Kovalev_D 218:b4067cac75c0 138 static unsigned int FConunt=0;
Kovalev_D 211:ac8251b067d2 139 int LowDZ,HiDZ;
Kovalev_D 218:b4067cac75c0 140 /* if(FConunt<4)
Kovalev_D 218:b4067cac75c0 141 {*/
Kovalev_D 218:b4067cac75c0 142 //FConunt++;
Kovalev_D 189:8a16378724c4 143 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 218:b4067cac75c0 144 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 145 CaunAddPlus = 0;
Kovalev_D 112:4a96133a1311 146 CaunAddMin = 0;
Kovalev_D 208:19150d2b528f 147 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;
Kovalev_D 218:b4067cac75c0 148 // }
Kovalev_D 218:b4067cac75c0 149 /*else
Kovalev_D 218:b4067cac75c0 150 {*/
Kovalev_D 218:b4067cac75c0 151 // FConunt=0;
Kovalev_D 218:b4067cac75c0 152 //Gyro.MaxAmp=Gyro.MaxAmp>>2;
Kovalev_D 218:b4067cac75c0 153 if(countFras<512)
Kovalev_D 218:b4067cac75c0 154 {
Kovalev_D 218:b4067cac75c0 155 countFras++;
Kovalev_D 218:b4067cac75c0 156 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
Kovalev_D 218:b4067cac75c0 157 }
Kovalev_D 218:b4067cac75c0 158 else
Kovalev_D 218:b4067cac75c0 159 {
Kovalev_D 218:b4067cac75c0 160 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
Kovalev_D 218:b4067cac75c0 161 Gyro.F_ras=Gyro.F_rasAdd>>9;
Kovalev_D 218:b4067cac75c0 162 Gyro.F_rasAdd=0;
Kovalev_D 218:b4067cac75c0 163 countFras=0;
Kovalev_D 218:b4067cac75c0 164 }
Kovalev_D 218:b4067cac75c0 165 if(Gyro.RgConA&0x20)
Kovalev_D 218:b4067cac75c0 166 {
Kovalev_D 209:224e7331a061 167 //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 218:b4067cac75c0 168 temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed));
Kovalev_D 218:b4067cac75c0 169 temp=temp>>6;
Kovalev_D 218:b4067cac75c0 170 LowDZ = ((Gyro.AmpSpeed<<3)*(-1));
Kovalev_D 218:b4067cac75c0 171 HiDZ = (Gyro.AmpSpeed<<3);
Kovalev_D 227:2774b56bfab0 172 Gyro.Amp -= temp>>4; // расчет амплитуды ВП с учетом разници(Gyro.AmpPer<<17)/100;
Kovalev_D 227:2774b56bfab0 173 if((Gyro.AmpPer) > (Gyro.AmpPerMax)) {Gyro.Amp = ((Gyro.AmpPerMax<<17)/100);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 227:2774b56bfab0 174 if((Gyro.AmpPer) < (Gyro.AmpPerMin)) {Gyro.Amp = ((Gyro.AmpPerMin<<17)/100);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 214:4c70e452c491 175 // Gyro.AmpPer = (Gyro.Amp)>>16; //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 218:b4067cac75c0 176 }
Kovalev_D 218:b4067cac75c0 177 LPC_MCPWM->MAT1 = T_vib_1;
Kovalev_D 218:b4067cac75c0 178 LPC_MCPWM->MAT2 = T_vib_2;
Kovalev_D 218:b4067cac75c0 179 // }
Kovalev_D 218:b4067cac75c0 180 }
Kovalev_D 112:4a96133a1311 181
Kovalev_D 191:40028201ddad 182
Kovalev_D 191:40028201ddad 183
Kovalev_D 112:4a96133a1311 184 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 196:f76dbc081e63 185 {
Kovalev_D 112:4a96133a1311 186 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 187 TempFaza = -4;
Kovalev_D 209:224e7331a061 188 Gyro.FrqPhaseEror=0;
Kovalev_D 209:224e7331a061 189 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0
Kovalev_D 209:224e7331a061 190 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f];}
Kovalev_D 208:19150d2b528f 191 if(Gyro.RgConA&0x40)
Kovalev_D 208:19150d2b528f 192 { //12
Kovalev_D 209:224e7331a061 193 Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;
Kovalev_D 208:19150d2b528f 194 }
Kovalev_D 209:224e7331a061 195 // Gyro.FrqPhaseEror = TempFaza<<10;
Kovalev_D 189:8a16378724c4 196 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 214:4c70e452c491 197 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты*/
Kovalev_D 214:4c70e452c491 198 LPC_TIM1->MR0 =(unsigned int)(103200000/(Gyro.Frq>>11));//запись в таймер нового значение частоты вибро
igor_v 0:8ad47e2b6f00 199 }
igor_v 0:8ad47e2b6f00 200
igor_v 0:8ad47e2b6f00 201 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 202 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 203 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 204 void cheng(void)
Kovalev_D 231:079835d508ef 205 {
Kovalev_D 231:079835d508ef 206 switch(CountV31) {
Kovalev_D 112:4a96133a1311 207 case 0:
Kovalev_D 214:4c70e452c491 208 ReVib();///обновление значений вибро
Kovalev_D 196:f76dbc081e63 209 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 210 Time_vibro=0;
Kovalev_D 215:b58b887fd367 211 Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 112:4a96133a1311 212 break;
Kovalev_D 214:4c70e452c491 213 case 8:
Kovalev_D 214:4c70e452c491 214 LPC_MCPWM->CON_CLR |= (1<<8);
Kovalev_D 214:4c70e452c491 215 LPC_MCPWM->CON_CLR |= (1<<16);
Kovalev_D 214:4c70e452c491 216
Kovalev_D 214:4c70e452c491 217 LPC_MCPWM->TC1 =1;
Kovalev_D 214:4c70e452c491 218 LPC_MCPWM->TC2 =1;
Kovalev_D 214:4c70e452c491 219
Kovalev_D 215:b58b887fd367 220 LPC_MCPWM->CON_SET |= (1<<8);
Kovalev_D 214:4c70e452c491 221 LPC_MCPWM->CON_SET |= (1<<16); //вкл
Kovalev_D 214:4c70e452c491 222 break;
Kovalev_D 214:4c70e452c491 223
Kovalev_D 112:4a96133a1311 224 case 16:
Kovalev_D 215:b58b887fd367 225 Time_vibro=0;
Kovalev_D 215:b58b887fd367 226 Gyro.VibroFrqRegulF=1; //
Kovalev_D 112:4a96133a1311 227 break;
Kovalev_D 214:4c70e452c491 228
Kovalev_D 215:b58b887fd367 229 case 31:
Kovalev_D 231:079835d508ef 230
Kovalev_D 214:4c70e452c491 231 break;
Kovalev_D 207:d1ce992f5d17 232 }
Kovalev_D 191:40028201ddad 233 }
Kovalev_D 191:40028201ddad 234 void AllRegul (void)
Kovalev_D 191:40028201ddad 235 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 236
Kovalev_D 191:40028201ddad 237 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 238 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 191:40028201ddad 239 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 209:224e7331a061 240 if (Gyro.VibroAMPRegulF == 1)
Kovalev_D 209:224e7331a061 241 {
Kovalev_D 209:224e7331a061 242 Gyro.VibroAMPRegulF = 0;
Kovalev_D 209:224e7331a061 243 VibroAMPRegul();
Kovalev_D 215:b58b887fd367 244 PLCRegul();
Kovalev_D 231:079835d508ef 245 HFORegul();
Kovalev_D 209:224e7331a061 246 } // Регулеровка Амплитуды виброподвеса
Kovalev_D 209:224e7331a061 247 if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();}
Kovalev_D 196:f76dbc081e63 248 }