STM32算法

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;}

3.PID算法

pid.h

// protection against multiple includes
#ifndef SAXBOPHONE_PID_H
#define SAXBOPHONE_PID_H#ifdef __cplusplus
extern "C"{
#endiftypedef struct pid_calibration {/** struct PID_Calibration* * Struct storing calibrated PID constants for a PID Controller* These are used for tuning the algorithm and the values they take are* dependent upon the application - (in other words, YMMV...)*/double kp; // Proportional gaindouble ki; // Integral gaindouble kd; // Derivative gain} PID_Calibration;typedef struct pid_state {/** struct PID_State* * Struct storing the current state of a PID Controller.* This is used as the input value to the PID algorithm function, which also* returns a PID_State struct reflecting the adjustments suggested by the algorithm.* * NOTE: The output field in this struct is set by the PID algorithm function, and* is ignored in the actual calculations.*/double actual; // The actual reading as measureddouble target; // The desired readingdouble time_delta; // Time since last sample/calculation - should be set when updating state// The previously calculated error between actual and target (zero initially)double previous_error;double integral; // Sum of integral error over timedouble output; // the modified output value calculated by the algorithm, to compensate for error} PID_State;/** PID Controller Algorithm implementation* * Given a PID calibration for the P, I and D values and a PID_State for the current* state of the PID controller, calculate the new state for the PID Controller and set* the output state to compensate for any error as defined by the algorithm*/PID_State pid_iterate(PID_Calibration calibration, PID_State state);#ifdef __cplusplus
} // extern "C"
#endif// end of header
#endif

pid.c

#include "pid.h"PID_State pid_iterate(PID_Calibration calibration, PID_State state) {// calculate difference between desired and actual values (the error)double error = state.target - state.actual;// calculate and update integralstate.integral += (error * state.time_delta);// calculate derivativedouble derivative = (error - state.previous_error) / state.time_delta;// calculate output value according to algorithmstate.output = ((calibration.kp * error) + (calibration.ki * state.integral) + (calibration.kd * derivative));// update state.previous_error to the error value calculated on this iterationstate.previous_error = error;// return the state struct reflecting the calculationsreturn state;
}

test.c

#include "pid.h"// initialise blank PID_Calibration struct and blank PID_State struct
PID_Calibration calibration;
PID_State state;void setup() {// configure the calibration and state structs// dummy gain valuescalibration.kp = 1.0;calibration.ki = 1.0;calibration.kd = 1.0;// an initial blank starting statestate.actual = 0.0;state.target = 0.0;state.time_delta = 1.0; // assume an arbitrary time interval of 1.0state.previous_error = 0.0;state.integral = 0.0;// start the serial line at 9600 baud!Serial.begin(9600);
}void loop() {// read in two bytes from serial, assume first is the target value and// second is the actual value. Output calculated result on serialif (Serial.available() >= 2) {// retrieve one byte as target value, cast to double and store in state structstate.target = (double) Serial.read();// same as above for actual valuestate.actual = (double) Serial.read();// now do PID calculation and assign output back to statestate = pid_iterate(calibration, state);// print results back on serialSerial.print("Target:\t");Serial.println(state.target);Serial.print("Actual:\t");Serial.println(state.actual);Serial.print("Output:\t");Serial.println(state.output);}
}