DisplacementSensor.c 1.75 KB
#include "DisplacementSensor.h"

//02 06 00 4C 00 00 48 2E
u8 setLiftZero[8] = {0x02,0x06,0x00,0x4C,0x00,0x00,0x48,0x2E};
u8 getLiftData[8] = {0x02,0x03,0x00,0x00,0x00,0x02,0xC4,0x38};
void getLiftHeight()
{
	static int i = 0;
	
	if(i++ >= 6)
	{
		i = 0;
		
		WriteUart(COM4,getLiftData,8);
	}
}

void SetLiftHeightZero()
{
	static int i = 0;
	if(i++ > 10)
	{	
		i = 0;
		WriteUart(COM4,setLiftZero,8);
	}
}

uint16_t AddCheckCRC2(unsigned char* cBuffer, short iBufLen)
{
	uint16_t i = 0, j = 0;
	uint16_t wCrc = 0xffff;
	uint16_t wPolynom = 0xA001;
	for (i = 0; i < iBufLen; i++)
	{
		wCrc ^= cBuffer[i];
		for (j = 0; j < 8; j++)
		{
			if (wCrc & 0x0001)
			{
				wCrc = (wCrc >> 1) ^ wPolynom;
			}
			else
			{
				wCrc = wCrc >> 1;
			}
		}
	}
	return wCrc;
}
void liftDataProcess1(unsigned char *Res)
{
	static unsigned char Buffer2[20];
	static unsigned short iii = 0;
	static unsigned char step = 0;
	unsigned short crcValue = 0;
	static int LiftHeight = 0;

	if (iii > 9)
	{
			iii = 0;
			step = 0;
	}
	if (step == 0 && *Res == 0x02) // $
	{
			iii = 0;
			step = 1;
	}
	else if(step == 1 && Buffer2[1] == 0x03)
	{
		 step = 2;
	}
	else if(step == 2 && Buffer2[2] == 0x04)
	{
		if(iii == 9)//½ÓÊÕÍê
		{
			step = 0;
			crcValue = Buffer2[7]|Buffer2[8]<<8;
			if(AddCheckCRC2(Buffer2,7) == crcValue)
			{
					DriverLifter1.Public.encoderPose = (Buffer2[3]<<24|Buffer2[4]<<16|Buffer2[5]<<8|Buffer2[6])/10 + agv.Parameter.LiftCompensation;//µ¥Î»MM
				LiftHeight = Buffer2[3]<<24|Buffer2[4]<<16|Buffer2[5]<<8|Buffer2[6];
			}
		}
	}
	Buffer2[iii++] = *Res;
	
}
void liftEncoderDataProcess()//2444  1240
{
	unsigned char buff[20];
	unsigned int len;
	unsigned int i;
	len = ReadUart(COM4, buff, 20);
	
	if(len > 0)
	{
		for (i = 0; i < len; i++)
		{
			liftDataProcess1(buff+i);
		}
	}

}