1.通过编码器对返回的错误速度进行滤波
c
#define MOTOR_BUFF_CIRCLE_SIZE 4
#define STATIC_ENCODER_VALUE 6
int32_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) )
{
//static
Robot_static_flag = 1;
Robot_dynamic_flag = 0;
}
else
{
//dynamic
Robot_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.中位值平均滤波
c
/***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-down
for (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
c
// protection against multiple includes
#ifndef SAXBOPHONE_PID_H
#define SAXBOPHONE_PID_H
#ifdef __cplusplus
extern "C"{
#endif
typedef 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 gain
double ki; // Integral gain
double 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 measured
double target; // The desired reading
double 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 time
double 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
#endifpid.c
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 integral
state.integral += (error * state.time_delta);
// calculate derivative
double derivative = (error - state.previous_error) / state.time_delta;
// calculate output value according to algorithm
state.output = (
(calibration.kp * error) + (calibration.ki * state.integral) + (calibration.kd * derivative)
);
// update state.previous_error to the error value calculated on this iteration
state.previous_error = error;
// return the state struct reflecting the calculations
return state;
}test.c
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 values
calibration.kp = 1.0;
calibration.ki = 1.0;
calibration.kd = 1.0;
// an initial blank starting state
state.actual = 0.0;
state.target = 0.0;
state.time_delta = 1.0; // assume an arbitrary time interval of 1.0
state.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 serial
if (Serial.available() >= 2) {
// retrieve one byte as target value, cast to double and store in state struct
state.target = (double) Serial.read();
// same as above for actual value
state.actual = (double) Serial.read();
// now do PID calculation and assign output back to state
state = pid_iterate(calibration, state);
// print results back on serial
Serial.print("Target:\t");
Serial.println(state.target);
Serial.print("Actual:\t");
Serial.println(state.actual);
Serial.print("Output:\t");
Serial.println(state.output);
}
}