Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@48:8697dfe679b9, 2016-02-10 (annotated)

Committer:
igor_v
Date:
Wed Feb 10 15:58:16 2016 +0000
Revision:
48:8697dfe679b9
Parent:
40:8a6494f61326
Child:
82:3b2399101978
Child:
85:0466ee8cdfc8
????????? ????????????? ??? (???????) ???? ?????????

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