Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
vibro.c@226:4a4d5bd5fcd7, 2017-12-26 (annotated)
- Committer:
- Kovalev_D
- Date:
- Tue Dec 26 05:14:09 2017 +0000
- Revision:
- 226:4a4d5bd5fcd7
- Parent:
- 218:b4067cac75c0
- Child:
- 227:2774b56bfab0
4hfo_1plc
Who changed what in which revision?
User | Revision | Line number | New 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 | 209:224e7331a061 | 7 | unsigned int OldMaxAmp=0,countFras=0; |
Kovalev_D | 207:d1ce992f5d17 | 8 | int z=25; |
Kovalev_D | 208:19150d2b528f | 9 | int i=16,tempi=0,klk=0; |
Kovalev_D | 226:4a4d5bd5fcd7 | 10 | |
Kovalev_D | 226:4a4d5bd5fcd7 | 11 | |
Kovalev_D | 193:a0fe8bfc97e4 | 12 | __irq void EINT3_IRQHandler() |
Kovalev_D | 208:19150d2b528f | 13 | { |
Kovalev_D | 226:4a4d5bd5fcd7 | 14 | |
Kovalev_D | 226:4a4d5bd5fcd7 | 15 | Gyro.EXT_Latch=1; |
Kovalev_D | 193:a0fe8bfc97e4 | 16 | LPC_GPIOINT->IO0IntClr |= (1<<1); |
Kovalev_D | 226:4a4d5bd5fcd7 | 17 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 18 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 19 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 20 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 21 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 22 | |
Kovalev_D | 226:4a4d5bd5fcd7 | 23 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 24 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 25 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 26 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 27 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 28 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 29 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 30 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 31 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 32 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 33 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 34 | |
Kovalev_D | 226:4a4d5bd5fcd7 | 35 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 36 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 37 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 38 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 39 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 40 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 41 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 42 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 43 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 44 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 45 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 46 | |
Kovalev_D | 226:4a4d5bd5fcd7 | 47 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 48 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 49 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 50 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 51 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 52 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 53 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 54 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 55 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 56 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 57 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 58 | |
Kovalev_D | 226:4a4d5bd5fcd7 | 59 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 60 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 61 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 62 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 63 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 64 | LoopOn |
Kovalev_D | 226:4a4d5bd5fcd7 | 65 | |
Kovalev_D | 193:a0fe8bfc97e4 | 66 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 67 | |
Kovalev_D | 193:a0fe8bfc97e4 | 68 | |
igor_v | 114:5cc38a53d8a7 | 69 | void VibroOut(void) // выставка ног вибро |
igor_v | 0:8ad47e2b6f00 | 70 | { |
Kovalev_D | 190:289514f730ee | 71 | if(CountV31>=16) |
Kovalev_D | 190:289514f730ee | 72 | {//первая нога вибро |
Kovalev_D | 89:a0d344db227e | 73 | // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1 |
Kovalev_D | 190:289514f730ee | 74 | if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) |
Kovalev_D | 190:289514f730ee | 75 | { |
Kovalev_D | 203:3a6615de9581 | 76 | SetV1//установить в регистре PinReg бит "вибро 1" в "0" |
Kovalev_D | 190:289514f730ee | 77 | } |
Kovalev_D | 190:289514f730ee | 78 | else |
Kovalev_D | 190:289514f730ee | 79 | { |
Kovalev_D | 207:d1ce992f5d17 | 80 | ClrV1 //установить в регистре PinReg бит "вибро 1" в "1" |
igor_v | 21:bc8c1cec3da6 | 81 | } |
Kovalev_D | 85:0466ee8cdfc8 | 82 | |
Kovalev_D | 190:289514f730ee | 83 | } |
Kovalev_D | 190:289514f730ee | 84 | else {//вторая нога вибро |
Kovalev_D | 190:289514f730ee | 85 | if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) |
Kovalev_D | 190:289514f730ee | 86 | { |
Kovalev_D | 207:d1ce992f5d17 | 87 | SetV2 //установить в регистре PinReg бит "вибро 2" в "0" |
Kovalev_D | 190:289514f730ee | 88 | } |
Kovalev_D | 190:289514f730ee | 89 | else |
Kovalev_D | 190:289514f730ee | 90 | { |
Kovalev_D | 203:3a6615de9581 | 91 | ClrV2//установить в регистре PinReg бит "вибро 2" в "1" |
igor_v | 21:bc8c1cec3da6 | 92 | } |
igor_v | 21:bc8c1cec3da6 | 93 | } |
igor_v | 0:8ad47e2b6f00 | 94 | } |
Kovalev_D | 214:4c70e452c491 | 95 | /* |
Kovalev_D | 193:a0fe8bfc97e4 | 96 | |
Kovalev_D | 193:a0fe8bfc97e4 | 97 | void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
Kovalev_D | 207:d1ce992f5d17 | 98 | { |
Kovalev_D | 207:d1ce992f5d17 | 99 | static int PeriodCount = 0,Period=0; |
Kovalev_D | 210:b02fa166315d | 100 | unsigned int Nmax=0, lowper=1; |
Kovalev_D | 209:224e7331a061 | 101 | Gyro.FrqHZ=Gyro.Frq>>16; |
Kovalev_D | 209:224e7331a061 | 102 | if(PeriodCount>= Gyro.AmpT) |
Kovalev_D | 209:224e7331a061 | 103 | { //если количество заходов в прерывание больше либо равно частоте ошумления. |
Kovalev_D | 209:224e7331a061 | 104 | PeriodCount=0;//сбрасываем таймер |
Kovalev_D | 214:4c70e452c491 | 105 | sprintf((Time),"%d %d %d\r\n", Gyro.AmpN1, Gyro.AmpN2-Gyro.AmpN1, Gyro.AmpPer); |
Kovalev_D | 214:4c70e452c491 | 106 | WriteCon(Time); |
Kovalev_D | 211:ac8251b067d2 | 107 | //Gyro.AmpPerDel=(Gyro.AmpPer*100)/Gyro.AmpPerDel; |
Kovalev_D | 209:224e7331a061 | 108 | if(Cheng_AMP_Flag==0) |
Kovalev_D | 209:224e7331a061 | 109 | { |
Kovalev_D | 214:4c70e452c491 | 110 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды |
Kovalev_D | 210:b02fa166315d | 111 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);//256 // |
Kovalev_D | 210:b02fa166315d | 112 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды 63 |
Kovalev_D | 210:b02fa166315d | 113 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 65 |
Kovalev_D | 210:b02fa166315d | 114 | Gyro.L_vibro=Gyro.AmpPer*208; |
Kovalev_D | 214:4c70e452c491 | 115 | sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); |
Kovalev_D | 214:4c70e452c491 | 116 | WriteCon(Time); |
Kovalev_D | 209:224e7331a061 | 117 | Cheng_AMP_Flag=1; |
Kovalev_D | 209:224e7331a061 | 118 | } |
Kovalev_D | 210:b02fa166315d | 119 | else |
Kovalev_D | 210:b02fa166315d | 120 | { |
Kovalev_D | 210:b02fa166315d | 121 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды |
Kovalev_D | 210:b02fa166315d | 122 | if((Gyro.RgConA&0x20)) |
Kovalev_D | 210:b02fa166315d | 123 | { |
Kovalev_D | 210:b02fa166315d | 124 | Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);//256 |
Kovalev_D | 210:b02fa166315d | 125 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/Gyro.FrqHZ); //левая граница амплитуды 61 |
Kovalev_D | 210:b02fa166315d | 126 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 67 |
Kovalev_D | 210:b02fa166315d | 127 | Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*208; |
Kovalev_D | 214:4c70e452c491 | 128 | sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); |
Kovalev_D | 214:4c70e452c491 | 129 | WriteCon(Time); |
Kovalev_D | 210:b02fa166315d | 130 | } |
Kovalev_D | 210:b02fa166315d | 131 | Cheng_AMP_Flag=0; |
Kovalev_D | 210:b02fa166315d | 132 | } |
Kovalev_D | 210:b02fa166315d | 133 | if(Gyro.AmpN2<(Gyro.AmpN1+2))Gyro.AmpN2=Gyro.AmpN1+2; |
Kovalev_D | 210:b02fa166315d | 134 | |
Kovalev_D | 207:d1ce992f5d17 | 135 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 210:b02fa166315d | 136 | Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 193:a0fe8bfc97e4 | 137 | } else { |
Kovalev_D | 193:a0fe8bfc97e4 | 138 | PeriodCount++;//таймер амплитуды |
Kovalev_D | 193:a0fe8bfc97e4 | 139 | } |
Kovalev_D | 214:4c70e452c491 | 140 | }*/ |
Kovalev_D | 214:4c70e452c491 | 141 | void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
Kovalev_D | 214:4c70e452c491 | 142 | { |
Kovalev_D | 214:4c70e452c491 | 143 | static int PeriodCount = 0,Period=0; |
Kovalev_D | 214:4c70e452c491 | 144 | unsigned int Nmax=0, lowper=1; |
Kovalev_D | 214:4c70e452c491 | 145 | Gyro.FrqHZ=Gyro.Frq>>16; |
Kovalev_D | 214:4c70e452c491 | 146 | if(PeriodCount>= Gyro.AmpT) |
Kovalev_D | 214:4c70e452c491 | 147 | { |
Kovalev_D | 214:4c70e452c491 | 148 | PeriodCount=0;//сбрасываем таймер |
Kovalev_D | 214:4c70e452c491 | 149 | if(Cheng_AMP_Flag==0) |
Kovalev_D | 214:4c70e452c491 | 150 | { |
Kovalev_D | 214:4c70e452c491 | 151 | //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды |
Kovalev_D | 214:4c70e452c491 | 152 | T_vib_1 = Gyro.AmpPer * T_vibP; |
Kovalev_D | 214:4c70e452c491 | 153 | T_vib_2 = T_vibP * (10000-Gyro.AmpPer); |
Kovalev_D | 214:4c70e452c491 | 154 | Gyro.L_vibro=Gyro.AmpPer*3; |
Kovalev_D | 214:4c70e452c491 | 155 | Cheng_AMP_Flag=1; |
Kovalev_D | 214:4c70e452c491 | 156 | } |
Kovalev_D | 214:4c70e452c491 | 157 | else |
Kovalev_D | 214:4c70e452c491 | 158 | { |
Kovalev_D | 214:4c70e452c491 | 159 | //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды |
Kovalev_D | 214:4c70e452c491 | 160 | if((Gyro.RgConA&0x20)) |
Kovalev_D | 214:4c70e452c491 | 161 | { |
Kovalev_D | 214:4c70e452c491 | 162 | T_vib_1 = T_vibP * (Gyro.AmpPer +(Gyro.AmpPerDel*100)); |
Kovalev_D | 214:4c70e452c491 | 163 | T_vib_2 = T_vibP * (10000 -(Gyro.AmpPer+(Gyro.AmpPerDel*100))); |
Kovalev_D | 214:4c70e452c491 | 164 | Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*3; |
Kovalev_D | 214:4c70e452c491 | 165 | } |
Kovalev_D | 214:4c70e452c491 | 166 | Cheng_AMP_Flag=0; |
Kovalev_D | 214:4c70e452c491 | 167 | } |
Kovalev_D | 214:4c70e452c491 | 168 | |
Kovalev_D | 214:4c70e452c491 | 169 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 214:4c70e452c491 | 170 | Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 214:4c70e452c491 | 171 | } |
Kovalev_D | 214:4c70e452c491 | 172 | else {PeriodCount++;} |
Kovalev_D | 208:19150d2b528f | 173 | |
Kovalev_D | 192:d32c8cf7bcd9 | 174 | } |
Kovalev_D | 207:d1ce992f5d17 | 175 | |
Kovalev_D | 112:4a96133a1311 | 176 | void VibroAMPRegul(void) //подстройка амплитуды ВП |
Kovalev_D | 208:19150d2b528f | 177 | { |
Kovalev_D | 209:224e7331a061 | 178 | |
Kovalev_D | 208:19150d2b528f | 179 | int temp=0; |
Kovalev_D | 218:b4067cac75c0 | 180 | static unsigned int FConunt=0; |
Kovalev_D | 211:ac8251b067d2 | 181 | int LowDZ,HiDZ; |
Kovalev_D | 218:b4067cac75c0 | 182 | /* if(FConunt<4) |
Kovalev_D | 218:b4067cac75c0 | 183 | {*/ |
Kovalev_D | 218:b4067cac75c0 | 184 | //FConunt++; |
Kovalev_D | 189:8a16378724c4 | 185 | Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin |
Kovalev_D | 218:b4067cac75c0 | 186 | Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin |
Kovalev_D | 112:4a96133a1311 | 187 | CaunAddPlus = 0; |
Kovalev_D | 112:4a96133a1311 | 188 | CaunAddMin = 0; |
Kovalev_D | 208:19150d2b528f | 189 | Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; |
Kovalev_D | 218:b4067cac75c0 | 190 | // } |
Kovalev_D | 218:b4067cac75c0 | 191 | /*else |
Kovalev_D | 218:b4067cac75c0 | 192 | {*/ |
Kovalev_D | 218:b4067cac75c0 | 193 | // FConunt=0; |
Kovalev_D | 218:b4067cac75c0 | 194 | //Gyro.MaxAmp=Gyro.MaxAmp>>2; |
Kovalev_D | 218:b4067cac75c0 | 195 | if(countFras<512) |
Kovalev_D | 218:b4067cac75c0 | 196 | { |
Kovalev_D | 218:b4067cac75c0 | 197 | countFras++; |
Kovalev_D | 218:b4067cac75c0 | 198 | Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; |
Kovalev_D | 218:b4067cac75c0 | 199 | } |
Kovalev_D | 218:b4067cac75c0 | 200 | else |
Kovalev_D | 218:b4067cac75c0 | 201 | { |
Kovalev_D | 218:b4067cac75c0 | 202 | Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; |
Kovalev_D | 218:b4067cac75c0 | 203 | Gyro.F_ras=Gyro.F_rasAdd>>9; |
Kovalev_D | 218:b4067cac75c0 | 204 | Gyro.F_rasAdd=0; |
Kovalev_D | 218:b4067cac75c0 | 205 | countFras=0; |
Kovalev_D | 218:b4067cac75c0 | 206 | } |
Kovalev_D | 218:b4067cac75c0 | 207 | if(Gyro.RgConA&0x20) |
Kovalev_D | 218:b4067cac75c0 | 208 | { |
Kovalev_D | 209:224e7331a061 | 209 | //расчет максимальной амплитуды из востановленного синуса р-р. |
Kovalev_D | 218:b4067cac75c0 | 210 | temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed)); |
Kovalev_D | 218:b4067cac75c0 | 211 | temp=temp>>6; |
Kovalev_D | 218:b4067cac75c0 | 212 | LowDZ = ((Gyro.AmpSpeed<<3)*(-1)); |
Kovalev_D | 218:b4067cac75c0 | 213 | HiDZ = (Gyro.AmpSpeed<<3); |
Kovalev_D | 218:b4067cac75c0 | 214 | Gyro.Amp -= temp>>4; // расчет амплитуды ВП с учетом разници |
Kovalev_D | 218:b4067cac75c0 | 215 | if((Gyro.Amp>>16) > (Gyro.AmpPerMax)) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// |
Kovalev_D | 218:b4067cac75c0 | 216 | if((Gyro.Amp>>16) < (Gyro.AmpPerMin)) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// |
Kovalev_D | 214:4c70e452c491 | 217 | // Gyro.AmpPer = (Gyro.Amp)>>16; //приведение амплитуды ВП к виду 0%-100% |
Kovalev_D | 218:b4067cac75c0 | 218 | } |
Kovalev_D | 218:b4067cac75c0 | 219 | LPC_MCPWM->MAT1 = T_vib_1; |
Kovalev_D | 218:b4067cac75c0 | 220 | LPC_MCPWM->MAT2 = T_vib_2; |
Kovalev_D | 218:b4067cac75c0 | 221 | // } |
Kovalev_D | 218:b4067cac75c0 | 222 | } |
Kovalev_D | 112:4a96133a1311 | 223 | |
Kovalev_D | 191:40028201ddad | 224 | |
Kovalev_D | 191:40028201ddad | 225 | |
Kovalev_D | 112:4a96133a1311 | 226 | void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) |
Kovalev_D | 196:f76dbc081e63 | 227 | { |
Kovalev_D | 112:4a96133a1311 | 228 | static int TempFaza, CountFaza; |
Kovalev_D | 112:4a96133a1311 | 229 | TempFaza = -4; |
Kovalev_D | 209:224e7331a061 | 230 | Gyro.FrqPhaseEror=0; |
Kovalev_D | 209:224e7331a061 | 231 | 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 | 232 | for (CountFaza = 0; CountFaza <8; CountFaza++ ) {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f];} |
Kovalev_D | 208:19150d2b528f | 233 | if(Gyro.RgConA&0x40) |
Kovalev_D | 208:19150d2b528f | 234 | { //12 |
Kovalev_D | 209:224e7331a061 | 235 | Gyro.Frq += TempFaza*Gyro.FrqChengSpeed; |
Kovalev_D | 208:19150d2b528f | 236 | } |
Kovalev_D | 209:224e7331a061 | 237 | // Gyro.FrqPhaseEror = TempFaza<<10; |
Kovalev_D | 189:8a16378724c4 | 238 | if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты |
Kovalev_D | 214:4c70e452c491 | 239 | else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты*/ |
Kovalev_D | 214:4c70e452c491 | 240 | LPC_TIM1->MR0 =(unsigned int)(103200000/(Gyro.Frq>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 214:4c70e452c491 | 241 | // LPC_TIM1->MR0 =(unsigned int) F_vib; |
igor_v | 0:8ad47e2b6f00 | 242 | } |
igor_v | 0:8ad47e2b6f00 | 243 | |
igor_v | 0:8ad47e2b6f00 | 244 | ////////////////////////////////////////////////////////////////////////////// |
Kovalev_D | 190:289514f730ee | 245 | /////////////////////////основного 32 тактного цикла////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 246 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 247 | void cheng(void) |
Kovalev_D | 216:189b0ea1dc38 | 248 | { static unsigned int counttt=0; |
Kovalev_D | 107:4d178bcc9d8a | 249 | switch(CountV31) { |
Kovalev_D | 112:4a96133a1311 | 250 | case 0: |
Kovalev_D | 216:189b0ea1dc38 | 251 | |
Kovalev_D | 214:4c70e452c491 | 252 | ReVib();///обновление значений вибро |
Kovalev_D | 196:f76dbc081e63 | 253 | Gyro.VibroAMPRegulF=1; |
Kovalev_D | 112:4a96133a1311 | 254 | Time_vibro=0; |
Kovalev_D | 215:b58b887fd367 | 255 | Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. |
Kovalev_D | 112:4a96133a1311 | 256 | break; |
Kovalev_D | 214:4c70e452c491 | 257 | case 8: |
Kovalev_D | 214:4c70e452c491 | 258 | LPC_MCPWM->CON_CLR |= (1<<8); |
Kovalev_D | 214:4c70e452c491 | 259 | LPC_MCPWM->CON_CLR |= (1<<16); |
Kovalev_D | 214:4c70e452c491 | 260 | |
Kovalev_D | 214:4c70e452c491 | 261 | LPC_MCPWM->TC1 =1; |
Kovalev_D | 214:4c70e452c491 | 262 | LPC_MCPWM->TC2 =1; |
Kovalev_D | 214:4c70e452c491 | 263 | |
Kovalev_D | 215:b58b887fd367 | 264 | LPC_MCPWM->CON_SET |= (1<<8); |
Kovalev_D | 214:4c70e452c491 | 265 | LPC_MCPWM->CON_SET |= (1<<16); //вкл |
Kovalev_D | 214:4c70e452c491 | 266 | break; |
Kovalev_D | 214:4c70e452c491 | 267 | |
Kovalev_D | 112:4a96133a1311 | 268 | case 16: |
Kovalev_D | 215:b58b887fd367 | 269 | Time_vibro=0; |
Kovalev_D | 215:b58b887fd367 | 270 | Gyro.VibroFrqRegulF=1; // |
Kovalev_D | 112:4a96133a1311 | 271 | break; |
Kovalev_D | 214:4c70e452c491 | 272 | |
Kovalev_D | 215:b58b887fd367 | 273 | case 31: |
Kovalev_D | 216:189b0ea1dc38 | 274 | /* if(counttt>199) |
Kovalev_D | 216:189b0ea1dc38 | 275 | { |
Kovalev_D | 216:189b0ea1dc38 | 276 | |
Kovalev_D | 216:189b0ea1dc38 | 277 | sprintf((Time)," %d %d %d %d \r\n ", SinMns, SinPls, SinMns+SinPls, faza); |
Kovalev_D | 216:189b0ea1dc38 | 278 | WriteCon(Time); |
Kovalev_D | 216:189b0ea1dc38 | 279 | counttt=0; |
Kovalev_D | 216:189b0ea1dc38 | 280 | SinMns=0; |
Kovalev_D | 216:189b0ea1dc38 | 281 | SinPls=0; |
Kovalev_D | 216:189b0ea1dc38 | 282 | } |
Kovalev_D | 216:189b0ea1dc38 | 283 | counttt++; |
Kovalev_D | 216:189b0ea1dc38 | 284 | */ |
Kovalev_D | 214:4c70e452c491 | 285 | break; |
Kovalev_D | 207:d1ce992f5d17 | 286 | } |
Kovalev_D | 191:40028201ddad | 287 | } |
Kovalev_D | 191:40028201ddad | 288 | void AllRegul (void) |
Kovalev_D | 191:40028201ddad | 289 | { ///////////////////////////контуры регулировки///////////////////////////// |
Kovalev_D | 191:40028201ddad | 290 | |
Kovalev_D | 191:40028201ddad | 291 | if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро |
Kovalev_D | 191:40028201ddad | 292 | if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD |
Kovalev_D | 191:40028201ddad | 293 | if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса |
Kovalev_D | 209:224e7331a061 | 294 | if (Gyro.VibroAMPRegulF == 1) |
Kovalev_D | 209:224e7331a061 | 295 | { |
Kovalev_D | 209:224e7331a061 | 296 | Gyro.VibroAMPRegulF = 0; |
Kovalev_D | 209:224e7331a061 | 297 | VibroAMPRegul(); |
Kovalev_D | 215:b58b887fd367 | 298 | PLCRegul(); |
Kovalev_D | 215:b58b887fd367 | 299 | if(Gyro.LG_Type==1) |
Kovalev_D | 215:b58b887fd367 | 300 | { |
Kovalev_D | 215:b58b887fd367 | 301 | HFORegul(); |
Kovalev_D | 215:b58b887fd367 | 302 | } |
Kovalev_D | 209:224e7331a061 | 303 | } // Регулеровка Амплитуды виброподвеса |
Kovalev_D | 209:224e7331a061 | 304 | if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();} |
Kovalev_D | 196:f76dbc081e63 | 305 | } |