AGV / agv_gelinmei · Files

#include "CanUpdate.h"
#include "user_motor.h"
#include "user_setup.h"
#include "includes.h"
#include "QRcode.h"

u8 setPosModel[] = {0x2F,0x60,0x60,0x00,0x01,0x00,0x00,0x00};
u8 setTargetPos[] = {0x23,0x7A,0x60,0x00,0x2C,0x37,0x09,0x00};
u8 setPosSpeed[] = {0x23,0x81,0x60,0x00,0x00,0x46,0xC3,0x23};
u8 setSpeedSlope1[] = {0x23,0x83,0x60,0x00,0x20,0x4E,0x08,0x00};
u8 setSpeedSlope2[] = {0x23,0x84,0x60,0x00,0x20,0x4E,0x08,0x00};
u8 enable[] = {0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00};
u8 enable06[] = {0x2B,0x40,0x60,0x00,0x06,0x00,0x00,0x00};
u8 enable0F[] = {0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00};
u8 PosEnable[] = {0x2B,0x40,0x60,0x00,0x3F,0x00,0x00,0x00};

u8 speedModel[] = {0x2F,0x60,0x60,0x00,0x03,0x00,0x00,0x00};
u8 setSpeed[] = {0x23,0xFF,0x60,0x00,0x00,0x00,0x00,0x00};

u8 setZeroPoint1[] = {0x2F,0x60,0x60,0x00,0x06,0x00,0x00,0x00};
u8 setZeroPoint2[] = {0x2F,0x98,0x60,0x00,0x23,0x00,0x00,0x00};
u8 setZeroPoint3[] = {0x2B,0x40,0x60,0x00,0x1F,0x00,0x00,0x00};

u8 getEncodespeed[8] = {0x40, 0x6C, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00};
u8 readPos[] = {0x40,0x63,0x60,0x00,0x00,0x00,0x00,0x00};
void DriverFunction(DriverStruct *Motor) //行走 举升 移轴 推杆电机
{
    if (Motor->Private.initStep != 255) //*****************************************如果未初始化先初始化
    {
        init_driver(Motor); //非阻塞函数
    }
    else //**************************************************************************无其他问题则发速度
    {
        if (set_speed(Motor) == 1) //返回0证明发了指令 则本轮不要发指令 否则会造成上一条指令无法匹配
        {
        }
    }
}

void steeringFunction(DriverStruct *Motor, DriverStruct *refMotor) //转向电机 行走轮有速度则开使能 无速度则视情况关使能
{
    if (Motor->Private.initStep != 255) //如果未初始化先初始化
    {
        init_driver(Motor); //非阻塞函数
    }
    else
    {
        set_steering_speed(Motor, refMotor); //发速度
    }
}

float GetSpeedSlope(float SlopeSpeed, float CurSpeed,float TargetSpeed)
{

	if (TargetSpeed > CurSpeed)
	{
		CurSpeed += SlopeSpeed;
	}
	else if (TargetSpeed < CurSpeed)
	{
		CurSpeed -= SlopeSpeed;
	}

	if (_fabsf(CurSpeed - TargetSpeed) <= SlopeSpeed)
	{
		CurSpeed = TargetSpeed;
	}
	
	return CurSpeed;
}

void sendLeftWheelInit()
{	
	if(KincoStruct1.initStep == 0)
	{
		CAN1_Send_Msg(0x601,speedModel,8,2);
	}
	else if(KincoStruct1.initStep == 1)
	{
		CAN1_Send_Msg(0x601,enable06,8,2);
	}
	else if(KincoStruct1.initStep == 2)
	{
		CAN1_Send_Msg(0x601,enable0F,8,2);
	}
}

void LeftWheelDataProcess(CanRxMsg *CanRxMsg)
{
	if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x60 && CanRxMsg->Data[2] == 0x60)//设置速度模式
	{
		KincoStruct1.initStep = 1;
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x06)//使能
	{
		KincoStruct1.initStep = 2;
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x0F)//使能
	{
		KincoStruct1.initStep = 3;//初始化完成
	}
	
	if(CanRxMsg->Data[0] == 0x43 && CanRxMsg->Data[1] == 0x6C && CanRxMsg->Data[2] == 0x60)
	{
		KincoStruct1.encodeSpeed = (float)((CanRxMsg->Data[7] << 24) + (CanRxMsg->Data[6]<<16)+(CanRxMsg->Data[5] << 8) + CanRxMsg->Data[4])/-2730.7*0.714*1.05;
		if(fabs(KincoStruct1.encodeSpeed) < 10) 
			KincoStruct1.encodeSpeed = 0;
	}
}

