Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@0:8ad47e2b6f00, 2016-01-30 (annotated)

Committer:
igor_v
Date:
Sat Jan 30 13:00:39 2016 +0000
Revision:
0:8ad47e2b6f00
Child:
21:bc8c1cec3da6
2016_01_30;

Who changed what in which revision?

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