Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@85:0466ee8cdfc8, 2016-03-23 (annotated)

Committer:
Kovalev_D
Date:
Wed Mar 23 11:10:55 2016 +0000
Revision:
85:0466ee8cdfc8
Parent:
48:8697dfe679b9
Child:
87:7e575d26d6d0
.

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 85:0466ee8cdfc8 10
Kovalev_D 85:0466ee8cdfc8 11 void Discharg ()
Kovalev_D 85:0466ee8cdfc8 12 {
Kovalev_D 85:0466ee8cdfc8 13 if (Gyro.Discharg)
Kovalev_D 85:0466ee8cdfc8 14 {
Kovalev_D 85:0466ee8cdfc8 15 //Проверка поджига
Kovalev_D 85:0466ee8cdfc8 16 Gyro.Discharg = Gyro.Discharg >> 1;
Kovalev_D 85:0466ee8cdfc8 17 if (Gyro.Discharg & 0x01) Gyro.PinReg |= PinRegBitD; else Gyro.PinReg &= ~PinRegBitD;
Kovalev_D 85:0466ee8cdfc8 18 }
Kovalev_D 85:0466ee8cdfc8 19 }
Kovalev_D 85:0466ee8cdfc8 20 void BackLight ()
Kovalev_D 85:0466ee8cdfc8 21 {
Kovalev_D 85:0466ee8cdfc8 22 if (Gyro.BackLight)
Kovalev_D 85:0466ee8cdfc8 23 { //Проверка подсветки
Kovalev_D 85:0466ee8cdfc8 24 Gyro.BackLight = Gyro.BackLight >> 1;
Kovalev_D 85:0466ee8cdfc8 25 if (Gyro.BackLight & 0x01) Gyro.PinReg |= PinRegBitL; else Gyro.PinReg &= ~PinRegBitL;
Kovalev_D 85:0466ee8cdfc8 26 }
Kovalev_D 85:0466ee8cdfc8 27 }
Kovalev_D 85:0466ee8cdfc8 28
Kovalev_D 85:0466ee8cdfc8 29
Kovalev_D 28:1c9acd3b224d 30 void init_Vibro ()
igor_v 21:bc8c1cec3da6 31 {
Kovalev_D 85:0466ee8cdfc8 32
Kovalev_D 85:0466ee8cdfc8 33
igor_v 30:17c84ed091b3 34 Gyro.FrqHZ=449;
igor_v 30:17c84ed091b3 35 Gyro.Frq = Gyro.FrqHZ<<16;
igor_v 21:bc8c1cec3da6 36
igor_v 30:17c84ed091b3 37 Gyro.AmpPer=20;
igor_v 30:17c84ed091b3 38 Gyro.AmpPerDel=10;
igor_v 30:17c84ed091b3 39 Gyro.Amp = 10*65535;
Kovalev_D 85:0466ee8cdfc8 40
Kovalev_D 85:0466ee8cdfc8 41 Gyro.AmpMin =10;// минимальное значение AmpT;
Kovalev_D 85:0466ee8cdfc8 42 Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления)
Kovalev_D 85:0466ee8cdfc8 43
igor_v 21:bc8c1cec3da6 44 }
igor_v 21:bc8c1cec3da6 45
igor_v 0:8ad47e2b6f00 46
Kovalev_D 85:0466ee8cdfc8 47 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 48 {
Kovalev_D 85:0466ee8cdfc8 49 if(CountV31>=16) {
Kovalev_D 85:0466ee8cdfc8 50 // 50 75
igor_v 30:17c84ed091b3 51 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) {
Kovalev_D 85:0466ee8cdfc8 52 Gyro.PinReg &= ~PinRegBit_1V;/*LoopOn*///установить в регистре PinReg бит "вибро 1" в "0"
igor_v 21:bc8c1cec3da6 53 } else {
Kovalev_D 40:8a6494f61326 54 Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 55 }
Kovalev_D 85:0466ee8cdfc8 56
igor_v 21:bc8c1cec3da6 57 } else {
igor_v 30:17c84ed091b3 58 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) {
Kovalev_D 40:8a6494f61326 59 Gyro.PinReg &= ~PinRegBit_2V; /*LoopOff*////установить в регистре PinReg бит "вибро 2" в "0"
igor_v 21:bc8c1cec3da6 60 } else {
Kovalev_D 40:8a6494f61326 61 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 62 }
igor_v 21:bc8c1cec3da6 63 }
igor_v 0:8ad47e2b6f00 64
igor_v 0:8ad47e2b6f00 65
igor_v 0:8ad47e2b6f00 66
igor_v 0:8ad47e2b6f00 67 }
igor_v 0:8ad47e2b6f00 68
igor_v 0:8ad47e2b6f00 69 void CalcAmpN(void)
igor_v 0:8ad47e2b6f00 70 {
Kovalev_D 85:0466ee8cdfc8 71 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 72 unsigned int Nmax=0;
igor_v 21:bc8c1cec3da6 73
Kovalev_D 85:0466ee8cdfc8 74 //расчет амплитуды относительно центральной точки
igor_v 21:bc8c1cec3da6 75
Kovalev_D 85:0466ee8cdfc8 76 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 85:0466ee8cdfc8 77 PeriodCount=0;
igor_v 21:bc8c1cec3da6 78
Kovalev_D 85:0466ee8cdfc8 79 if (Flag==0) { //сейчас малая амплитуда?
Kovalev_D 85:0466ee8cdfc8 80 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 85:0466ee8cdfc8 81 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды
igor_v 21:bc8c1cec3da6 82 }
Kovalev_D 85:0466ee8cdfc8 83 //250 100000 / 399
Kovalev_D 85:0466ee8cdfc8 84 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); //
Kovalev_D 85:0466ee8cdfc8 85 // 50 (( 250 * (100-20)) /400) //
Kovalev_D 85:0466ee8cdfc8 86 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400); //левая граница амплитуды
Kovalev_D 85:0466ee8cdfc8 87 // 75 ((250 /2)-50)
Kovalev_D 85:0466ee8cdfc8 88 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
igor_v 21:bc8c1cec3da6 89 Flag=1;
igor_v 21:bc8c1cec3da6 90
igor_v 21:bc8c1cec3da6 91 }
igor_v 21:bc8c1cec3da6 92
igor_v 21:bc8c1cec3da6 93 else {
igor_v 30:17c84ed091b3 94 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 85:0466ee8cdfc8 95 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды
igor_v 21:bc8c1cec3da6 96 }
Kovalev_D 85:0466ee8cdfc8 97 //250
igor_v 30:17c84ed091b3 98 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 85:0466ee8cdfc8 99 //56 ((250*(100-20+10))/400)
Kovalev_D 85:0466ee8cdfc8 100 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400);//левая граница амплитуды
Kovalev_D 85:0466ee8cdfc8 101 //69 ((250/2)-56
Kovalev_D 85:0466ee8cdfc8 102 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
igor_v 21:bc8c1cec3da6 103 Flag=0;
igor_v 21:bc8c1cec3da6 104 }
Kovalev_D 85:0466ee8cdfc8 105
igor_v 21:bc8c1cec3da6 106 //Tnoise=55;
Kovalev_D 85:0466ee8cdfc8 107 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 85:0466ee8cdfc8 108
igor_v 30:17c84ed091b3 109 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
igor_v 21:bc8c1cec3da6 110 //изменение флага предидущей амплитуды
Kovalev_D 85:0466ee8cdfc8 111
igor_v 21:bc8c1cec3da6 112 } else {
igor_v 21:bc8c1cec3da6 113 PeriodCount++;
igor_v 21:bc8c1cec3da6 114 }
igor_v 0:8ad47e2b6f00 115 }
igor_v 0:8ad47e2b6f00 116
igor_v 0:8ad47e2b6f00 117 //////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 118 /////////////////////////функция работы вибропривода//////////////////////////
igor_v 0:8ad47e2b6f00 119 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 120 void cheng(void)
igor_v 21:bc8c1cec3da6 121 {
igor_v 21:bc8c1cec3da6 122 static int TempFaza, CountFaza;
Kovalev_D 85:0466ee8cdfc8 123 if (Buff_Restored_sin [CountV255] > MaxAmp) {MaxAmp=Buff_Restored_sin [CountV255];}
Kovalev_D 85:0466ee8cdfc8 124 switch(CountV31) {
igor_v 21:bc8c1cec3da6 125 case 0:
Kovalev_D 85:0466ee8cdfc8 126 CalcAmpN();//расчет ошумления и амплитуды задание интервалов выставки ножек вибро.
Kovalev_D 85:0466ee8cdfc8 127 //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; //прием частоты из аск_глд старый протокол
Kovalev_D 85:0466ee8cdfc8 128 LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));//запись в таймер 1 значение частоты вибро
Kovalev_D 85:0466ee8cdfc8 129 Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); //запись частоты для выдачи в аск_глд старый протокол
igor_v 21:bc8c1cec3da6 130 Time_vibro=0;
igor_v 21:bc8c1cec3da6 131 break;
igor_v 21:bc8c1cec3da6 132 case 10:
igor_v 30:17c84ed091b3 133 Gyro.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници
igor_v 21:bc8c1cec3da6 134 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 30:17c84ed091b3 135 if(Gyro.Amp>2500000) {
igor_v 30:17c84ed091b3 136 Gyro.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа//////////
igor_v 21:bc8c1cec3da6 137 }
igor_v 21:bc8c1cec3da6 138 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 139
igor_v 30:17c84ed091b3 140 Gyro.AmpPer = Gyro.Amp>>16;
igor_v 21:bc8c1cec3da6 141 MaxAmp=0;
igor_v 21:bc8c1cec3da6 142 break;
Kovalev_D 85:0466ee8cdfc8 143
igor_v 21:bc8c1cec3da6 144 case 16:
igor_v 21:bc8c1cec3da6 145 Time_vibro=0;
igor_v 21:bc8c1cec3da6 146
igor_v 21:bc8c1cec3da6 147 // расчет Фазы с учетор разници
igor_v 21:bc8c1cec3da6 148 TempFaza = -4;
igor_v 21:bc8c1cec3da6 149 for (CountFaza = 0; CountFaza < 8; CountFaza++ ) {
igor_v 21:bc8c1cec3da6 150 if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++;
igor_v 21:bc8c1cec3da6 151 } // расчет Фазы с учетор разници
igor_v 30:17c84ed091b3 152 Gyro.Frq -=TempFaza*1000;
igor_v 21:bc8c1cec3da6 153 break;
igor_v 21:bc8c1cec3da6 154 }
igor_v 21:bc8c1cec3da6 155
igor_v 21:bc8c1cec3da6 156 }