void sendRightWheelInit()
{
	if(KincoStruct2.initStep == 0)
	{
		CAN1_Send_Msg(0x602,speedModel,8,2);
	}
	else if(KincoStruct2.initStep == 1)
	{
		CAN1_Send_Msg(0x602,enable06,8,2);
	}
	else if(KincoStruct2.initStep == 2)
	{
		CAN1_Send_Msg(0x602,enable0F,8,2);
	}
}

void RightWheelDataProcess(CanRxMsg *CanRxMsg)
{
	if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x60 && CanRxMsg->Data[2] == 0x60)//设置速度模式
	{
		KincoStruct2.initStep = 1;
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x06)//使能
	{
		KincoStruct2.initStep = 2;
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x0F)//使能
	{
		KincoStruct2.initStep = 3;//初始化完成
	}
	if(CanRxMsg->Data[0] == 0x43 && CanRxMsg->Data[1] == 0x6C && CanRxMsg->Data[2] == 0x60)
	{
		KincoStruct2.encodeSpeed = (float)((CanRxMsg->Data[7] << 24) + (CanRxMsg->Data[6]<<16)+(CanRxMsg->Data[5] << 8) + CanRxMsg->Data[4])/2730.7*0.714*1.05;
		if(fabs(KincoStruct2.encodeSpeed) < 10) 
			KincoStruct2.encodeSpeed = 0;
	}
}
extern void change_data(u8 *_data, int data);
void sendKincoSpeed()
{
	static int lastTime1 = 0,lastTime2 = 0;
	static u8 data1[8] = {0};
	static u8 data2[8] = {0};
	static float curSpeed1 = 0,curSpeed2 = 0;
	static float speedSlope1 = 0,speedSlope2 = 0;
	
	static int i = 0,EnableCount = 10,UnEnableCount = 100;
	
	if(i++ >10)
	{
		i = 0;
		CAN1_Send_Msg(0x601,getEncodespeed,8,0);
		
		CAN1_Send_Msg(0x602,getEncodespeed,8,0);
	}
	if(KincoStruct1.initStep != 3)
	{
		sendLeftWheelInit();
	}
	else if(KincoStruct2.initStep != 3)
	{
		sendRightWheelInit();
	}
	else
	{
		if(!agv.Public.i_QuickStopSig)
		{
			UnEnableCount = 100;
			if(EnableCount-- > 0)//先开使能
			{
				CAN1_Send_Msg(0x601,enable0F,8,0);
				
				CAN1_Send_Msg(0x602,enable0F,8,0);
			}
			else//发送速度
			{
				if(fabs(KincoStruct1.setSpeed) > fabs(curSpeed1))
					speedSlope1 = 2.5;
				else
					speedSlope1 = 4;
				
				if(fabs(KincoStruct2.setSpeed) > fabs(curSpeed2))
					speedSlope2 = 2.5;
				else
					speedSlope2 = 4;
				
				KincoStruct1.setSpeed = GetSpeedSlope(speedSlope1,curSpeed1,KincoStruct1.setSpeed);
				curSpeed1 = KincoStruct1.setSpeed;
				
				KincoStruct2.setSpeed = GetSpeedSlope(speedSlope2,curSpeed2,KincoStruct2.setSpeed);
				curSpeed2 = KincoStruct2.setSpeed;
				
				int speed1 = (int)(KincoStruct1.setSpeed/0.714*-2730.7);
				int speed2 = (int)(KincoStruct2.setSpeed/0.714*2730.7);

				//左电机
				if(agv.Public.SystemTime - lastTime1 > 5)
				{
					lastTime1 = agv.Public.SystemTime;
					change_data(setSpeed,speed1);
					CAN1_Send_Msg(0x601,setSpeed,8,0);
					
					sendRotateSpeed();
				
					//右电机
					change_data(setSpeed,speed2);
					
					CAN1_Send_Msg(0x602,setSpeed,8,0);
					
				}
			}
		}
		else
		{
			EnableCount = 10;//重新赋值使能计数
			if(UnEnableCount-- > 0)
			{
				change_data(setSpeed,0);
				CAN1_Send_Msg(0x601,setSpeed,8,0);
				CAN1_Send_Msg(0x602,setSpeed,8,0);
			}
			else
			{
				CAN1_Send_Msg(0x601,enable06,8,0);
				CAN1_Send_Msg(0x602,enable06,8,0);
			}
		}
	}
}




