Dmitry Kovalev
fork
Diff: Command.c
- Revision:
- 214:4c70e452c491
- Parent:
- 213:9953db9543d6
- Child:
- 215:b58b887fd367
--- a/Command.c Wed Jul 26 13:24:39 2017 +0000
+++ b/Command.c Wed Aug 16 09:00:25 2017 +0000
@@ -6,6 +6,8 @@
unsigned int Rate_Flag;
unsigned int CountBuFFIn;
unsigned int N=0,CRC_N;
+unsigned int Rate_7=0;
+int iRate_7=0;
unsigned int Param1=0;
unsigned int Param2=0;
unsigned int Param3=0;
@@ -123,24 +125,148 @@
{
switch(Gyro.ModeOut)
{
- case 1: if(Gyro.Rate1_Event ) CMD_Rate(); break;
- case 2: if(Gyro.Reper_Event ) CMD_Rate2(); break;
- case 3: if(Gyro.Event_500Hz ) CMD_Delta_PS(); break;
- case 4: if(Gyro.EXT_Latch ) CMD_Delta_PS(); break;
- case 5: if(Gyro.Event_500Hz ) CMD_Delta_Bins(); break;
- case 6: if(Gyro.EXT_Latch ) CMD_B_Delta(); break;
- 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 1: if(Gyro.Rate1_Event ) CMD_Rate(); break;
+ case 2: if(Gyro.Reper_Event ) CMD_Rate2(); break;
+ case 3: if(Gyro.Event_500Hz ) CMD_Delta_PS(); break;
+ case 4: if(Gyro.EXT_Latch ) CMD_Delta_PS(); break;
+ case 5: if(Gyro.Event_500Hz ) CMD_Delta_Bins(); break;
+ case 6: if(Gyro.EXT_Latch ) CMD_B_Delta(); break;
+ 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;
}
}
+void BuffClear(void)
+{
+ for(int q=0; q<60; q++)
+ {
+ BuffTemp[q]=0;
+ }
+}
+void CMD_Rate7(void)
+{
+
+ // (CountV64)&0x3f]*Gyro.ModAmp туды
+ //BuffADC_64Point[ (CountFaza + Gyro.PLC_Phase) & 0x3f]
+ BuffClear();
+
+ int Temp;
+ int Temp1;
+ float Temp2;
+ int Shift = -11463;
+ static unsigned int t=16;
+
+ BuffTemp[0] = Gyro.SOC_Out;
+ BuffTemp[1] = Gyro.My_Addres;
+
+ iRate_7 =(((BuffADC_64Point[(t - 0 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 2] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 3] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков
+
+ iRate_7 = (((BuffADC_64Point[(t - 2 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 4] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 5] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+
+ iRate_7 =(((BuffADC_64Point[(t - 4 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 6] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 7] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((BuffADC_64Point[(t - 6 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 8] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 9] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((BuffADC_64Point[(t - 8 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 10] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 11] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((BuffADC_64Point[(t - 10 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 12] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 13] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((BuffADC_64Point[(t - 12 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 14] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 15] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((BuffADC_64Point[(t - 14 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff;
+ BuffTemp[ 16] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 17] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+
+
+
+
+ iRate_7 = (((ModArraySin[(t - 0)&0x3f])))- 50;
+ BuffTemp[ 18] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 19] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 2)&0x3f])))- 50;
+ BuffTemp[ 20] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 21] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 4)&0x3f])))- 50;
+ BuffTemp[ 22] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 23] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 6)&0x3f])))- 50;
+ BuffTemp[ 24] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 25] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 8)&0x3f])))- 50;
+ BuffTemp[ 26] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 27] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 10)&0x3f])))- 50;
+ BuffTemp[ 28] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 29] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 12)&0x3f])))- 50;
+ BuffTemp[ 30] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 31] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = (((ModArraySin[(t - 14)&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;
+ BuffTemp[ 36] =(Rate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 37] =(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 0: Temp2 = (Spi.DAC_A*1.538) - 0xd80f;
+ iRate_7 = (int)Temp2; break;
+ }
+ //Rate_7 = 30000;//(unsigned int)(iRate_7&0xffff);
+
+ BuffTemp[ 38] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[ 39] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ Check(BuffTemp, 42);
+ WriteConN (BuffTemp,42);
+ if(t<47)t+=16;
+ else t =16;
+
+}
+
+
+
void CMD_M_Param_R(void)
{
+
unsigned int NP=0;
unsigned int Param;
+
NP = BuffTemp[3];
for (int i=0;i<8;i++)
@@ -198,8 +324,8 @@
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);*/
+ sprintf((Time),"%d \r\n", temp/32);
+ WriteCon(Time);
temp=temp*20;
Gyro.AmpTarget=(unsigned int)(temp);
break;
@@ -320,8 +446,8 @@
Gyro.CuruAngle = Gyro.CuruAngle + OldCuruAngle; //(72 град/с)
- OldCuruAngle=Gyro.CuruAngle & 0x7; //сохраняем 3 бит для след измирений
- Temp=Gyro.CuruAngle>>3; //приводим к форме вывода
+ OldCuruAngle=Gyro.CuruAngle & 0xf; //сохраняем 3 бит для след измирений
+ Temp=Gyro.CuruAngle>>4; //приводим к форме вывода
Gyro.CuruAngle = 0;
BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт разности счетчиков
@@ -504,8 +630,8 @@
Gyro.CuruAngle = Gyro.CuruAngle + OldCuruAngle; //(72 град/с)
- OldCuruAngle=Gyro.CuruAngle & 0x1f; //сохраняем 5 бит для след измирений
- Temp=Gyro.CuruAngle>>5; //приводим к форме вывода
+ OldCuruAngle=Gyro.CuruAngle & 0x3f; //сохраняем 5 бит для след измирений
+ Temp=Gyro.CuruAngle>>6; //приводим к форме вывода
Gyro.CuruAngle = 0;
@@ -524,11 +650,12 @@
BuffTemp[ 9] = (Temp >> 0) & 0xff;// 10 --частота расщипления
//(unsigned int)((Gyro.DacIn+30000)*0.6667);
- if(Gyro.LG_Type) {Temp1 =(int)(((Spi.DAC_A/0.6667)-30000)-0x7fff);}
- else
+ switch(Gyro.LG_Type)
{
- Temp2 = ((Spi.DAC_A/0.65)-22544)-0x7fff;
- Temp1= (int)Temp2;
+ case 1: Temp1 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break;
+
+ case 0: Temp2 = (Spi.DAC_A*1.538) - 0xd80f;
+ Temp1= (int)Temp2; break;
}
//Temp1=Gyro.DacIn/*-0x7fff*/;
BuffTemp[10] = (Temp1 >> 8) & 0xff;//выход регулятора гвч; 11 --выход регулятора ГВЧ
@@ -605,10 +732,13 @@
{
unsigned int Temp;
Gyro.Reper_Event=0;
-
+ for(int q=0; q<64; q++)
+ {
+ BuffTemp[q]=0;
+ }
BuffTemp[ 0] = Gyro.SOC_Out;
BuffTemp[ 1] = Gyro.My_Addres;
-
+ /*
Temp=Gyro.CaunPlusReper;
BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт счетчика +.
@@ -619,6 +749,7 @@
BuffTemp[ 4] =(Temp >> 8) & 0xff;//старший байт счетчика -.
BuffTemp[ 5] =(Temp >> 0) & 0xff;//младший байт счетчика -.
+*/
Check(BuffTemp, 8);
WriteConN (BuffTemp,8);
@@ -653,14 +784,12 @@
if(HFO)
{
Gyro.DacIn=temp;
- if(Gyro.LG_Type==1) Spi.DAC_A = (unsigned int)((Gyro.DacIn+30000)*0.6667);
+ if(Gyro.LG_Type==1) Spi.DAC_A = ((unsigned int)((Gyro.DacIn+0x7011)*0.6667));
else Spi.DAC_A = (unsigned int)((temp));
}
else Spi.DAC_B = (unsigned int)temp;
-
-/* sprintf((Time)," %d %d \r\n", Spi.DAC_A,Gyro.DacIn);
- WriteCon(Time);
-*/
+
+
/*
sprintf((Time),"%d %d %f \r\n", temp, Spi.DAC_A, (temp+10744)*0.79);
@@ -688,11 +817,13 @@
// anser = BuffTemp[3]>>7;
temp1 = (((BuffTemp[4]<<8) | BuffTemp[5])&0xFFFF);
- Gyro.Frq = (122780000/temp1)<<12;
-
+ Gyro.Frq = (122780000/temp1)<<12;
+ // Gyro.Frq = (103200000/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);
- /* sprintf((Time),"%d %d \r\n",temp2,Gyro.AmpPer);
+ Gyro.Amp = (Gyro.AmpPer)<<15;
+ /*sprintf((Time),"%d \r\n",Gyro.AmpPer);
WriteCon(Time); */
// if(anser)
@@ -986,11 +1117,14 @@
case 0xDD: TempParam=(BuffTemp[3]) & 0x9f; //Rate
switch(TempParam) {
case Rate1 : CMD_Rate(); break;
- case Rate2 : Gyro.ModeOut=9; break;
+ case Rate2 : Gyro.ModeOut=9; break;
case Rate3 : CMD_Rate3(); break;
case PRate1 : Gyro.ModeOut=1; break;
- case PRate2 : Gyro.ModeOut=2; break;
- case PRate3 : Gyro.ModeOut=8; break;
+ case PRate2 : Gyro.ModeOut=2; break;
+ case PRate3 : Gyro.ModeOut=8; break;
+ case Rate7 : CMD_Rate7(); break;
+ case PRate7 : Gyro.ModeOut=10; break;
+
}
break; //DropDelay(); выставка задержки для мультидропа сбрасывается в команде CMD_Maintenance() на () микросекунд;
case 0xA0: /*DropDelay();*/ TempParam=(BuffTemp[3]) & 0x90; //Delta_PS
@@ -1029,6 +1163,7 @@
unsigned int temp=0;
if (ReadCon (Time))
{
+ int a=0;
/////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
//////////////////////Выбор FlashMod///////////////////////////
@@ -1055,35 +1190,52 @@
WriteCon(Time);
}*/
if (Time[0] == 'V') { // выдача технологическая
- Gyro.Debag += 10;
- }
+ F_vib += 1;
+ LPC_MCPWM->LIM1 = F_vib;
+ LPC_MCPWM->LIM2 = F_vib;
+ sprintf((Time),"%d \r\n", F_vib);
+ WriteCon(Time);
+ }
if (Time[0] == 'v') { // выдача технологическая
- Gyro.Debag -= 10;
-
+ F_vib -= 1;
+ LPC_MCPWM->LIM1 = F_vib;
+ LPC_MCPWM->LIM2 = F_vib;
+
}
if (Time[0] == 'C') { // выдача технологическая
- Gyro.Debag += 1000;
-
+ AmpP += 1;
+ T_vibP = F_vib/10000;
+ T_vib_1 = AmpP * T_vibP;
+ T_vib_2 = T_vibP * (10000-AmpP +1);
+ LPC_MCPWM->MAT2 = T_vib_2;
+ LPC_MCPWM->MAT1 = T_vib_1;
}
if (Time[0] == 'c') { // выдача технологическая
- Gyro.Debag -= 1000;
-
+ AmpP -= 1;
+ T_vibP = F_vib/10000;
+ T_vib_1 = AmpP * T_vibP;
+ T_vib_2 = T_vibP * (10000-AmpP+1 );
+
+ LPC_MCPWM->MAT2 = T_vib_2;
+ LPC_MCPWM->MAT1 = T_vib_1;
}
if (Time[0] == 'X') { // выдача технологическая
- Spi.DAC_A += 3000;
+ Spi.DAC_A += 1000;
sprintf((Time),"%d \r\n", Spi.DAC_A);
WriteCon(Time);
}
if (Time[0] == 'x') { // выдача технологическая
- Spi.DAC_A -= 3000;
+ Spi.DAC_A -= 1000;
sprintf((Time),"%d \r\n", Spi.DAC_A);
WriteCon(Time);
}
if (Time[0] == 'p') { // выдача технологическая
- PlcOFF //Gyro.RgConA&0x8=0;
- Gyro.LogMod=1;
+ // PlcOFF //Gyro.RgConA&0x8=0;
+
+ if(Gyro.LogMod) Gyro.LogMod=0;
+ else Gyro.LogMod=5;
// MODFlag=1;
- Spi.DAC_B = 32767;
+ // Spi.DAC_B = 32767;
}
if (Time[0] == 'o') { // выдача технологическая
if(Gyro.LogMod==2)Gyro.LogMod=0;
@@ -1091,24 +1243,40 @@
// Spi.DAC_B = 32000;
}
+
+
if (Time[0] == 'm')
{ // выдача технологическая
- /* for(q=0; q<32; q++)
- {
- sprintf((Time),"%d ", Buff_1Point[q]);
- WriteCon(Time);
- }
- sprintf((Time),"\r\n");
- WriteCon(Time);
- for(q=0; q<32; q++)
- {
- sprintf((Time),"%d ", Buff_Restored_sin [q]);
- WriteCon(Time);
- }*/
- sprintf((Time),"\r\n");
- WriteCon(Time);
- }
-
+ for(int q=0; q<64; q++)
+ {
+ sprintf((Time),"%d \r\n", BuffADC_64Point[q]);
+ WriteCon(Time);
+ }
+ }
+
+ if (Time[0] == 'n')
+ { // выдача технологическая
+ for(int q=0; q<64; q++)
+ {
+ sprintf((Time),"%d \r\n", BuffADC_32PointD[q]);
+ WriteCon(Time);
+ }
+ }
+ if (Time[0] == 'b')
+ { // выдача технологическая
+ for(int q=0; q<64; q++)
+ {
+ sprintf((Time)," %d \r\n", BuffADC_1Point[ q] );
+ WriteCon(Time);
+ }
+ }
+
+
+
+
+
+
+
if (Time[0] == 'u')
{ // выдача технологическая
MODFlag=0; //Mod vibro Hz