Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
Diff: Command.c
- Revision:
- 225:f8fee6c586cc
- Parent:
- 224:598aec95415f
- Child:
- 226:4a4d5bd5fcd7
diff -r 598aec95415f -r f8fee6c586cc Command.c --- a/Command.c Tue Nov 21 07:31:05 2017 +0000 +++ b/Command.c Tue Dec 12 05:53:39 2017 +0000 @@ -39,13 +39,13 @@ } void DropDelay(void) -{ - Gyro.DropDelayGLD = DropDelayGLD_0; - switch(Gyro.My_Addres) { - case 1: Gyro.DropDelayGLD = DropDelayGLD_1; break; - case 2: Gyro.DropDelayGLD = DropDelayGLD_2; break; - case 3: Gyro.DropDelayGLD = DropDelayGLD_3; break; - } + { + Gyro.DropDelayGLD = DropDelayGLD_0; + switch(Gyro.My_Addres) { + case 1: Gyro.DropDelayGLD = DropDelayGLD_1; break; + case 2: Gyro.DropDelayGLD = DropDelayGLD_2; break; + case 3: Gyro.DropDelayGLD = DropDelayGLD_3; break; + } } /* @@ -135,7 +135,7 @@ case 7: if(Gyro.Event_500Hz ) CMD_B_Delta(); break; case 8: if(Gyro.Rate3_Event ) CMD_Rate3(); break; case 9: if(Gyro.Reper_Event ){ CMD_Rate2(); Gyro.ModeOut=0;} break; - case 10: if(Gyro.EvenRate7 ){ CMD_Rate7(); Gyro.EvenRate7=0;} break; + case 10: CMD_Rate7();Gyro.ModeOut=0;break;//if(Gyro.EvenRate7 ){ CMD_Rate7(); Gyro.EvenRate7=0;} break; case 11: if(Gyro.EvenRate5K ){ M_Rate5K(); Gyro.EvenRate5K--;} break; } } @@ -160,114 +160,66 @@ //BuffADC_64Point[ (CountFaza + Gyro.PLC_Phase) & 0x3f] BuffClear(); - int Temp; + unsigned int Temp; + int ADC_Worm[70]; int Temp1; float Temp2; static unsigned int t=0; BuffTemp[0] = Gyro.SOC_Out; BuffTemp[1] = Gyro.My_Addres; - - iRate_7 = Buff_OUT[(t)] ; - BuffTemp[ 2] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 3] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков + MinWorms=BuffADC_10v_F[10]; + MaxWorms=BuffADC_10v_F[10]; + for(int q=0; q<64; q++) + { + if(MinWorms > BuffADC_10v_F[q]) MinWorms = (BuffADC_10v_F[q]); + if(MaxWorms < BuffADC_10v_F[q]) MaxWorms = (BuffADC_10v_F[q]); + } + AmpWorms = (MaxWorms-MinWorms)>>1; + + for(int q=0; q<64; q++) + { + ADC_Worm[q] = (BuffADC_10v_F[q] - (MinWorms+AmpWorms));// от -128 до 128 + } - iRate_7 = Buff_OUT[(t)+8] ; - BuffTemp[ 4] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 5] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = Buff_OUT[(t)+16]; - BuffTemp[ 6] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 7] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = Buff_OUT[(t)+24]; - BuffTemp[ 8] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 9] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = Buff_OUT[(t)+32] ; - BuffTemp[10] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[11] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = Buff_OUT[(t)+40] ; - BuffTemp[12] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[13] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 =Buff_OUT[(t)+ 48]; - BuffTemp[14] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[15] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = Buff_OUT[(t)+56] ; - BuffTemp[16] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[17] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - + for(int q=32; q<64; q++) + { + ADC_Worm[q] = ADC_Worm[q] * (-1); + } + + for(int q=0; q<64; q++) + { + ADC_Worm[q] = ADC_Worm[q] + AmpWorms+1; + Temp = (ADC_Worm[q])>>9; + BuffTemp[q+2] = (Temp) & 0xff;//младший байт разности счетчиков + } - - - - iRate_7 = ModArraySin[(t + Gyro.PLC_Phase)&0x3f] - 50; - BuffTemp[ 18] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 19] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = ModArraySin[(t + 8 + (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 20] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 21] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = ModArraySin[(t + 16 + (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 22] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 23] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - iRate_7 = ModArraySin[(t + 24 + (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 24] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 25] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = ModArraySin[(t + 32 + (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 26] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 27] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = ModArraySin[(t + 40 + (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 28] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 29] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = ModArraySin[(t + 48 + (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 30] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 31] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - iRate_7 = ModArraySin[(t + 56+ (Gyro.PLC_Phase))&0x3f] - 50; - BuffTemp[ 32] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 33] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - - - iRate_7 = (Spi.DAC_B - 0x7fff); - BuffTemp[ 34] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 35] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - - - - Rate_7 = Gyro.PLC_Eror_count>>3; - BuffTemp[ 36] =(Rate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 37] =(Rate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = ((Spi.DAC_B-0x7fff)&0xffff); + BuffTemp[66]=(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[67]=(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + Rate_7 = Gyro.PLC_Eror_count>>3; + BuffTemp[68]=(Rate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[69]=(Rate_7 >> 0) & 0xff;//младший байт разности счетчиков; Gyro.PLC_Eror_count=0; switch(Gyro.LG_Type) { - case 1: iRate_7 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; + case 1: iRate_7 = 0xffff-Spi.DAC_A; break; case 0: Temp2 = (Spi.DAC_A*1.538) - 0xd80f; iRate_7 = (int)Temp2; - //iRate_7 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; } - //Rate_7 = 30000;//(unsigned int)(iRate_7&0xffff); - BuffTemp[ 38] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков - BuffTemp[ 39] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + Rate_7 = (unsigned int)(iRate_7&0xffff); + BuffTemp[70]= (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[71]= (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; - Check(BuffTemp, 42); - WriteConN (BuffTemp,42); - /* if(t<60)t+=32; - else t =0;*/ - + Check(BuffTemp,74); + WriteConN (BuffTemp,74); } @@ -317,12 +269,12 @@ switch(NP) { case 0: Gyro.My_Addres = GyroP.Str.My_Addres; break; - case 1: Gyro.HFO_ref = /*GyroP.Str.HFO_ref>>1;*/(unsigned int)(GyroP.Str.HFO_ref)*0.82; break; + case 1: Gyro.HFO_ref = /*GyroP.Str.HFO_ref>>1;*/(unsigned int)(GyroP.Str.HFO_ref) ; break; case 2: Gyro.HFO_Gain = GyroP.Str.HFO_Gain; break; - case 3: if(Gyro.LG_Type) Gyro.HFO_Max=((int)(GyroP.Str.DAC_current_Work*0.67)-2000); + case 3: if(Gyro.LG_Type) Gyro.HFO_Max=0xffff-GyroP.Str.DAC_current_Work; //((int)(GyroP.Str.DAC_current_Work*0.67)-2000); else Spi.DAC_A = ((((int)(GyroP.Str.DAC_current_Work+ 0x7fff)&0xffff)+22544)*0.65); break; - case 4: if(Gyro.LG_Type) Gyro.HFO_Min=((int)(GyroP.Str.DAC_current_Start*0.67)-2000); break; + case 4: if(Gyro.LG_Type) Gyro.HFO_Min=0xffff-GyroP.Str.DAC_current_Start; break; //((int)(GyroP.Str.DAC_current_Start*0.67)-2000); break; case 5: Gyro.TimeToJump = GyroP.Str.TimeToJump; break; case 6: Gyro.JumpDelta = GyroP.Str.JumpDelta; break; @@ -340,8 +292,7 @@ case 16: Gyro.FrqHZmax = (7680000 / GyroP.Str.FrqHZmax)<<16; break; case 18: temp=((GyroP.Str.VB_Fdf_Hi<<16) | GyroP.Str.VB_Fdf_Lo); - sprintf((Time),"%d \r\n", temp/32); - WriteCon(Time); + temp=temp*20; Gyro.AmpTarget=(unsigned int)(temp); break; @@ -368,13 +319,25 @@ case 60: Gyro.GLD_Serial = GyroP.Str.GLD_Serial; break; - case 10: Gyro.DownTreshold =0x7fff+GyroP.Str.DownTreshold; break; - case 11: Gyro.HighTreshold =0x7fff+GyroP.Str.HighTreshold; break; - case 9: Gyro.PLCDelay = GyroP.Str.PLCDelay/10; break; - /*case 105: Gyro.ResetLevelCool = GyroP.Str.ResetLevelCool; break; - case 55: Gyro.ResetLevelHeat = GyroP.Str.ResetLevelHeat; break;*/ + case 10: Gyro.DownTreshold = (unsigned int)((GyroP.Str.DownTreshold-0x7fff)&0xffff); break; + case 11: Gyro.HighTreshold = (unsigned int)((GyroP.Str.HighTreshold-0x7fff)&0xffff); break; + + case 9: Gyro.PLCDelay = GyroP.Str.PLCDelay; break; + + case 105:Gyro.ResetLevelCool = (unsigned int)((GyroP.Str.ResetLevelCool-0x7fff)&0xffff); break; + case 55: Gyro.ResetLevelHeat = (unsigned int)((GyroP.Str.ResetLevelHeat-0x7fff)&0xffff); break; + + + + + //27 + + /// Gyro.PLCDelay = GyroP.Str.PLCDelay; + //105 + + /* case 39: Gyro.ResetLevelCool = 0x7fff+GyroP.Str.ResetLevelCool; break; - case 31: Gyro.ResetLevelHeat = 0x7fff+GyroP.Str.ResetLevelHeat; break; //27 + case 31: Gyro.ResetLevelHeat = 0x7fff+GyroP.Str.ResetLevelHeat; break; */ //27 //105 @@ -620,7 +583,7 @@ //температурный канал 5 case 20: OutDeltaPS = 0; - OutDeltaPS = Gyro.TermLM;//Spi.ADC1;// //Gyro.Termo;//0xa4=164 + OutDeltaPS = Gyro.Termo;//Spi.ADC1;// //Gyro.Termo;//0xa4=164 BuffTemp[5] =(OutDeltaPS >> 8) & 0xff;//температура break; @@ -643,29 +606,19 @@ void M_Rate5K(void) { - unsigned int Temp; - BuffTemp[ 0] = Gyro.SOC_Out; //1 --старт данных - BuffTemp[ 1] = Gyro.My_Addres; //2 --адрес отвечающего устройствва - + unsigned int Temp; //Temp=Gyro.CuruAngle; //приводим к форме вывода BuffTemp[ 3] =(Gyro.CuruAngle >> 8) & 0xff;//старший байт разности счетчиков 7 --младший байт разности счетчиков BuffTemp[ 2] =(Gyro.CuruAngle >> 0) & 0xff;//младший байт разности счетчиков 8 --старший байт разности счетчиков Gyro.CuruAngle=0; - // Temp =(Spi.DAC_B-0x7fff); + Temp = ((Spi.DAC_B-0x7fff)&0xffff); BuffTemp[5] = (Spi.DAC_B >> 8) & 0xff; //23 Выход регулятора рабочего периметра BuffTemp[4] = (Spi.DAC_B >> 0) & 0xff; - /* switch(Gyro.LG_Type) - { - case 1: Temp =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; - - case 0: // Temp2 = (Spi.DAC_A*1.538) - 0xd80f; - Temp =(int)(((float)(Spi.DAC_A)/0.64)- 56000); break; - } - */ - Temp=Spi.DAC_A; + + Temp = 0xffff - Spi.DAC_A; BuffTemp[7] =(Temp >> 8) & 0xff;//выход регулятора гвч; 11 --выход регулятора ГВЧ BuffTemp[6] =(Temp >> 0) & 0xff; @@ -718,7 +671,7 @@ //(unsigned int)((Gyro.DacIn+30000)*0.6667); switch(Gyro.LG_Type) { - case 1: Temp1 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; + case 1: Temp1 = 0xffff - Spi.DAC_A; break;//(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; case 0: // Temp2 = (Spi.DAC_A*1.538) - 0xd80f; Temp1 =(int)(((float)(Spi.DAC_A)/0.64)- 56000); break; @@ -731,6 +684,7 @@ if(Gyro.LG_Type) Temp =(int)(tempDeltaRegul); else Temp =(unsigned int)((0x7fff-Spi.ADC5)*0.79)<<1; + tempDeltaRegul=0; BuffTemp[12]=(Temp >> 8) & 0xff;//// HFO 13 --сигнал ошибки регулятора ГВЧ BuffTemp[13]=(Temp >> 0) & 0xff;//// 14 --сигнал ошибки регулятора ГВЧ @@ -754,7 +708,7 @@ BuffTemp[20] = (Temp >> 8) & 0xff;//регулятор датчика угла поворота //21 старший байт регулятора датчика угла поворота BuffTemp[21] = (Temp >> 0) & 0xff; //22 - Temp1 =(int)(3300+(0x7fff+(Spi.DAC_B*1.083))); + Temp1 =((Spi.DAC_B-0x7fff)&0xffff);///(3300+(0x7fff+(Spi.DAC_B*1.083))); BuffTemp[22] = (Temp1 >> 8) & 0xff; //23 Выход регулятора рабочего периметра BuffTemp[23] = (Temp1 >> 0) & 0xff; //24 @@ -798,6 +752,7 @@ void CMD_Rate2(void) { unsigned int Temp; + //float y = 52646.45 Gyro.Reper_Event=0; for(int q=0; q<64; q++) { @@ -851,26 +806,14 @@ if(HFO) { Gyro.DacIn=temp; - if(Gyro.LG_Type==1) Spi.DAC_A = ((unsigned int)((Gyro.DacIn+0x7011)*0.6667)); + if(Gyro.LG_Type==1) Spi.DAC_A = 0xffff-Gyro.DacIn; //((unsigned int)((Gyro.DacIn+0x7011)*0.6667)); else Spi.DAC_A = (unsigned int)(0xffff-Gyro.DacIn); //Spi.DAC_A = ((unsigned int)((Gyro.DacIn+23200)*0.64)); - - /*Temp2 = (Spi.DAC_A*1.538) - 0xd80f; - Temp1= (int)Temp2; */ - } - else Spi.DAC_B = (unsigned int)temp; - /* - sprintf((Time),"%d %d %f \r\n", temp, Spi.DAC_A, (temp+10744)*0.79); - WriteCon(Time); - */ - - /*{ - DACF =(temp*K_DAC)+deltaDAC; - Spi.DAC_B =(unsigned int)(DACF) (unsigned int)(temp*K_DAC+deltaDAC); // K_DAC); - }*/ - /* if(Consol) - {*/ - for(int q=0; q<64; q++) + + else Spi.DAC_B = (unsigned int)((temp-0x7fff)&0xffff); + sprintf((Time),"%d %d \r\n", Spi.DAC_B, temp); + WriteCon(Time); + for(int q=0; q<64; q++) { BuffTemp[q]=0; } @@ -880,26 +823,18 @@ BuffTemp[3] =0; Check(BuffTemp, CRC_N); WriteConN (BuffTemp,CRC_N); - /* }*/ } void CMD_M_vib() { unsigned int temp1,temp2,anser; - // anser = BuffTemp[3]>>7; - temp1 = (((BuffTemp[4]<<8) | BuffTemp[5])&0xFFFF); - Gyro.Frq = (122780000/temp1)<<12; - // Gyro.Frq = (103200000/temp1)<<12; + Gyro.Frq = (122780000/temp1)<<12; F_vib=103200000/((Gyro.Frq>>16)*2); temp2 = (((BuffTemp[6]<<8) | BuffTemp[7])&0xFFFF); Gyro.AmpPer = ((((((Gyro.Frq>>12)*200)/16)*temp2)/7680000)/2); Gyro.Amp = (Gyro.AmpPer)<<15; - /*sprintf((Time),"%d \r\n",Gyro.AmpPer); - WriteCon(Time); */ - - // if(anser) - // { + BuffTemp[0] = Gyro.SOC_Out; BuffTemp[1] = Gyro.My_Addres; BuffTemp[2] = Gyro.CMD_In; @@ -907,7 +842,6 @@ BuffTemp[4] = 0x0; Check(BuffTemp, CRC_N); WriteConN (BuffTemp,CRC_N); - // } } void CMD_M_Control_D8()///установка\сброс регистров управления { @@ -1197,7 +1131,7 @@ case PRate2 : Gyro.ModeOut=2; break; case PRate3 : Gyro.ModeOut=8; break; case Rate7 : CMD_Rate7(); break; - case PRate7 : Gyro.ModeOut=10; break; + case PRate7 : Gyro.ModeOut=10; break; case Rate5K : M_Rate5K(); break; case PRate5K: if(Gyro.ModeOut){} @@ -1240,14 +1174,32 @@ void TechLog(void) { unsigned int temp=0; - /*if (ReadCon (Time)) + if (ReadCon (Time)) { int a=0; + + + + + if (Time[0] == 'y') + { //Mod 250 Hz PLCRegul250(); + if( Gyro.Debag) Gyro.Debag=0; + else Gyro.Debag=1; + } + if (Time[0] == 'u') + { //Mod 250 Hz PLCRegul250(); + if( Gyro.Debag2) Gyro.Debag2=0; + else Gyro.Debag2=1; + } + if (Time[0] == 'C') Spi.DAC_A+=200; + if (Time[0] == 'c') Spi.DAC_A-=200; + if (Time[0] == 'X') Spi.DAC_B+=2000; + if (Time[0] == 'x') Spi.DAC_B-=2000; ///////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////// //////////////////////Выбор FlashMod/////////////////////////// //////////////////////////////////////////////////////////////// - + /* if (Time[0] == 'B') Gyro.FlashMod=1; if (Time[0] == 'R') {Gyro.FlashMod=3;} if (Time[0] == 'E') {Gyro.FlashMod=4;} @@ -1284,10 +1236,7 @@ - if (Time[0] == 'C') Spi.DAC_A+=200; - if (Time[0] == 'c') Spi.DAC_A-=200; - if (Time[0] == 'X') Spi.DAC_B+=2000; - if (Time[0] == 'x') Spi.DAC_B-=2000; + @@ -1337,7 +1286,7 @@ } } - + if (Time[0] == 'n') { // выдача технологическая for(int q=0; q<64; q++) @@ -1364,7 +1313,7 @@ - + if (Time[0] == 'u') { // выдача технологическая @@ -1375,14 +1324,7 @@ Gyro.ShowMod2=0; //Mod vibro Hz } - if (Time[0] == 'y') - { //Mod 250 Hz PLCRegul250(); - - if( Gyro.Debag) Gyro.Debag=0; - else Gyro.Debag=1; - - - } + if (Time[0] == 'f') { // выдача технологическая Gyro.CuruAngleLog = 0; //Mod vibro Hz @@ -1420,9 +1362,9 @@ Gyro.Gain_Cos = GyroP.Str.Gain_Cos; Out_G_photo(Gyro.Gain_Sin, Gyro.Gain_Cos); } - + */ - }*/ + } }