Dmitry Kovalev / Mbed 2 deprecated LGstaandart

forkd

Dependencies:   mbed

Fork of LG2 by Dmitry Kovalev

host/Source/App/vibro.c@23:12e6183f04d4, 2016-02-03 (annotated)

Committer:
Kovalev_D
Date:
Wed Feb 03 10:44:42 2016 +0300
Revision:
23:12e6183f04d4
[thyz

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kovalev_D 23:12e6183f04d4 1 #include "vibro.h"
Kovalev_D 23:12e6183f04d4 2 #include "stdlib.h"
Kovalev_D 23:12e6183f04d4 3 #include "lpc17xx.h"
Kovalev_D 23:12e6183f04d4 4 #include "ctime"
Kovalev_D 23:12e6183f04d4 5 #include "InputOutput.h"
Kovalev_D 23:12e6183f04d4 6 #include "Parameters.h"
Kovalev_D 23:12e6183f04d4 7 #include "CntrlGLD.h"
Kovalev_D 23:12e6183f04d4 8 #include "CyclesSync.h"
Kovalev_D 23:12e6183f04d4 9 #include "SIP.h"
Kovalev_D 23:12e6183f04d4 10 //extern OUTPUT Output;
Kovalev_D 23:12e6183f04d4 11 #include "QEI.h"
Kovalev_D 23:12e6183f04d4 12 #include "Global.h"
Kovalev_D 23:12e6183f04d4 13 #include "MTimer.h"
Kovalev_D 23:12e6183f04d4 14
Kovalev_D 23:12e6183f04d4 15 struct MAIN Main;
Kovalev_D 23:12e6183f04d4 16
Kovalev_D 23:12e6183f04d4 17
Kovalev_D 23:12e6183f04d4 18 volatile int V1 = 0 ;
Kovalev_D 23:12e6183f04d4 19 volatile int Temp = 0 ;
Kovalev_D 23:12e6183f04d4 20 volatile unsigned int Flag=0;
Kovalev_D 23:12e6183f04d4 21
Kovalev_D 23:12e6183f04d4 22 unsigned int FrecTemp=0;
Kovalev_D 23:12e6183f04d4 23 /////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 24 /////////////////////////èíèöèàëèçàöèÿ âèáðîïðèâîäà//////////////////////////
Kovalev_D 23:12e6183f04d4 25 /////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 26 unsigned int init_Vibro ()
Kovalev_D 23:12e6183f04d4 27 {
Kovalev_D 23:12e6183f04d4 28 Main.FrqRate=40;
Kovalev_D 23:12e6183f04d4 29 Main.FrqMin=380;
Kovalev_D 23:12e6183f04d4 30 Main.FrqHZ=449;
Kovalev_D 23:12e6183f04d4 31 Main.Frq = Main.FrqHZ<<16;
Kovalev_D 23:12e6183f04d4 32
Kovalev_D 23:12e6183f04d4 33 Main.AmpPer=20;
Kovalev_D 23:12e6183f04d4 34 Main.AmpPerDel=10;
Kovalev_D 23:12e6183f04d4 35 Main.Amp = 10*65535;
Kovalev_D 23:12e6183f04d4 36 Main.AmpL = 20*650;
Kovalev_D 23:12e6183f04d4 37 /*
Kovalev_D 23:12e6183f04d4 38 Vib.AmpH=50*650;
Kovalev_D 23:12e6183f04d4 39 // Vib.AmpT=600;
Kovalev_D 23:12e6183f04d4 40 // Output.Str.T_Vibro=40;
Kovalev_D 23:12e6183f04d4 41 //LPC_GPIO0->FIOSET |= (1<<5);
Kovalev_D 23:12e6183f04d4 42 */
Kovalev_D 23:12e6183f04d4 43 return 0;
Kovalev_D 23:12e6183f04d4 44 }
Kovalev_D 23:12e6183f04d4 45
Kovalev_D 23:12e6183f04d4 46
Kovalev_D 23:12e6183f04d4 47 void VibroOut(void)
Kovalev_D 23:12e6183f04d4 48 {
Kovalev_D 23:12e6183f04d4 49
Kovalev_D 23:12e6183f04d4 50
Kovalev_D 23:12e6183f04d4 51 if(CountV31>=16)
Kovalev_D 23:12e6183f04d4 52 {
Kovalev_D 23:12e6183f04d4 53 if((Time_vibro>Main.AmpN1) && (Time_vibro<Main.AmpN2)) {ClrV2 /*LoopOn*/}
Kovalev_D 23:12e6183f04d4 54 else{ SetV2 }
Kovalev_D 23:12e6183f04d4 55 }
Kovalev_D 23:12e6183f04d4 56 else
Kovalev_D 23:12e6183f04d4 57 {
Kovalev_D 23:12e6183f04d4 58 if((Time_vibro>Main.AmpN1)&&(Time_vibro<Main.AmpN2)) {ClrV1 /*LoopOff*/}
Kovalev_D 23:12e6183f04d4 59 else{ SetV1 }
Kovalev_D 23:12e6183f04d4 60 }
Kovalev_D 23:12e6183f04d4 61
Kovalev_D 23:12e6183f04d4 62
Kovalev_D 23:12e6183f04d4 63
Kovalev_D 23:12e6183f04d4 64 }
Kovalev_D 23:12e6183f04d4 65
Kovalev_D 23:12e6183f04d4 66 void CalcAmpN(void)
Kovalev_D 23:12e6183f04d4 67 {
Kovalev_D 23:12e6183f04d4 68 static int PeriodCount = 0,Tnoise;
Kovalev_D 23:12e6183f04d4 69
Kovalev_D 23:12e6183f04d4 70
Kovalev_D 23:12e6183f04d4 71
Kovalev_D 23:12e6183f04d4 72 unsigned int Nmax=0;
Kovalev_D 23:12e6183f04d4 73
Kovalev_D 23:12e6183f04d4 74
Kovalev_D 23:12e6183f04d4 75 if(PeriodCount>= Tnoise)//åñëè êîëè÷åñòâî çàõîäîâ â ïðåðûâàíèå áîëüøå ëèáî ðàâíî ÷àñòîòå îøóìëåíèÿ.
Kovalev_D 23:12e6183f04d4 76 {
Kovalev_D 23:12e6183f04d4 77 if (Flag==0)//ñåé÷àñ ìàëàÿ àìïëèòóäà?
Kovalev_D 23:12e6183f04d4 78 {
Kovalev_D 23:12e6183f04d4 79
Kovalev_D 23:12e6183f04d4 80 if(Main.AmpPer>90){Main.AmpPer=90;}
Kovalev_D 23:12e6183f04d4 81 Nmax =(unsigned int)((100000/(Main.Frq>>16))-1);
Kovalev_D 23:12e6183f04d4 82 Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer))/400);
Kovalev_D 23:12e6183f04d4 83 Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1);
Kovalev_D 23:12e6183f04d4 84 Flag=1;
Kovalev_D 23:12e6183f04d4 85
Kovalev_D 23:12e6183f04d4 86 }
Kovalev_D 23:12e6183f04d4 87
Kovalev_D 23:12e6183f04d4 88 else
Kovalev_D 23:12e6183f04d4 89 {
Kovalev_D 23:12e6183f04d4 90 if((Main.AmpPer+Main.AmpPerDel)>90){Main.AmpPer=90-Main.AmpPerDel;}
Kovalev_D 23:12e6183f04d4 91
Kovalev_D 23:12e6183f04d4 92 Nmax =(unsigned int)((100000/(Main.Frq>>16))-1);
Kovalev_D 23:12e6183f04d4 93 Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer+Main.AmpPerDel))/400);
Kovalev_D 23:12e6183f04d4 94 Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1);
Kovalev_D 23:12e6183f04d4 95 Flag=0;
Kovalev_D 23:12e6183f04d4 96 }
Kovalev_D 23:12e6183f04d4 97 Main.AmpMin =10;
Kovalev_D 23:12e6183f04d4 98 Main.AmpTD =30;
Kovalev_D 23:12e6183f04d4 99 //Tnoise=55;
Kovalev_D 23:12e6183f04d4 100 srand(Global_Time);
Kovalev_D 23:12e6183f04d4 101 Main.AmpT = (rand() % Main.AmpTD+Main.AmpMin);// ÎØÓÌËÅÍÈÅ amp
Kovalev_D 23:12e6183f04d4 102 Tnoise=Main.AmpT;
Kovalev_D 23:12e6183f04d4 103 //èçìåíåíèå ôëàãà ïðåäèäóùåé àìïëèòóäû
Kovalev_D 23:12e6183f04d4 104 PeriodCount=0;
Kovalev_D 23:12e6183f04d4 105 }
Kovalev_D 23:12e6183f04d4 106 else{PeriodCount++;}
Kovalev_D 23:12e6183f04d4 107
Kovalev_D 23:12e6183f04d4 108
Kovalev_D 23:12e6183f04d4 109
Kovalev_D 23:12e6183f04d4 110 }
Kovalev_D 23:12e6183f04d4 111
Kovalev_D 23:12e6183f04d4 112 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 113 /////////////////////////ôóíêöèÿ ðàáîòû âèáðîïðèâîäà//////////////////////////
Kovalev_D 23:12e6183f04d4 114 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 115 void cheng(void)
Kovalev_D 23:12e6183f04d4 116 {
Kovalev_D 23:12e6183f04d4 117 static int TempFaza, CountFaza;
Kovalev_D 23:12e6183f04d4 118
Kovalev_D 23:12e6183f04d4 119
Kovalev_D 23:12e6183f04d4 120 //Noise();
Kovalev_D 23:12e6183f04d4 121
Kovalev_D 23:12e6183f04d4 122 //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
Kovalev_D 23:12e6183f04d4 123 /*
Kovalev_D 23:12e6183f04d4 124 // CountV++;
Kovalev_D 23:12e6183f04d4 125 // CountV &= 0x1f;
Kovalev_D 23:12e6183f04d4 126 */
Kovalev_D 23:12e6183f04d4 127 if (Buff_Restored_sin [CountV255] > MaxAmp)
Kovalev_D 23:12e6183f04d4 128 {
Kovalev_D 23:12e6183f04d4 129 MaxAmp=Buff_Restored_sin [CountV255];
Kovalev_D 23:12e6183f04d4 130 }
Kovalev_D 23:12e6183f04d4 131
Kovalev_D 23:12e6183f04d4 132
Kovalev_D 23:12e6183f04d4 133
Kovalev_D 23:12e6183f04d4 134
Kovalev_D 23:12e6183f04d4 135
Kovalev_D 23:12e6183f04d4 136 switch(CountV31)
Kovalev_D 23:12e6183f04d4 137 {
Kovalev_D 23:12e6183f04d4 138 case 0:
Kovalev_D 23:12e6183f04d4 139 CalcAmpN();
Kovalev_D 23:12e6183f04d4 140 //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
Kovalev_D 23:12e6183f04d4 141 LPC_TIM1->MR0 =(unsigned int)(100000000/(Main.Frq>>11));
Kovalev_D 23:12e6183f04d4 142 Output.Str.T_Vibro=(unsigned int)((7680000*16/Main.Frq)*4096);
Kovalev_D 23:12e6183f04d4 143 VibroPush();
Kovalev_D 23:12e6183f04d4 144 // LoopOn
Kovalev_D 23:12e6183f04d4 145 // LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq);
Kovalev_D 23:12e6183f04d4 146 Time_vibro=0;
Kovalev_D 23:12e6183f04d4 147 break;
Kovalev_D 23:12e6183f04d4 148 case 10:
Kovalev_D 23:12e6183f04d4 149
Kovalev_D 23:12e6183f04d4 150 // if (MaxAmp>(65535+400))
Kovalev_D 23:12e6183f04d4 151 // {
Kovalev_D 23:12e6183f04d4 152 // Vib.Amp-=1000;//
Kovalev_D 23:12e6183f04d4 153 // }
Kovalev_D 23:12e6183f04d4 154 // else
Kovalev_D 23:12e6183f04d4 155 // {
Kovalev_D 23:12e6183f04d4 156 // Vib.Amp+=1000;
Kovalev_D 23:12e6183f04d4 157 // }
Kovalev_D 23:12e6183f04d4 158
Kovalev_D 23:12e6183f04d4 159
Kovalev_D 23:12e6183f04d4 160
Kovalev_D 23:12e6183f04d4 161
Kovalev_D 23:12e6183f04d4 162 Main.Amp -= (MaxAmp - 65536 - 300)*4; // ðàñ÷åò àìïëèòóäâ ñ ó÷åòîì ðàçíèöè
Kovalev_D 23:12e6183f04d4 163 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 164 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 165 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 166 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 167 if(Main.Amp>2500000){Main.Amp=1200000;}// âðåìåííîå îãðàíè÷åíèå ðîñòà àìïëèòóäû â ñëó÷àå íåïîäîæåííîãî ãèðîñêîïà//////////
Kovalev_D 23:12e6183f04d4 168 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 169 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 170 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 171 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 23:12e6183f04d4 172
Kovalev_D 23:12e6183f04d4 173
Kovalev_D 23:12e6183f04d4 174 Main.AmpPer = Main.Amp>>16;
Kovalev_D 23:12e6183f04d4 175 MaxAmp=0;
Kovalev_D 23:12e6183f04d4 176 FrecTemp=Main.AmpPer;//äëÿ âûâîäà â òåðìèíàë
Kovalev_D 23:12e6183f04d4 177 break;
Kovalev_D 23:12e6183f04d4 178 case 16:
Kovalev_D 23:12e6183f04d4 179 VibroPull();
Kovalev_D 23:12e6183f04d4 180 //SetV2
Kovalev_D 23:12e6183f04d4 181 // LoopOff
Kovalev_D 23:12e6183f04d4 182 Time_vibro=0;
Kovalev_D 23:12e6183f04d4 183
Kovalev_D 23:12e6183f04d4 184
Kovalev_D 23:12e6183f04d4 185
Kovalev_D 23:12e6183f04d4 186
Kovalev_D 23:12e6183f04d4 187
Kovalev_D 23:12e6183f04d4 188
Kovalev_D 23:12e6183f04d4 189 // ðàñ÷åò Ôàçû ñ ó÷åòîð ðàçíèöè
Kovalev_D 23:12e6183f04d4 190 TempFaza = -4;
Kovalev_D 23:12e6183f04d4 191 for (CountFaza = 0; CountFaza < 8; CountFaza++ )
Kovalev_D 23:12e6183f04d4 192 {
Kovalev_D 23:12e6183f04d4 193 if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++;
Kovalev_D 23:12e6183f04d4 194 } // ðàñ÷åò Ôàçû ñ ó÷åòîð ðàçíèöè
Kovalev_D 23:12e6183f04d4 195 Main.Frq -=TempFaza*1000;
Kovalev_D 23:12e6183f04d4 196
Kovalev_D 23:12e6183f04d4 197
Kovalev_D 23:12e6183f04d4 198
Kovalev_D 23:12e6183f04d4 199
Kovalev_D 23:12e6183f04d4 200 // if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535)
Kovalev_D 23:12e6183f04d4 201 // {
Kovalev_D 23:12e6183f04d4 202 // Vib.Frq=Vib.Frq-10000;
Kovalev_D 23:12e6183f04d4 203 // }
Kovalev_D 23:12e6183f04d4 204
Kovalev_D 23:12e6183f04d4 205 // else
Kovalev_D 23:12e6183f04d4 206 // {
Kovalev_D 23:12e6183f04d4 207 // Vib.Frq=Vib.Frq+1000;
Kovalev_D 23:12e6183f04d4 208 // }
Kovalev_D 23:12e6183f04d4 209 break;
Kovalev_D 23:12e6183f04d4 210 }
Kovalev_D 23:12e6183f04d4 211
Kovalev_D 23:12e6183f04d4 212
Kovalev_D 23:12e6183f04d4 213
Kovalev_D 23:12e6183f04d4 214
Kovalev_D 23:12e6183f04d4 215 /*
Kovalev_D 23:12e6183f04d4 216 if((CountV & 0x0f) == V1)
Kovalev_D 23:12e6183f04d4 217 {
Kovalev_D 23:12e6183f04d4 218 ClrV1
Kovalev_D 23:12e6183f04d4 219 ClrV2
Kovalev_D 23:12e6183f04d4 220 }*/
Kovalev_D 23:12e6183f04d4 221 }