void sendLiftSpeed()
{
	change_data(setSpeed,(int)Lifter1.setSpeed);
	//第一次进入速度模式
	if(Lifter1.initState == 0)
	{
		CAN1_Send_Msg(0x603,speedModel,8,0);
		Uart_Printf(COM1,"举升设置速度模式\r\n");
	}
	else if(Lifter1.initState == 1)
	{
		CAN1_Send_Msg(0x603,enable06,8,0);
		Uart_Printf(COM1,"举升设置使能06\r\n");
	}
	else if(Lifter1.initState == 2)
	{
		CAN1_Send_Msg(0x603,enable0F,8,0);
		Uart_Printf(COM1,"举升设置使能0F\r\n");
	}
	else//初始化完成，发送目标速度
	{
		CAN1_Send_Msg(0x603,setSpeed,8,0);
	}
	Uart_Printf(COM1,"设置举升速度 = %.2f,举升状态 = %.2f  初始化 = %d\r\n",Lifter1.setSpeed,Lifter1.liftState,Lifter1.initState);
}

void LiftDataProcess(CanRxMsg *CanRxMsg)
{
	static int lastEncodeValue = 0,EncodeValue = 0;
	
	if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x60 && CanRxMsg->Data[2] == 0x60)//设置速度模式
	{
		Lifter1.initState = 1;
		Uart_Printf(COM1,"举升设置速度模式成功\r\n");
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x06)//使能
	{
		Lifter1.initState = 2;
		Uart_Printf(COM1,"举升设置使能06成功\r\n");
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x0F)//使能
	{
		Lifter1.initState = 3;
		Uart_Printf(COM1,"举升设置使能0F成功\r\n");
	}
	
	//读电机位置
	if(CanRxMsg->Data[0] == 0x43 && CanRxMsg->Data[1] == 0x63 && CanRxMsg->Data[2] == 0x60)//
	{
		EncodeValue = CanRxMsg->Data[4]|CanRxMsg->Data[5]<<8|CanRxMsg->Data[6]<<16|CanRxMsg->Data[7]<<24;
		
		Lifter1.encodePos +=  (float)(EncodeValue - lastEncodeValue)/80000;
		
		lastEncodeValue = EncodeValue;
	}
}


void sendRotateSpeed()
{
	change_data(setSpeed,(int)Rotate1.setSpeed);
	//第一次进入速度模式
	if(Rotate1.initState == 0)
	{
		CAN1_Send_Msg(0x604,speedModel,8,0);
		Uart_Printf(COM1,"旋转设置速度模式\r\n");
	}
	else if(Rotate1.initState == 1)
	{
		CAN1_Send_Msg(0x604,enable06,8,0);
		Uart_Printf(COM1,"旋转设置使能06\r\n");
	}
	else if(Rotate1.initState == 2)
	{
		CAN1_Send_Msg(0x604,enable0F,8,0);
		Uart_Printf(COM1,"旋转设置使能0F\r\n");
	}
	else//初始化完成，发送目标速度2418367
	{
		CAN1_Send_Msg(0x604,setSpeed,8,0);
	}
}

