Diff: Command.c

Revision:

0:8ad47e2b6f00

Child:

17:44b2801ac94b

Child:

21:bc8c1cec3da6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Command.c	Sat Jan 30 13:00:39 2016 +0000
@@ -0,0 +1,462 @@
+#include "stdlib.h"
+#include "LPC17xx.h"
+#include "Command.h"
+#include "el_lin.h"
+#include "vibro.h"
+#include "Global.h"
+#include "uart_m.h"
+#include "CntrlGLD.h"
+#include "QEI.h"
+#include "InputOutput.h"
+#include "SPI.h"
+unsigned int Rate_Flag;
+unsigned int PC_Comand;
+unsigned int n;
+unsigned int CountBuFFIn;
+unsigned int ParamIn;
+unsigned int Err;
+	unsigned int N=0,CRC_N;
+	unsigned int Param1=0;
+	unsigned int Param2=0;
+	unsigned int Param3=0;	
+	unsigned int Param4=0;
+	
+/*
+void exec_CMD(void)       //r. === ��������� ������������� ���������� � ���������� �������
+{
+	unsigned int Comand_code;
+	
+	//rx_buf_copy = 1;                  //r. ����������� ����� ����������� ��������� ������
+
+	Comand_code = (rcv_buf[2] & 0xFF) << 8;
+	PC_Comand = Comand_code | (rcv_buf[3] & 0xFF);	//e. save it in the memory for echo-transmission //r. ��������� ��� � ������ ��� �������� ��������
+}*/
+void CMD_Maintenance(void)
+{
+	BuffTemp[0] = Main.SOC_Out;
+	BuffTemp[1] = Main.My_Addres;
+	BuffTemp[2] = Main.Firmware_Version;
+	BuffTemp[3] = Main.GLD_Serial;
+	BuffTemp[4]=0x00;
+	BuffTemp[5]=0x00;
+	Check(BuffTemp, 8);
+	WriteConN (BuffTemp,8);
+}
+
+void CMD_Rate(void)
+{	
+	
+	unsigned int T_VibH   , T_VibL;				// ������� ������������.
+	unsigned int Cnt_PlsL , Cnt_PlsH;			// ������� +.
+	unsigned int Cnt_MnsL , Cnt_MnsH;			// ������� -.
+	unsigned int Cnt_DifL , Cnt_DifH;			// �������� ���������.
+  unsigned int F_rasL   , F_rasH;				// ������� �����������.
+	
+	unsigned int HF_regL  , HF_regH;      // ����� ���������� ���.               �� �������
+	unsigned int HF_difL  , HF_difH;      // ������ ������ ����������� ���.      �� �������
+	
+	unsigned int WP_regL  , WP_regH; 
+	
+	unsigned int ADC1L, ADC2L, ADC3L, ADC4L, ADC5L; 
+	unsigned int ADC1H, ADC2H, ADC3H, ADC4H, ADC5H; 
+	
+	
+	LoopOn	
+//	Main.T_Vib=(unsigned int)((7680000*16/Main.Frq)*4096);
+	Main.T_Vib=(unsigned int)((7680000*16/(Main.Frq>>12)));
+  Main.Firmware_Version=0xff; /// ������������� �������
+	
+	T_VibH   = ( Main.T_Vib>>8  ) TakeByte;
+	T_VibL   = ( Main.T_Vib     ) TakeByte;
+	
+	Cnt_PlsH = ( Main.Cnt_Pls>>8) TakeByte;
+	Cnt_PlsL = ( Main.Cnt_Pls   ) TakeByte;
+	
+	Cnt_MnsH = ( Main.Cnt_Mns>>8) TakeByte;
+	Cnt_MnsL = ( Main.Cnt_Mns   ) TakeByte;
+	
+	Cnt_DifH = ( Main.Cnt_Dif>>8) TakeByte;
+	Cnt_DifL = ( Main.Cnt_Dif   ) TakeByte;
+	
+	F_rasH   = ( Main.F_ras>>8  ) TakeByte;
+	F_rasL   = ( Main.F_ras     ) TakeByte;
+	
+	ADC1H    = ( Spi.ADC1>>8    ) TakeByte;
+	ADC1L		 = ( Spi.ADC1       ) TakeByte;
+	
+	ADC2H    = ( Spi.ADC2>>8    ) TakeByte;
+	ADC2L		 = ( Spi.ADC2       ) TakeByte;
+	
+	ADC3H    = ( Spi.ADC3>>8    ) TakeByte;
+	ADC3L		 = ( Spi.ADC3       ) TakeByte;
+	
+	ADC4H    = ( Spi.ADC4>>8    ) TakeByte;
+	ADC4L		 = ( Spi.ADC4       ) TakeByte;
+	
+	ADC5H    = ( Spi.ADC5>>8    ) TakeByte;
+	ADC5L		 = ( Spi.ADC5       ) TakeByte;
+	
+	WP_regH  = ( Spi.DAC_B>>8   ) TakeByte;
+	WP_regL  = ( Spi.DAC_B      ) TakeByte;
+	
+	HF_regH=0xff;
+	HF_regL=0xff;
+
+  HF_difH=0xff;
+	HF_difL=0xff;
+
+	
+	BuffTemp[ 0] = Main.SOC_Out;
+	BuffTemp[ 1] = Main.My_Addres;
+		
+	BuffTemp[ 2] = Cnt_PlsH;//������� ���� �������� +.
+	BuffTemp[ 3] = Cnt_PlsL;//������� ���� �������� +.
+	
+	BuffTemp[ 4] = Cnt_MnsH;//������� ���� �������� -.
+	BuffTemp[ 5] = Cnt_MnsL;//������� ���� �������� -.
+	
+	BuffTemp[ 6] = Cnt_DifH;//������� ���� �������� ���������
+	BuffTemp[ 7] = Cnt_DifL;//������� ���� �������� ���������
+	
+	BuffTemp[ 8] = F_rasH;
+	BuffTemp[ 9] = F_rasL;
+	
+	BuffTemp[10] = HF_regH;
+	BuffTemp[11] = HF_regL;
+	
+	BuffTemp[12] = HF_difH;
+	BuffTemp[13] = HF_difL;
+	
+	BuffTemp[14] = T_VibH ;//
+	BuffTemp[15] = T_VibL; //?
+	
+	BuffTemp[16] = Main.Firmware_Version;//��
+	BuffTemp[17] = Main.Firmware_Version;//��
+	
+	BuffTemp[18] = T_VibH>>1;
+	BuffTemp[19] = T_VibL;
+	
+	BuffTemp[20] = Main.Firmware_Version;//��
+	BuffTemp[21] = Main.Firmware_Version;//��
+	
+	BuffTemp[22] = WP_regH;              //���    Spi.DAC_B
+	BuffTemp[23] = WP_regL;              //���
+	
+	BuffTemp[24] = Main.Firmware_Version;// ��
+	BuffTemp[25] = Main.Firmware_Version;// ��
+	
+	BuffTemp[26] = ADC1H;
+	BuffTemp[27] = ADC1L;
+	
+	BuffTemp[28] = ADC1H;
+	BuffTemp[29] = ADC1L;
+	
+	BuffTemp[30] = ADC1H;
+	BuffTemp[31] = ADC1L;
+	
+  BuffTemp[32] = ADC1H;
+	BuffTemp[33] = ADC1L;
+	
+	BuffTemp[34] = ADC1H;
+	BuffTemp[35] = ADC1L;
+	
+	BuffTemp[36] = 0x00;//ADC6
+	BuffTemp[37] = 0x00;//ADC6
+	
+	BuffTemp[38] = Main.Firmware_Version;
+	BuffTemp[39] = Main.Firmware_Version;
+	BuffTemp[40] = Main.Firmware_Version;
+	BuffTemp[41] = Main.Firmware_Version;
+	
+	
+	Check(BuffTemp, 44);
+	WriteConN (BuffTemp,44);
+	LoopOff
+}
+
+void CMD_M_Control_D8()///���������\����� ��������� ����������
+{ 
+	int bit,NReg;
+	BuffTemp[0] = Main.SOC_Out;      //DD
+	BuffTemp[1] = Main.My_Addres;    //00
+	BuffTemp[2] = Main.CMD_In;       //D8
+             //10?	
+	if((Param1&0x80))
+	 {
+		bit=1;
+	 }
+	 else 
+	   {
+		  bit=0;
+	   }
+	
+	if ((Param1 & 0x10) == 0) 
+	 {
+		 NReg=0;
+		switch(Param1&0xf)  // Main.RgConA
+	     {
+		    case 0x0:
+					Main.RgConA|=bit;
+		    break;
+				
+				case 0x1:
+					Main.RgConA|=bit<<0x1;
+		    break;
+				
+				case 0x2:
+					Main.RgConA|=bit<<0x2;
+		    break;
+				
+				case 0x3:
+					Main.RgConA|=bit<<0x3;
+		    break;
+				
+				case 0x4:
+					Main.RgConA|=bit<<0x4;
+		    break;
+				
+				case 0x5:
+					Main.RgConA|=bit<<0x5;
+		    break;
+				
+				case 0x6:
+					Main.RgConA|=bit<<0x6;
+		    break;
+				
+				case 0x7:
+					Main.RgConA|=bit<<0x7;
+		    break;
+	     }
+		BuffTemp[3] = NReg<<4; 
+		BuffTemp[4] = (Main.RgConA>>8 )& 0xff;
+		BuffTemp[5] =  Main.RgConA     & 0xff;
+	}
+	else
+	{
+		 NReg=1;            // Main.RgConB
+		switch(Param1&0xf)  // Main.RgConB
+    	{
+				case 0x0:
+				Main.RgConB|=bit;
+				break;
+				case 0x1:
+				Main.RgConB|=bit<1;
+				break;
+				case 0x2:
+				Main.RgConB|=bit<2;
+				break;
+	}
+		BuffTemp[3] =  NReg<<4; 
+		BuffTemp[4] = (Main.RgConB>>8 ) & 0xff;
+		BuffTemp[5] =  Main.RgConB      & 0xff;
+	}
+ 
+	Check(BuffTemp, CRC_N);
+	WriteConN (BuffTemp,CRC_N);
+}
+
+ void CMD_M_Control_D9()///������ ��������� ����������
+ {
+	int bit,NReg;
+	BuffTemp[0] = Main.SOC_Out;      //DD
+	BuffTemp[1] = Main.My_Addres;    //00
+	BuffTemp[2] = Main.CMD_In;       //D9
+	 if ((Param1 & 0x10) == 0) 
+	 {
+		 BuffTemp[3]=0<<4;
+		 BuffTemp[4] = (Main.RgConA>>8 )& 0xff;
+		 BuffTemp[5] =  Main.RgConA     & 0xff;
+	 }
+	 else
+	 {
+		 BuffTemp[3]=1<<4;
+		 BuffTemp[4] = (Main.RgConB>>8 ) & 0xff;
+		 BuffTemp[5] =  Main.RgConB      & 0xff;
+	 }
+	Check(BuffTemp, CRC_N);
+	WriteConN (BuffTemp,CRC_N);
+ }
+// (����� (���) ���� 0...3, ������� ���� ���������� ����, ������� ���� ���� ����)
+void CMD_M_Stymul()
+{
+	int temp;
+	 temp =((BuffTemp[4]<<8) | BuffTemp[5]);
+			 // temp=0;
+	 temp=temp&0xFFFF;
+   DACF =(temp*K_DAC)+deltaDAC;
+   Spi.DAC_B =(unsigned int)(DACF) /*(unsigned int)(temp*K_DAC+deltaDAC)*/; // K_DAC);
+	
+	//int temp;
+//	temp =((BuffTemp[4]<<8) | BuffTemp[5]);
+	//temp = 65535 - temp;
+	/*if(temp<1000)
+	{
+		temp=0;
+	}
+	else if(temp>60000)
+	{
+		temp=65535;
+	}*/
+	//Spi.DAC_B =(((BuffTemp[4]<<8) | BuffTemp[5])*K_DAC+deltaDAC);/*((BuffTemp[4]<<8) | BuffTemp[5])-0xFFFF*///;  //((rcv_buf[4] << 8) | (int)rcv_buf[5]);K_DAC+deltaDAC
+	//BuffTemp[0] = temp;      //DD
+	//WriteConN (BuffTemp,1);
+}
+
+unsigned int Check(char *c, unsigned int Count)
+{
+	int i=1;
+	unsigned int temp,CRC;
+
+	
+	temp=1;
+	CRC=0;
+	
+	
+	for(;i<Count-2;i++)
+	{
+		CRC+=c[i];
+	}
+	
+	if(c[Count-2]!=((CRC>>8)&0xFF))
+	{
+		temp=0;
+		Main.RsErrLine = (Main.RsErrLine)&=0x2;
+	}
+
+	if(c[Count-1]!=((CRC>>0)&0xFF))
+	{
+		temp=0;
+//		Main.RsErrLine= (Main.RsErrLine)=0x2;
+	}
+	
+	c[Count-2]=(CRC>>8)&0xFF;
+	c[Count-1]=(CRC>>0)&0xFF;
+	
+	
+	
+	return temp;
+}
+
+int Getlengf(void)
+{
+	unsigned int lengf=0;
+	switch(Main.CMD_In)
+		{ 
+			case 0x99://maintainance
+      lengf=6;	
+			CRC_N=8;
+			break;
+			
+			case 0x0A: //m_stymul
+			lengf=8;
+			break;
+			
+			case 0xA5://DeviceMode
+
+      lengf=6;
+      CRC_N=8;			
+			break;
+			
+			case 0xDD://m_rate
+
+      lengf=6;
+      CRC_N=44;			
+			break;
+			
+			case 0xB0://DeviceMode
+
+      lengf=6;
+      CRC_N=8;			
+			break;
+			
+			case 0xD8://m_control
+		
+      lengf=6;	
+      CRC_N=8;			
+			break;
+			
+			case 0xD9://m_control
+
+      lengf=6;	
+      CRC_N=8;			
+			break;
+		}
+	return lengf;
+}
+
+void Read_CMD(void)
+{	
+	Main.SOC_Out=0xDD;	
+	Main.RsErrLine = (Main.RsErrLine)& 0xffff; 
+
+	
+	CountBuFFIn=ReadChekCon(BuffTemp);
+	if(CountBuFFIn==1) 	
+        {
+				if (BuffTemp[0] != SOC_In) 
+					{
+						ReadCon(BuffTemp); 
+						Main.RsErrLine += 0x100;//WriteCon("\n\r ...Error.... ");
+						BuffTemp[99]=Main.RsErrLine;
+					}
+				}
+	else if(CountBuFFIn==2) 
+				{
+				if (BuffTemp[1] != Main.My_Addres) 
+					{
+						ReadCon(BuffTemp);  
+						Main.RsErrLine += 0x1;//WriteCon("\n\r ...Error_ADDRES.... ");
+					}
+				}
+	else if(CountBuFFIn==3) 	
+				{
+				Main.CMD_In=BuffTemp[2];
+				N=Getlengf();
+				}		
+				
+	else if(CountBuFFIn==4 && N==6) 	
+				{
+				 Param1=BuffTemp[3];
+				}			
+	else if((CountBuFFIn==5)&&(N==7)) 	
+				{
+				//LoopOn
+			  Param1=BuffTemp[3];
+			  Param2=BuffTemp[4];
+					 
+				}				
+	else if((CountBuFFIn==6)&&(N==8)) 	
+				{
+			  	Param1=BuffTemp[3];
+			    Param2=BuffTemp[4];
+					Param3=BuffTemp[5];
+				}							
+	else if(CountBuFFIn > (N-1)) 
+			  {
+				ReadCon(BuffTemp);
+				if(Check(BuffTemp, CountBuFFIn))
+				  {	
+						switch(Main.CMD_In)
+						{
+							case CMD_MAINT: 
+					    CMD_Maintenance();
+							break;
+							case 0xD8: 
+					    CMD_M_Control_D8();
+							case 0xD9: 
+					    CMD_M_Control_D9();
+							break;
+							case 0x0A: 
+					    CMD_M_Stymul(Param1,Param2,Param3);
+							break;
+							case 0xDD:
+							Rate_Flag=1;
+						  break;
+							Main.RsErrLine = 0;
+						}
+				 				
+					}
+        }
+}
+
+