1.通过编码器对返回的错误速度进行滤波

#define MOTOR_BUFF_CIRCLE_SIZE 4
#define STATIC_ENCODER_VALUE   6int32_t LMotor_Encoder_buff[MOTOR_BUFF_CIRCLE_SIZE] = {0};
uint8_t LEindex = 0;
int32_t LMotor_Encoder_last = 0;
int32_t L_Encoder_change = 0;int32_t RMotor_Encoder_buff[MOTOR_BUFF_CIRCLE_SIZE] = {0};
uint8_t REindex = 0;
int32_t RMotor_Encoder_last = 0;
int32_t R_Encoder_change = 0;uint8_t Robot_static_flag = 0;
uint8_t Robot_dynamic_flag = 0;int32_t Encoder_buff_change_value(int32_t *buff,uint8_t size)
{ int i = 0;int32_t value = 0;for(i = 0; i < size;i++){value += buff[i]; }return value;
}u8 Speed_Filter_send(int32_t L_speed,int32_t R_speed)
{	 CAN1Sedbuf[0]=L_speed;CAN1Sedbuf[1]=L_speed>>8;CAN1Sedbuf[2]=L_speed>>16;CAN1Sedbuf[3]=L_speed>>24;CAN1Sedbuf[4]=R_speed;CAN1Sedbuf[5]=R_speed>>8;CAN1Sedbuf[6]=R_speed>>16;CAN1Sedbuf[7]=R_speed>>24;	CAN1_Send(0x183,8);	 Delay_Us(200);
}void Updata_Motor_Speed(void)
{		
//	if((Motor_Vel_receive[0] < 10) && (Motor_Vel_receive[0] > -10))
//	{
//	  Motor_Vel_receive[0] = 0;
//	}//	if((Motor_Vel_receive[1] < 10) && (Motor_Vel_receive[1] > -10))
//	{
//	  Motor_Vel_receive[1] = 0;
//	}	LEindex = LEindex % MOTOR_BUFF_CIRCLE_SIZE;LMotor_Encoder_buff[LEindex] = Motor_Encoder_receive[0] - LMotor_Encoder_last;LEindex++;LMotor_Encoder_last = Motor_Encoder_receive[0];L_Encoder_change = Encoder_buff_change_value(LMotor_Encoder_buff,MOTOR_BUFF_CIRCLE_SIZE);REindex = REindex % MOTOR_BUFF_CIRCLE_SIZE;RMotor_Encoder_buff[REindex] = Motor_Encoder_receive[1] - RMotor_Encoder_last;REindex++;RMotor_Encoder_last = Motor_Encoder_receive[1];R_Encoder_change = Encoder_buff_change_value(RMotor_Encoder_buff,MOTOR_BUFF_CIRCLE_SIZE);	if( (abs(L_Encoder_change) < STATIC_ENCODER_VALUE) && (abs(R_Encoder_change) < STATIC_ENCODER_VALUE) ){//staticRobot_static_flag = 1;Robot_dynamic_flag = 0;}else{//dynamicRobot_static_flag = 0;Robot_dynamic_flag = 1;}if( (Robot_static_flag == 1) && (Robot_dynamic_flag == 0) ){Motor_Speed[0] = 0;Motor_Speed[1] = 0;Motor_Odom = 0;Motor_gyro_z = 0;		Speed_Filter_send(0,0);}else{Motor_Speed[0] = rpm2vel(Motor_Vel_receive[0]);Motor_Speed[1] = -rpm2vel(Motor_Vel_receive[1]);Motor_Odom = (Motor_Speed[0] + Motor_Speed[1])/2.0f;Motor_gyro_z = ((Motor_Speed[1] - Motor_Speed[0])/WHRRL_L);		Speed_Filter_send(Motor_Vel_receive[0],Motor_Vel_receive[1]);}Motor_L_Encoder = Encoder2Distance(Motor_Encoder_receive[0]);Motor_R_Encoder = -Encoder2Distance(Motor_Encoder_receive[1]);
}

2.中位值平均滤波

/***note
input:Speed_input_value;
outout:Speed_output_value;**/#define FILTER_BUFFER_SIZE 4
uint8 speed_filter[FILTER_BUFFER_SIZE] ={0};void TMSpeed_filter(void) 
{static unit8 ad_save_location=0;speed_filter[ad_save_location]  = Speed_input_value;    /*sample value get input value*/  if (ad_save_location > (FILTER_BUFFER_SIZE-1)) { ad_save_location = 0;TM_filter();                            //中位值平均滤波}else{ad_save_location++;}
}/*************************************************************************** * Function:    void compositor(u8 channel)* Description: 中位值平均滤波* Parameters:  None                * Returns:       * Author:      Teana**************************************************************************/
void compositor(void)
{unit8 exchange;unit8 i,j;u16 change_value;for (j=FILTER_BUFFER_SIZE;j>1;j--){for (i=0;i<j-1;i++){exchange = 0;if (speed_filter[i]>speed_filter[i+1]){change_value = speed_filter[i];speed_filter[i] = speed_filter[i+1];speed_filter[i+1] = change_value;exchange = 1;}}if (exchange == 0)return;}
}void TM_filter(void)
{unit8 index;unit8 count;u16 sum_data = 0;compositor(); //filter up-downfor (count=1;count<FILTER_BUFFER_SIZE-1;count++){sum_data +=speed_filter[count];}Speed_output_value= sum_data / (FILTER_BUFFER_SIZE - 2);sum_data = 0;}