Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@196:f76dbc081e63, 2016-09-23 (annotated)

Committer:
Kovalev_D
Date:
Fri Sep 23 05:34:50 2016 +0000
Revision:
196:f76dbc081e63
Parent:
194:8f3cb37a5541
Child:
197:7a05523bf588
???

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 112:4a96133a1311 4 volatile unsigned int Cheng_AMP_Flag=0;
Kovalev_D 190:289514f730ee 5 //int reper=0;
Kovalev_D 193:a0fe8bfc97e4 6 int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo;
Kovalev_D 191:40028201ddad 7 unsigned int OldMaxAmp=0;
Kovalev_D 193:a0fe8bfc97e4 8
Kovalev_D 193:a0fe8bfc97e4 9
Kovalev_D 193:a0fe8bfc97e4 10 __irq void EINT3_IRQHandler()
Kovalev_D 193:a0fe8bfc97e4 11 {
Kovalev_D 193:a0fe8bfc97e4 12 Gyro.DeltaEXT_Event=1;
Kovalev_D 193:a0fe8bfc97e4 13 Gyro.B_Delta_EventEXT=1;
Kovalev_D 193:a0fe8bfc97e4 14 LPC_GPIOINT->IO0IntClr |= (1<<1);
Kovalev_D 193:a0fe8bfc97e4 15 // CMD_Delta_Bins();
Kovalev_D 193:a0fe8bfc97e4 16 }
Kovalev_D 193:a0fe8bfc97e4 17
Kovalev_D 193:a0fe8bfc97e4 18
igor_v 114:5cc38a53d8a7 19 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 20 {
Kovalev_D 190:289514f730ee 21 if(CountV31>=16)
Kovalev_D 190:289514f730ee 22 {//первая нога вибро
Kovalev_D 89:a0d344db227e 23 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
Kovalev_D 190:289514f730ee 24 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 25 {
Kovalev_D 115:e5a230e5af52 26 Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "0"
Kovalev_D 190:289514f730ee 27 }
Kovalev_D 190:289514f730ee 28 else
Kovalev_D 190:289514f730ee 29 {
Kovalev_D 40:8a6494f61326 30 Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 31 }
Kovalev_D 85:0466ee8cdfc8 32
Kovalev_D 190:289514f730ee 33 }
Kovalev_D 190:289514f730ee 34 else {//вторая нога вибро
Kovalev_D 190:289514f730ee 35 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 36 {
Kovalev_D 190:289514f730ee 37 Gyro.PinReg &= ~PinRegBit_2V; //установить в регистре PinReg бит "вибро 2" в "0"
Kovalev_D 190:289514f730ee 38 }
Kovalev_D 190:289514f730ee 39 else
Kovalev_D 190:289514f730ee 40 {
Kovalev_D 40:8a6494f61326 41 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 42 }
igor_v 21:bc8c1cec3da6 43 }
igor_v 0:8ad47e2b6f00 44 }
igor_v 0:8ad47e2b6f00 45
Kovalev_D 191:40028201ddad 46
Kovalev_D 193:a0fe8bfc97e4 47 void Calc2AmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 48 {
Kovalev_D 193:a0fe8bfc97e4 49 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 50 static int PeriodCount = 0;
Kovalev_D 193:a0fe8bfc97e4 51 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 52 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 53 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 54 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 55
Kovalev_D 193:a0fe8bfc97e4 56 if(Gyro.AmpSC <55)countA++;
Kovalev_D 193:a0fe8bfc97e4 57 if(countA >2)
Kovalev_D 193:a0fe8bfc97e4 58 {
Kovalev_D 193:a0fe8bfc97e4 59 countA=0;
Kovalev_D 193:a0fe8bfc97e4 60 srand(Global_Time);
Kovalev_D 193:a0fe8bfc97e4 61 if(Cheng_AMP_Flag)
Kovalev_D 193:a0fe8bfc97e4 62 {
Kovalev_D 193:a0fe8bfc97e4 63 Cheng_AMP_Flag=0;
Kovalev_D 193:a0fe8bfc97e4 64 Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 65 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 66 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 67 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 68 }
Kovalev_D 193:a0fe8bfc97e4 69 else
Kovalev_D 193:a0fe8bfc97e4 70 {
Kovalev_D 193:a0fe8bfc97e4 71 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 72 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 73 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 74 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 75 }
Kovalev_D 193:a0fe8bfc97e4 76 } //8046
Kovalev_D 193:a0fe8bfc97e4 77
Kovalev_D 193:a0fe8bfc97e4 78 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 79 }
Kovalev_D 191:40028201ddad 80
Kovalev_D 190:289514f730ee 81 void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
igor_v 0:8ad47e2b6f00 82 {
Kovalev_D 191:40028201ddad 83 Gyro.AmpSC=0;
Kovalev_D 85:0466ee8cdfc8 84 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 85 unsigned int Nmax=0;
Kovalev_D 191:40028201ddad 86 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 191:40028201ddad 87 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 191:40028201ddad 88 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 191:40028201ddad 89
Kovalev_D 196:f76dbc081e63 90 if(Gyro.AmpSC <5)countA++;
Kovalev_D 193:a0fe8bfc97e4 91 if(countA >3)
Kovalev_D 191:40028201ddad 92 {
Kovalev_D 191:40028201ddad 93 countA=0;
Kovalev_D 191:40028201ddad 94
Kovalev_D 191:40028201ddad 95 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 191:40028201ddad 96 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 191:40028201ddad 97 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 191:40028201ddad 98 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 99
Kovalev_D 191:40028201ddad 100 tempDP=Gyro.AmpPerDel;
Kovalev_D 191:40028201ddad 101 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 192:d32c8cf7bcd9 102 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 192:d32c8cf7bcd9 103 else Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 104 } //8046
Kovalev_D 193:a0fe8bfc97e4 105
Kovalev_D 193:a0fe8bfc97e4 106 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 107 }
Kovalev_D 193:a0fe8bfc97e4 108
Kovalev_D 193:a0fe8bfc97e4 109
Kovalev_D 193:a0fe8bfc97e4 110
Kovalev_D 193:a0fe8bfc97e4 111
Kovalev_D 193:a0fe8bfc97e4 112 void CalcAmpD(void)
Kovalev_D 193:a0fe8bfc97e4 113 {
Kovalev_D 193:a0fe8bfc97e4 114 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 115 countA++;
Kovalev_D 193:a0fe8bfc97e4 116 if( countA>1)
Kovalev_D 193:a0fe8bfc97e4 117 {
Kovalev_D 193:a0fe8bfc97e4 118 countA=0;
Kovalev_D 193:a0fe8bfc97e4 119
Kovalev_D 193:a0fe8bfc97e4 120 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 121 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 122 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 123 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 124
Kovalev_D 193:a0fe8bfc97e4 125 // if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 126
Kovalev_D 193:a0fe8bfc97e4 127
Kovalev_D 193:a0fe8bfc97e4 128 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 129 case 0:
Kovalev_D 193:a0fe8bfc97e4 130 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 131 if (Gyro.AmpPerDel>10){Znak=1; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 132 break;
Kovalev_D 193:a0fe8bfc97e4 133 case 1:
Kovalev_D 193:a0fe8bfc97e4 134 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 135 if (Gyro.AmpPerDel<1)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 136 if (fnoize>6)Znak=2;
Kovalev_D 193:a0fe8bfc97e4 137 break;
Kovalev_D 196:f76dbc081e63 138
Kovalev_D 193:a0fe8bfc97e4 139 case 2:
Kovalev_D 193:a0fe8bfc97e4 140 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 141 if (Gyro.AmpPerDel>7){Znak=3; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 142 break;
Kovalev_D 193:a0fe8bfc97e4 143 case 3:
Kovalev_D 193:a0fe8bfc97e4 144 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 145 if (Gyro.AmpPerDel<1)Znak=2;
Kovalev_D 193:a0fe8bfc97e4 146 if (fnoize>12){Znak=4;/*fnoize=0;*/}
Kovalev_D 193:a0fe8bfc97e4 147 break;
Kovalev_D 196:f76dbc081e63 148
Kovalev_D 193:a0fe8bfc97e4 149 case 4:
Kovalev_D 193:a0fe8bfc97e4 150 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 151 if (Gyro.AmpPerDel>15){Znak=5; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 152 break;
Kovalev_D 193:a0fe8bfc97e4 153 case 5:
Kovalev_D 193:a0fe8bfc97e4 154 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 155 if (Gyro.AmpPerDel<1)Znak=4;
Kovalev_D 193:a0fe8bfc97e4 156 if (fnoize>18){Znak=6;/*fnoize=0;*/}
Kovalev_D 193:a0fe8bfc97e4 157 break;
Kovalev_D 196:f76dbc081e63 158
Kovalev_D 193:a0fe8bfc97e4 159 case 6:
Kovalev_D 193:a0fe8bfc97e4 160 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 161 if (Gyro.AmpPerDel>6){Znak=7;fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 162 break;
Kovalev_D 193:a0fe8bfc97e4 163 case 7:
Kovalev_D 193:a0fe8bfc97e4 164 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 165 if (Gyro.AmpPerDel<1)Znak=6;
Kovalev_D 193:a0fe8bfc97e4 166 if (fnoize>24){Znak=0;fnoize=0;}
Kovalev_D 193:a0fe8bfc97e4 167 break;
Kovalev_D 193:a0fe8bfc97e4 168 }
Kovalev_D 191:40028201ddad 169 }
Kovalev_D 193:a0fe8bfc97e4 170 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 171 }
Kovalev_D 193:a0fe8bfc97e4 172
Kovalev_D 193:a0fe8bfc97e4 173
Kovalev_D 193:a0fe8bfc97e4 174
Kovalev_D 193:a0fe8bfc97e4 175
Kovalev_D 193:a0fe8bfc97e4 176
Kovalev_D 193:a0fe8bfc97e4 177 void CalcAmpI(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 178 {
Kovalev_D 193:a0fe8bfc97e4 179 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 180 static int PeriodCount = 0 ;
Kovalev_D 193:a0fe8bfc97e4 181 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 182 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 183 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 184 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 185
Kovalev_D 193:a0fe8bfc97e4 186 countA++;
Kovalev_D 193:a0fe8bfc97e4 187 if( Gyro.AmpSC<60)
Kovalev_D 193:a0fe8bfc97e4 188 {
Kovalev_D 193:a0fe8bfc97e4 189 // countA=0;
Kovalev_D 193:a0fe8bfc97e4 190
Kovalev_D 193:a0fe8bfc97e4 191 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 192 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 193 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 194 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 195
Kovalev_D 193:a0fe8bfc97e4 196 // tempDP=Gyro.AmpPerDel;
Kovalev_D 193:a0fe8bfc97e4 197 // srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 193:a0fe8bfc97e4 198 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 199
Kovalev_D 193:a0fe8bfc97e4 200 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 201 case 0:
Kovalev_D 193:a0fe8bfc97e4 202 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 203 if (Gyro.AmpPerDel<1)Znak=1;
Kovalev_D 193:a0fe8bfc97e4 204 break;
Kovalev_D 193:a0fe8bfc97e4 205
Kovalev_D 193:a0fe8bfc97e4 206 case 1:
Kovalev_D 193:a0fe8bfc97e4 207 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 208 if (Gyro.AmpPerDel>13)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 209 break;
Kovalev_D 193:a0fe8bfc97e4 210 }
Kovalev_D 193:a0fe8bfc97e4 211
Kovalev_D 193:a0fe8bfc97e4 212 } //8046
Kovalev_D 193:a0fe8bfc97e4 213 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 214 }
Kovalev_D 193:a0fe8bfc97e4 215
Kovalev_D 193:a0fe8bfc97e4 216
Kovalev_D 193:a0fe8bfc97e4 217
Kovalev_D 193:a0fe8bfc97e4 218 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 219 {
Kovalev_D 193:a0fe8bfc97e4 220 static int PeriodCount = 0;
Kovalev_D 193:a0fe8bfc97e4 221 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 222
Kovalev_D 193:a0fe8bfc97e4 223 //расчет амплитуды относительно центральной точки
Kovalev_D 193:a0fe8bfc97e4 224 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 193:a0fe8bfc97e4 225 PeriodCount=0;//сбрасываем таймер
Kovalev_D 193:a0fe8bfc97e4 226
Kovalev_D 193:a0fe8bfc97e4 227 if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
Kovalev_D 193:a0fe8bfc97e4 228 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 229 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 230 }
Kovalev_D 193:a0fe8bfc97e4 231 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); //
Kovalev_D 193:a0fe8bfc97e4 232 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 233 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 234 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 235
Kovalev_D 193:a0fe8bfc97e4 236 }
Kovalev_D 193:a0fe8bfc97e4 237
Kovalev_D 193:a0fe8bfc97e4 238 else {
Kovalev_D 193:a0fe8bfc97e4 239 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 240 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 241 }
Kovalev_D 193:a0fe8bfc97e4 242
Kovalev_D 193:a0fe8bfc97e4 243 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 244 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 245 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 246 Cheng_AMP_Flag=0;
Kovalev_D 193:a0fe8bfc97e4 247
Kovalev_D 193:a0fe8bfc97e4 248 }
Kovalev_D 193:a0fe8bfc97e4 249 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 193:a0fe8bfc97e4 250 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 251
Kovalev_D 193:a0fe8bfc97e4 252 } else {
Kovalev_D 193:a0fe8bfc97e4 253 PeriodCount++;//таймер амплитуды
Kovalev_D 193:a0fe8bfc97e4 254 }
Kovalev_D 191:40028201ddad 255
Kovalev_D 196:f76dbc081e63 256 LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 196:f76dbc081e63 257
Kovalev_D 196:f76dbc081e63 258
Kovalev_D 196:f76dbc081e63 259
Kovalev_D 192:d32c8cf7bcd9 260 }
Kovalev_D 112:4a96133a1311 261 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 112:4a96133a1311 262 {
Kovalev_D 189:8a16378724c4 263 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 264 CaunAddPlus = 0;
Kovalev_D 190:289514f730ee 265 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 266 CaunAddMin = 0;
Kovalev_D 193:a0fe8bfc97e4 267 /*Gyro.tempdelta=Gyro.CaunPlus - Gyro.CaunMin ;*/
Kovalev_D 193:a0fe8bfc97e4 268 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 191:40028201ddad 269 Gyro.Amp -= (Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed; // расчет амплитуды ВП с учетом разници
Kovalev_D 191:40028201ddad 270 if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 196:f76dbc081e63 271 if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 191:40028201ddad 272 if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 191:40028201ddad 273 // Gyro.MaxAmp =0;
Kovalev_D 196:f76dbc081e63 274
Kovalev_D 112:4a96133a1311 275 }
Kovalev_D 112:4a96133a1311 276
Kovalev_D 191:40028201ddad 277
Kovalev_D 191:40028201ddad 278
Kovalev_D 112:4a96133a1311 279 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 196:f76dbc081e63 280 {
Kovalev_D 112:4a96133a1311 281 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 282 TempFaza = -4;
Kovalev_D 191:40028201ddad 283 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0
Kovalev_D 191:40028201ddad 284 if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;}
Kovalev_D 189:8a16378724c4 285
Kovalev_D 189:8a16378724c4 286 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 189:8a16378724c4 287 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
igor_v 0:8ad47e2b6f00 288 }
igor_v 0:8ad47e2b6f00 289
igor_v 0:8ad47e2b6f00 290 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 291 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 292 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 293 void cheng(void)
Kovalev_D 99:3d8f206ceac2 294 {
Kovalev_D 107:4d178bcc9d8a 295 switch(CountV31) {
Kovalev_D 112:4a96133a1311 296 case 0:
Kovalev_D 196:f76dbc081e63 297 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 298 Time_vibro=0;
Kovalev_D 191:40028201ddad 299 Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 196:f76dbc081e63 300
Kovalev_D 112:4a96133a1311 301 break;
Kovalev_D 196:f76dbc081e63 302
Kovalev_D 112:4a96133a1311 303 case 16:
Kovalev_D 194:8f3cb37a5541 304 //Gyro.Reper_Event=1;
Kovalev_D 112:4a96133a1311 305 Time_vibro=0;
Kovalev_D 191:40028201ddad 306 Gyro.VibroFrqRegulF=1; //
Kovalev_D 112:4a96133a1311 307 break;
Kovalev_D 196:f76dbc081e63 308
Kovalev_D 196:f76dbc081e63 309 case 31:
Kovalev_D 196:f76dbc081e63 310 for (int i = 0; i < 32; i++ )
Kovalev_D 196:f76dbc081e63 311 {
Kovalev_D 196:f76dbc081e63 312 Gyro.CuruAngle += Buff_32Point [i];
Kovalev_D 196:f76dbc081e63 313 Gyro.tempdelta += (Buff_Restored_sin[i]);
Kovalev_D 196:f76dbc081e63 314 Gyro.tempdelta2 += (Buff_Restored_sin2[i]);
Kovalev_D 196:f76dbc081e63 315 }
Kovalev_D 196:f76dbc081e63 316 if(Gyro.LogHZ)
Kovalev_D 196:f76dbc081e63 317 {
Kovalev_D 196:f76dbc081e63 318 sprintf((Time),"%d %d %d\r\n", Gyro.CuruAngle,Gyro.tempdelta,Gyro.tempdelta2); WriteCon(Time);
Kovalev_D 196:f76dbc081e63 319 Gyro.CuruAngle=0;
Kovalev_D 196:f76dbc081e63 320 Gyro.tempdelta=0;
Kovalev_D 196:f76dbc081e63 321 Gyro.tempdelta2=0;
Kovalev_D 196:f76dbc081e63 322
Kovalev_D 196:f76dbc081e63 323 }
Kovalev_D 196:f76dbc081e63 324
Kovalev_D 193:a0fe8bfc97e4 325 break;
igor_v 21:bc8c1cec3da6 326 }
Kovalev_D 191:40028201ddad 327 }
Kovalev_D 191:40028201ddad 328 void AllRegul (void)
Kovalev_D 191:40028201ddad 329 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 330
Kovalev_D 191:40028201ddad 331 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 332 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 196:f76dbc081e63 333 if (Gyro.ADS_NewData == 1)
Kovalev_D 196:f76dbc081e63 334 {
Kovalev_D 196:f76dbc081e63 335 Gyro.ADS_NewData = 0;
Kovalev_D 196:f76dbc081e63 336 switch(Gyro.LogPLC) {
Kovalev_D 196:f76dbc081e63 337 case 0: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 338 case 1: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 339 case 2: ShowMod(); break;
Kovalev_D 196:f76dbc081e63 340 }
Kovalev_D 196:f76dbc081e63 341 } // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
Kovalev_D 191:40028201ddad 342 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 191:40028201ddad 343 if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса
Kovalev_D 192:d32c8cf7bcd9 344 if (Gyro.VibroNoiseF == 1)
Kovalev_D 192:d32c8cf7bcd9 345 {
Kovalev_D 196:f76dbc081e63 346 /*Gyro.AmpPerDel=10;
Kovalev_D 193:a0fe8bfc97e4 347 Gyro.AmpMin =10;// минимальное значение AmpT;
Kovalev_D 193:a0fe8bfc97e4 348 Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления)
Kovalev_D 196:f76dbc081e63 349 Gyro.VibroNoiseF = 0;
Kovalev_D 196:f76dbc081e63 350 OLDCalcAmpN();*/
Kovalev_D 196:f76dbc081e63 351
Kovalev_D 196:f76dbc081e63 352 switch(Gyro.flag) {
Kovalev_D 196:f76dbc081e63 353 case 1:
Kovalev_D 196:f76dbc081e63 354 Gyro.VibroNoiseF = 0;
Kovalev_D 193:a0fe8bfc97e4 355 OLDCalcAmpN();
Kovalev_D 196:f76dbc081e63 356 break;
Kovalev_D 196:f76dbc081e63 357
Kovalev_D 196:f76dbc081e63 358 case 2:
Kovalev_D 196:f76dbc081e63 359 Gyro.AmpMin =3;
Kovalev_D 193:a0fe8bfc97e4 360 Gyro.AmpTD =10;
Kovalev_D 193:a0fe8bfc97e4 361 Gyro.VibroNoiseF = 0;
Kovalev_D 196:f76dbc081e63 362 Calc2AmpN();
Kovalev_D 196:f76dbc081e63 363 break;
Kovalev_D 196:f76dbc081e63 364
Kovalev_D 196:f76dbc081e63 365 case 3:
Kovalev_D 193:a0fe8bfc97e4 366 Gyro.AmpMin =1;
Kovalev_D 193:a0fe8bfc97e4 367 Gyro.AmpTD =10;
Kovalev_D 192:d32c8cf7bcd9 368 Gyro.VibroNoiseF = 0;
Kovalev_D 193:a0fe8bfc97e4 369 //CalcAmpN();
Kovalev_D 193:a0fe8bfc97e4 370 Gyro.AmpTarget =15000;
Kovalev_D 196:f76dbc081e63 371 //CalcAmpI();
Kovalev_D 196:f76dbc081e63 372 CalcAmpD();
Kovalev_D 196:f76dbc081e63 373 break;
Kovalev_D 196:f76dbc081e63 374 }
Kovalev_D 192:d32c8cf7bcd9 375 } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
Kovalev_D 191:40028201ddad 376 if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно
Kovalev_D 191:40028201ddad 377
Kovalev_D 191:40028201ddad 378 }