int EncodeValue = 0;
void RotateDataProcess(CanRxMsg *CanRxMsg)
{
	static u8 FirstFlag = 1;//第一次上电，角度置0
	static int lastEncodeValue = 0;
	if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x60 && CanRxMsg->Data[2] == 0x60)//设置速度模式
	{
		Rotate1.initState = 1;
		Uart_Printf(COM1,"旋转设置速度模式成功\r\n");
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x06)//使能
	{
		Rotate1.initState = 2;
		Uart_Printf(COM1,"旋转设置使能06成功\r\n");
	}
	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60&&CanRxMsg->Data[4] == 0x0F)//使能
	{
		Rotate1.initState = 3;
		Uart_Printf(COM1,"旋转设置使能0F成功\r\n");
	}
	
	//读电机位置
	if(CanRxMsg->Data[0] == 0x43 && CanRxMsg->Data[1] == 0x63 && CanRxMsg->Data[2] == 0x60)//
	{
		EncodeValue = CanRxMsg->Data[4]|CanRxMsg->Data[5]<<8|CanRxMsg->Data[6]<<16|CanRxMsg->Data[7]<<24;
		if(FirstFlag)//第一次上电，角度置0
		{
			FirstFlag = 0;
			lastEncodeValue = EncodeValue;
		}
		if(abs(EncodeValue - lastEncodeValue) > 100)
		{
			Rotate1.encodeAngle +=  (float)(lastEncodeValue - EncodeValue)*360/2560000;
			if(Rotate1.encodeAngle > 180)
			{
				Rotate1.encodeAngle -= 360;
			}
			else if(Rotate1.encodeAngle < -180)
			{
				Rotate1.encodeAngle += 360;
			}
			lastEncodeValue = EncodeValue;	
		}
	}
}

