Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@21:bc8c1cec3da6, 2016-02-03 (annotated)

Committer:
igor_v
Date:
Wed Feb 03 07:19:30 2016 +0000
Revision:
21:bc8c1cec3da6
Parent:
0:8ad47e2b6f00
Child:
28:1c9acd3b224d
Child:
30:17c84ed091b3
?????????? ??????? ??????????, ???????? ??? ?????? ?????????? ??????????? ?????????.

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