Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

vibro.c@40:8a6494f61326, 2016-02-06 (annotated)

Committer:
Kovalev_D
Date:
Sat Feb 06 18:16:06 2016 +0000
Revision:
40:8a6494f61326
Parent:
33:341521841d3a
Child:
48:8697dfe679b9
??????? ?????????? ? ??????????? ???????? ???, ?????? ???? ?????? ???????.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 0:8ad47e2b6f00 1 #include "Global.h"
igor_v 21:bc8c1cec3da6 2
igor_v 0:8ad47e2b6f00 3
igor_v 30:17c84ed091b3 4 GyroT Gyro;
igor_v 0:8ad47e2b6f00 5
igor_v 0:8ad47e2b6f00 6
igor_v 0:8ad47e2b6f00 7 volatile int V1 = 0 ;
igor_v 0:8ad47e2b6f00 8 volatile int Temp = 0 ;
igor_v 0:8ad47e2b6f00 9 volatile unsigned int Flag=0;
igor_v 0:8ad47e2b6f00 10
igor_v 0:8ad47e2b6f00 11 unsigned int FrecTemp=0;
igor_v 0:8ad47e2b6f00 12 /////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 13 /////////////////////////инициализация вибропривода//////////////////////////
igor_v 0:8ad47e2b6f00 14 /////////////////////////////////////////////////////////////////////////////
Kovalev_D 28:1c9acd3b224d 15 void init_Vibro ()
igor_v 21:bc8c1cec3da6 16 {
igor_v 30:17c84ed091b3 17 Gyro.FrqRate=40;
igor_v 30:17c84ed091b3 18 Gyro.FrqMin=380;
igor_v 30:17c84ed091b3 19 Gyro.FrqHZ=449;
igor_v 30:17c84ed091b3 20 Gyro.Frq = Gyro.FrqHZ<<16;
igor_v 21:bc8c1cec3da6 21
igor_v 30:17c84ed091b3 22 Gyro.AmpPer=20;
igor_v 30:17c84ed091b3 23 Gyro.AmpPerDel=10;
igor_v 30:17c84ed091b3 24 Gyro.Amp = 10*65535;
igor_v 30:17c84ed091b3 25 Gyro.AmpL = 20*650;
igor_v 21:bc8c1cec3da6 26 /*
igor_v 21:bc8c1cec3da6 27 Vib.AmpH=50*650;
igor_v 21:bc8c1cec3da6 28 // Vib.AmpT=600;
igor_v 21:bc8c1cec3da6 29 // Output.Str.T_Vibro=40;
igor_v 21:bc8c1cec3da6 30 //LPC_GPIO0->FIOSET |= (1<<5);
igor_v 21:bc8c1cec3da6 31 */
igor_v 21:bc8c1cec3da6 32 }
igor_v 21:bc8c1cec3da6 33
igor_v 0:8ad47e2b6f00 34
igor_v 0:8ad47e2b6f00 35 void VibroOut(void)
igor_v 0:8ad47e2b6f00 36 {
Kovalev_D 40:8a6494f61326 37 if(CountV31>=16) {
igor_v 30:17c84ed091b3 38 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) {
Kovalev_D 40:8a6494f61326 39 Gyro.PinReg &= ~PinRegBit_1V;/*LoopOn*///установить в регистре PinReg бит "вибро 1" в "0"
Kovalev_D 40:8a6494f61326 40 LedOFF
igor_v 21:bc8c1cec3da6 41 } else {
Kovalev_D 40:8a6494f61326 42 Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1"
Kovalev_D 40:8a6494f61326 43 LedON
igor_v 21:bc8c1cec3da6 44 }
igor_v 21:bc8c1cec3da6 45 } else {
igor_v 30:17c84ed091b3 46 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) {
Kovalev_D 40:8a6494f61326 47 Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0"
igor_v 21:bc8c1cec3da6 48 } else {
Kovalev_D 40:8a6494f61326 49 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 50 }
igor_v 21:bc8c1cec3da6 51 }
igor_v 0:8ad47e2b6f00 52
igor_v 0:8ad47e2b6f00 53
igor_v 0:8ad47e2b6f00 54
igor_v 0:8ad47e2b6f00 55 }
igor_v 0:8ad47e2b6f00 56
igor_v 0:8ad47e2b6f00 57 void CalcAmpN(void)
igor_v 0:8ad47e2b6f00 58 {
igor_v 21:bc8c1cec3da6 59 static int PeriodCount = 0,Tnoise;
igor_v 21:bc8c1cec3da6 60
igor_v 21:bc8c1cec3da6 61
igor_v 21:bc8c1cec3da6 62
igor_v 21:bc8c1cec3da6 63 unsigned int Nmax=0;
igor_v 21:bc8c1cec3da6 64
igor_v 21:bc8c1cec3da6 65
igor_v 21:bc8c1cec3da6 66 if(PeriodCount>= Tnoise) { //если количество заходов в прерывание больше либо равно частоте ошумления.
igor_v 21:bc8c1cec3da6 67 if (Flag==0) { //сейчас малая амплитуда?
igor_v 21:bc8c1cec3da6 68
igor_v 30:17c84ed091b3 69 if(Gyro.AmpPer>90) {
igor_v 30:17c84ed091b3 70 Gyro.AmpPer=90;
igor_v 21:bc8c1cec3da6 71 }
igor_v 30:17c84ed091b3 72 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
igor_v 30:17c84ed091b3 73 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400);
igor_v 30:17c84ed091b3 74 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1);
igor_v 21:bc8c1cec3da6 75 Flag=1;
igor_v 21:bc8c1cec3da6 76
igor_v 21:bc8c1cec3da6 77 }
igor_v 21:bc8c1cec3da6 78
igor_v 21:bc8c1cec3da6 79 else {
igor_v 30:17c84ed091b3 80 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
igor_v 30:17c84ed091b3 81 Gyro.AmpPer=90-Gyro.AmpPerDel;
igor_v 21:bc8c1cec3da6 82 }
igor_v 21:bc8c1cec3da6 83
igor_v 30:17c84ed091b3 84 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
igor_v 30:17c84ed091b3 85 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400);
igor_v 30:17c84ed091b3 86 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1);
igor_v 21:bc8c1cec3da6 87 Flag=0;
igor_v 21:bc8c1cec3da6 88 }
igor_v 30:17c84ed091b3 89 Gyro.AmpMin =10;
igor_v 30:17c84ed091b3 90 Gyro.AmpTD =30;
igor_v 21:bc8c1cec3da6 91 //Tnoise=55;
igor_v 21:bc8c1cec3da6 92 srand(Global_Time);
igor_v 30:17c84ed091b3 93 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
igor_v 30:17c84ed091b3 94 Tnoise=Gyro.AmpT;
igor_v 21:bc8c1cec3da6 95 //изменение флага предидущей амплитуды
igor_v 21:bc8c1cec3da6 96 PeriodCount=0;
igor_v 21:bc8c1cec3da6 97 } else {
igor_v 21:bc8c1cec3da6 98 PeriodCount++;
igor_v 21:bc8c1cec3da6 99 }
igor_v 21:bc8c1cec3da6 100
igor_v 21:bc8c1cec3da6 101
igor_v 21:bc8c1cec3da6 102
igor_v 0:8ad47e2b6f00 103 }
igor_v 0:8ad47e2b6f00 104
igor_v 0:8ad47e2b6f00 105 //////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 106 /////////////////////////функция работы вибропривода//////////////////////////
igor_v 0:8ad47e2b6f00 107 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 108 void cheng(void)
igor_v 21:bc8c1cec3da6 109 {
igor_v 21:bc8c1cec3da6 110 static int TempFaza, CountFaza;
igor_v 0:8ad47e2b6f00 111
igor_v 0:8ad47e2b6f00 112
igor_v 21:bc8c1cec3da6 113 //Noise();
igor_v 0:8ad47e2b6f00 114
igor_v 21:bc8c1cec3da6 115 //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
igor_v 21:bc8c1cec3da6 116 /*
igor_v 21:bc8c1cec3da6 117 // CountV++;
igor_v 21:bc8c1cec3da6 118 // CountV &= 0x1f;
igor_v 21:bc8c1cec3da6 119 */
igor_v 21:bc8c1cec3da6 120 if (Buff_Restored_sin [CountV255] > MaxAmp) {
igor_v 21:bc8c1cec3da6 121 MaxAmp=Buff_Restored_sin [CountV255];
igor_v 21:bc8c1cec3da6 122 }
igor_v 21:bc8c1cec3da6 123
igor_v 21:bc8c1cec3da6 124
igor_v 21:bc8c1cec3da6 125
igor_v 0:8ad47e2b6f00 126
igor_v 21:bc8c1cec3da6 127
igor_v 21:bc8c1cec3da6 128 switch(CountV31) {
igor_v 21:bc8c1cec3da6 129 case 0:
igor_v 21:bc8c1cec3da6 130 CalcAmpN();
igor_v 21:bc8c1cec3da6 131 //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
igor_v 30:17c84ed091b3 132 LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));
igor_v 30:17c84ed091b3 133 Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096);
Kovalev_D 40:8a6494f61326 134 // LoopOn
igor_v 21:bc8c1cec3da6 135 // LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq);
igor_v 21:bc8c1cec3da6 136 Time_vibro=0;
igor_v 21:bc8c1cec3da6 137 break;
igor_v 21:bc8c1cec3da6 138 case 10:
igor_v 21:bc8c1cec3da6 139
igor_v 0:8ad47e2b6f00 140 // if (MaxAmp>(65535+400))
igor_v 0:8ad47e2b6f00 141 // {
igor_v 0:8ad47e2b6f00 142 // Vib.Amp-=1000;//
igor_v 0:8ad47e2b6f00 143 // }
igor_v 0:8ad47e2b6f00 144 // else
igor_v 0:8ad47e2b6f00 145 // {
igor_v 0:8ad47e2b6f00 146 // Vib.Amp+=1000;
igor_v 0:8ad47e2b6f00 147 // }
igor_v 21:bc8c1cec3da6 148
igor_v 21:bc8c1cec3da6 149
igor_v 21:bc8c1cec3da6 150
igor_v 0:8ad47e2b6f00 151
igor_v 30:17c84ed091b3 152 Gyro.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници
igor_v 21:bc8c1cec3da6 153 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 154 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 155 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 156 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 30:17c84ed091b3 157 if(Gyro.Amp>2500000) {
igor_v 30:17c84ed091b3 158 Gyro.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа//////////
igor_v 21:bc8c1cec3da6 159 }
igor_v 21:bc8c1cec3da6 160 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 161 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 162 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 163 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 164
igor_v 0:8ad47e2b6f00 165
igor_v 30:17c84ed091b3 166 Gyro.AmpPer = Gyro.Amp>>16;
igor_v 21:bc8c1cec3da6 167 MaxAmp=0;
igor_v 30:17c84ed091b3 168 FrecTemp=Gyro.AmpPer;//для вывода в терминал
igor_v 21:bc8c1cec3da6 169 break;
igor_v 21:bc8c1cec3da6 170 case 16:
Kovalev_D 40:8a6494f61326 171
igor_v 21:bc8c1cec3da6 172 //SetV2
igor_v 21:bc8c1cec3da6 173 // LoopOff
igor_v 21:bc8c1cec3da6 174 Time_vibro=0;
igor_v 21:bc8c1cec3da6 175
igor_v 21:bc8c1cec3da6 176
igor_v 21:bc8c1cec3da6 177
igor_v 0:8ad47e2b6f00 178
igor_v 21:bc8c1cec3da6 179
igor_v 0:8ad47e2b6f00 180
igor_v 21:bc8c1cec3da6 181 // расчет Фазы с учетор разници
igor_v 21:bc8c1cec3da6 182 TempFaza = -4;
igor_v 21:bc8c1cec3da6 183 for (CountFaza = 0; CountFaza < 8; CountFaza++ ) {
igor_v 21:bc8c1cec3da6 184 if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++;
igor_v 21:bc8c1cec3da6 185 } // расчет Фазы с учетор разници
igor_v 30:17c84ed091b3 186 Gyro.Frq -=TempFaza*1000;
igor_v 21:bc8c1cec3da6 187
igor_v 21:bc8c1cec3da6 188
igor_v 21:bc8c1cec3da6 189
igor_v 21:bc8c1cec3da6 190
igor_v 0:8ad47e2b6f00 191 // if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535)
igor_v 0:8ad47e2b6f00 192 // {
igor_v 0:8ad47e2b6f00 193 // Vib.Frq=Vib.Frq-10000;
igor_v 0:8ad47e2b6f00 194 // }
igor_v 0:8ad47e2b6f00 195
igor_v 0:8ad47e2b6f00 196 // else
igor_v 0:8ad47e2b6f00 197 // {
igor_v 0:8ad47e2b6f00 198 // Vib.Frq=Vib.Frq+1000;
igor_v 0:8ad47e2b6f00 199 // }
igor_v 21:bc8c1cec3da6 200 break;
igor_v 21:bc8c1cec3da6 201 }
igor_v 21:bc8c1cec3da6 202
igor_v 0:8ad47e2b6f00 203
igor_v 0:8ad47e2b6f00 204
igor_v 0:8ad47e2b6f00 205
igor_v 21:bc8c1cec3da6 206 /*
igor_v 21:bc8c1cec3da6 207 if((CountV & 0x0f) == V1)
igor_v 21:bc8c1cec3da6 208 {
igor_v 21:bc8c1cec3da6 209 ClrV1
igor_v 21:bc8c1cec3da6 210 ClrV2
igor_v 21:bc8c1cec3da6 211 }*/
igor_v 21:bc8c1cec3da6 212 }