void DataProcess215(CanRxMsg *CanRxMsg)//手自动模式
{
	
}
void DataProcess315(CanRxMsg *CanRxMsg)//举升模式
{

}
void DataProcess181(CanRxMsg *CanRxMsg)//叉车速度
{
	DriverSteering1.Public.encoderSpeed = (CanRxMsg->Data[0]|(CanRxMsg->Data[1] << 8));
}
void DataProcess182(CanRxMsg *CanRxMsg)//电量
{
	agv.Public.Voltage = CanRxMsg->Data[4];
}
void DataProcess183(CanRxMsg *CanRxMsg)//舵轮角度
{
	DriverSteering1.Public.encoderPose = CanRxMsg->Data[0]|(CanRxMsg->Data[1] << 8) + 20;
	
	DriverSteering1.Public.encoderSetAngle = CanRxMsg->Data[3]|(CanRxMsg->Data[4] << 8);
}
void DataProcess381(CanRxMsg *CanRxMsg)//行走错误代码
{
	u8 i = 0,errorFlag = 0;
	
	for(i = 0;i < 8;i++)
	{
		errorFlag |=  CanRxMsg->Data[i];
	}
	if(errorFlag)
	{
		agv.Public.i_OpenWheelPowerFlag = 0;
	}

}
void DataProcess382(CanRxMsg *CanRxMsg)//举升错误代码
{
	u8 i = 0,errorFlag = 0;
	
	for(i = 0;i < 8;i++)
	{
		errorFlag |=  CanRxMsg->Data[i];
	}
	if(errorFlag)
	{
		agv.Public.i_OpenWheelPowerFlag = 0;
	}

}
void DataProcess383(CanRxMsg *CanRxMsg)//转向错误代码
{
	u8 i = 0,errorFlag = 0;
	
	for(i = 0;i < 8;i++)
	{
		errorFlag |=  CanRxMsg->Data[i];
	}
	if(errorFlag)
	{
		agv.Public.i_OpenWheelPowerFlag = 0;
	}

}
	unsigned short crc16_table[256] =
{
   0x0000, 0x3D65, 0x7ACA, 0x47AF, 0xF594, 0xC8F1, 0x8F5E, 0xB23B,
   0xD64D, 0xEB28, 0xAC87, 0x91E2, 0x23D9, 0x1EBC, 0x5913, 0x6476,
   0x91FF, 0xAC9A, 0xEB35, 0xD650, 0x646B, 0x590E, 0x1EA1, 0x23C4,
   0x47B2, 0x7AD7, 0x3D78, 0x001D, 0xB226, 0x8F43, 0xC8EC, 0xF589,
   0x1E9B, 0x23FE, 0x6451, 0x5934, 0xEB0F, 0xD66A, 0x91C5, 0xACA0,
   0xC8D6, 0xF5B3, 0xB21C, 0x8F79, 0x3D42, 0x0027, 0x4788, 0x7AED,
   0x8F64, 0xB201, 0xF5AE, 0xC8CB, 0x7AF0, 0x4795, 0x003A, 0x3D5F,
   0x5929, 0x644C, 0x23E3, 0x1E86, 0xACBD, 0x91D8, 0xD677, 0xEB12,
   0x3D36, 0x0053, 0x47FC, 0x7A99, 0xC8A2, 0xF5C7, 0xB268, 0x8F0D,
   0xEB7B, 0xD61E, 0x91B1, 0xACD4, 0x1EEF, 0x238A, 0x6425, 0x5940,
   0xACC9, 0x91AC, 0xD603, 0xEB66, 0x595D, 0x6438, 0x2397, 0x1EF2,
   0x7A84, 0x47E1, 0x004E, 0x3D2B, 0x8F10, 0xB275, 0xF5DA, 0xC8BF,
   0x23AD, 0x1EC8, 0x5967, 0x6402, 0xD639, 0xEB5C, 0xACF3, 0x9196,
   0xF5E0, 0xC885, 0x8F2A, 0xB24F, 0x0074, 0x3D11, 0x7ABE, 0x47DB,
   0xB252, 0x8F37, 0xC898, 0xF5FD, 0x47C6, 0x7AA3, 0x3D0C, 0x0069,
   0x641F, 0x597A, 0x1ED5, 0x23B0, 0x918B, 0xACEE, 0xEB41, 0xD624,
   0x7A6C, 0x4709, 0x00A6, 0x3DC3, 0x8FF8, 0xB29D, 0xF532, 0xC857,
   0xAC21, 0x9144, 0xD6EB, 0xEB8E, 0x59B5, 0x64D0, 0x237F, 0x1E1A,
   0xEB93, 0xD6F6, 0x9159, 0xAC3C, 0x1E07, 0x2362, 0x64CD, 0x59A8,
   0x3DDE, 0x00BB, 0x4714, 0x7A71, 0xC84A, 0xF52F, 0xB280, 0x8FE5,
   0x64F7, 0x5992, 0x1E3D, 0x2358, 0x9163, 0xAC06, 0xEBA9, 0xD6CC,
   0xB2BA, 0x8FDF, 0xC870, 0xF515, 0x472E, 0x7A4B, 0x3DE4, 0x0081,
   0xF508, 0xC86D, 0x8FC2, 0xB2A7, 0x009C, 0x3DF9, 0x7A56, 0x4733,
   0x2345, 0x1E20, 0x598F, 0x64EA, 0xD6D1, 0xEBB4, 0xAC1B, 0x917E,
   0x475A, 0x7A3F, 0x3D90, 0x00F5, 0xB2CE, 0x8FAB, 0xC804, 0xF561,
   0x9117, 0xAC72, 0xEBDD, 0xD6B8, 0x6483, 0x59E6, 0x1E49, 0x232C,
   0xD6A5, 0xEBC0, 0xAC6F, 0x910A, 0x2331, 0x1E54, 0x59FB, 0x649E,
   0x00E8, 0x3D8D, 0x7A22, 0x4747, 0xF57C, 0xC819, 0x8FB6, 0xB2D3,
   0x59C1, 0x64A4, 0x230B, 0x1E6E, 0xAC55, 0x9130, 0xD69F, 0xEBFA,
   0x8F8C, 0xB2E9, 0xF546, 0xC823, 0x7A18, 0x477D, 0x00D2, 0x3DB7,
   0xC83E, 0xF55B, 0xB2F4, 0x8F91, 0x3DAA, 0x00CF, 0x4760, 0x7A05,
   0x1E73, 0x2316, 0x64B9, 0x59DC, 0xEBE7, 0xD682, 0x912D, 0xAC48
};



unsigned short calcuCrc16_DNP(unsigned char* ibuff, int len )
{
	unsigned short value = 0xFFFF;
   unsigned short i;
   for( i = 0; i < len; i++ )
   {
      value = ((value << 8) & 0xFF00) ^ crc16_table[(value >> 8) ^ (0xFF &ibuff[i])];
   }
   return value;
}


void SendBuff(u16 ID,unsigned char* buff,unsigned char iBufLen)
{
	u8 crcBuff[8] = {0}; 
	u16 crcValue = 0;
	
	crcBuff[0] =	(ID >> 8)&0xFF;
	crcBuff[1] = 	ID&0xFF;
	crcBuff[2] =	 buff[0];
	crcBuff[3] =	 buff[1];
	crcBuff[4] =	 buff[2];
	crcBuff[5] =	 buff[3];
	crcBuff[6] =	 buff[4];
	crcBuff[7] =	 buff[5];
	
	crcValue = calcuCrc16_DNP(crcBuff,8);
	
	buff[6] = crcValue & 0xFF;
	buff[7] = (crcValue >> 8) & 0xFF;
	
	CAN1_Send_Msg(ID,buff,iBufLen,0);
}


u8 cmdData715[] = {0x05};
u8 cmdData195[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
u8 cmdData295[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
u8 cmdData395[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
u8 cmdData495[] = {0x00,0x09,0x00,0x00,0x00,0x00,0x00,0x00};

extern u8 AngleCompensation,AngleCompensationFront;

void _AppCanUpdate(void)
{	
	static short lastSetAngle = 0;
	static short setAngle = 0;
	static int lastTime1 = 0,lastTime2 = 0,lastTime3 = 0,lastTime4 = 0;
	static float curSpeed = 0,setSpeed = 0;
	
  setSpeed = (short)(-DriverSteering1.Command.speed*3.6);//单位m/h
	
	if(!agv.Public.Error_Flag)//不是急停,雷达，防撞条，速度斜坡
	{
		if(fabs(setSpeed) > fabs(curSpeed))
		setSpeed = GetSpeedSlope(DriverSteering1.Command.speedSlope,curSpeed,setSpeed);
	}

	curSpeed = setSpeed;
	
	if(agv.Command.Au_Hand && !agv.Public.i_QuickStopSig && agv.Parameter.DebuggingSpeedON_OFF) 
		{
			if(agv.Parameter.DebuggingSpeedON_OFF == 1)
			{
				setSpeed = 300*3.6;
				}
			else if(agv.Parameter.DebuggingSpeedON_OFF == 2)
			{
				setSpeed = -300*3.6;
			}
		}
		
	cmdData195[0] = agv.Command.Au_Hand;
	
	cmdData195[2] = (short)DriverLifter1.Command.speed;//起升或下降
	
	cmdData295[0] = (short)setSpeed & 0xFF;
	
	cmdData295[1] = ((short)setSpeed >> 8) & 0xFF;
	
//	cmdData295[4] |= agv.Public.i_QuickStopSig;//急停触发软停止
//	cmdData295[4] |= (agv.Public.i_QuickStopSig << 1);//急停触发硬停止
	cmdData295[4] = 0;
	
	
	
	if(agv.Public.SystemTime - lastTime1 > 41)
	{	
		CAN1_Send_Msg(0x715,cmdData715,1,0);
		
		lastTime1 = agv.Public.SystemTime;
	}
	
	if(agv.Public.SystemTime - lastTime2 > 42)
	{		
		cmdData495[5] += 0x10;
		
		SendBuff(0x495,cmdData495,8);
		
		lastTime2 = agv.Public.SystemTime;
	}
	
	if(agv.Public.SystemTime - lastTime3 > 403)
	{	
		CAN1_Send_Msg(0x395,cmdData395,8,0);
		
		lastTime3 = agv.Public.SystemTime;
	}
	
	if(agv.Public.SystemTime - lastTime4 > 10)
	{		
		lastTime4 = agv.Public.SystemTime;
		
		cmdData195[5] += 0x10;	
	
		SendBuff(0x195,cmdData195,8);
		
		setAngle = (short)DriverSteering1.Command.angle;
		
		if(agv.Command.CurDirection == 1 || agv.Command.CurDirection == 2|| agv.Command.CurDirection == 0)
			setAngle = (short)DriverSteering1.Command.angle - AngleCompensationFront;//单位0.1度
		
		
		if(setAngle - lastSetAngle > 70)
		{
			setAngle = lastSetAngle + 70;
		}
		else if(setAngle - lastSetAngle < -70)
		{
			setAngle = lastSetAngle - 70;
		}
		 
		lastSetAngle = setAngle;
		
		cmdData295[2] = setAngle & 0xFF;
	
		cmdData295[3] = (setAngle >> 8) & 0xFF;
		
		cmdData295[5] += 0x10;
		
		SendBuff(0x295,cmdData295,8);
	}

	
	if(agv.Public.Error_Flag == 0x200 || agv.Public.Error_Flag == 0x40)
	{
		if(DriverLifter1.Command.speed == 0)
			agv.Command.MusicFlag = 1;
	}
	Music_Select(agv.Command.MusicFlag, agv.Command.Vol);
}