基于STM32F103C8T6单片机的PID平衡小车程序 -

#include "sys.h"
//期望角度（机械中值——>不一定为0）
float Mid_angle = 0;
//直立环（Kp、Kd）与速度环（Kp、Ki）的PID参数
float
// Vertical_Kp = 400, //直立环Kp
// Vertical_Kd = 1; //直立环Kd
Vertical_Kp = 240, //直立环Kp
Vertical_Kd = 0.6; //直立环Kd
float
Velocity_Kp = 0.38, //速度环Kp
Velocity_Ki = 0.0019; //速度环Ki
float
Turn_Kp = 1.1; //转向环Kp
//PID控制输出
int Velocity_out, Vertical_out, Turn_out;
//声明函数
int Velocity_PI(int Encoder_left, int Encoder_right);
int Vertical_PD(float Mid_angle, float Real_angle, float gyro_y);
int Turn_P(int gyro_z);
void EXTI9_5_IRQHandler()
{
if(EXTI_GetITStatus(EXTI_Line5) != 0 )
{
int PWM_out;
if(PBin(5) == 0) //判断是否为低电平（外部中断下降沿触发）
{
EXTI_ClearITPendingBit(EXTI_Line5);
//1.1、采集编码器数据
Encoder_left = -Read_Speed(2); //读取编码器2数据
Encoder_right = Read_Speed(4); //读取编码器4数据
//1.2、采集MPU6050数据
mpu_dmp_get_data(&Pitch, &Roll, &Yaw);
MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
MPU_Get_Accelerometer(&aacx, &aacy, &aacz);
//2、带入参数进行PID控制
Vertical_out = Vertical_PD(Mid_angle, Pitch, gyroy); //前后移动由Pitch控制
Velocity_out = Velocity_PI(Encoder_left, Encoder_right);
Turn_out = Turn_P(gyroz);
PWM_out = Vertical_out - Vertical_Kp * Velocity_out; //串级PID推导公式（速度环与直立环）
//3、将PID输出值赋给电机
Motor_1 = PWM_out - Turn_out;
Motor_2 = PWM_out + Turn_out;
Limit(&Motor_1, &Motor_2);
Load(Motor_1, Motor_2);
}
}
}
//速度环 PI控制
/*
输入：
Encoder_left 左编码器速度
Encoder_right 右编码器速度
输出：
PWM值
*/
int Velocity_PI(int encoder_left, int encoder_right)
{
static int
PWM_out,
Encoder_err, //Encoder_err 为编码器速度偏差
Encoder_S; //Encoder_S 为编码器速度的积分值
static int
Encoder_err_low_out, //Encoder_err_low_out 为低通滤波器输出
Encoder_err_low_out_last; //Encoder_err_low_out_last 为上一次低通滤波器输出
float
a = 0.7; //a 为低通滤波器的系数
//1、计算速度偏差
Encoder_err = (encoder_left + encoder_right) - 0; //除以2会造成舍去误差
// Encoder_err = (Encoder_left + Encoder_right) / 2 - 0; //0表示为期望速度
//2、对速度偏差进行低通滤波
//（滤除高频干扰，使得波形更加平滑，防止速度突变影响直立环的正常工作）
Encoder_err_low_out = (1 - a) * Encoder_err + a * Encoder_err_low_out_last;
Encoder_err_low_out_last = Encoder_err_low_out;
//3、对速度偏差进行积分，得到位移
Encoder_S += Encoder_err_low_out;
//4、积分限幅
Encoder_S = Encoder_S > 10000 ? 10000 : (Encoder_S < (-10000) ? (-10000) : Encoder_S);
//5、速度环输出
PWM_out = Velocity_Kp * Encoder_err_low_out + Velocity_Ki * Encoder_S;
return PWM_out;
}
//直立环 PD控制
/*
输入：
Mid_angle 期望角度（中值角度）
Real_angle 真实角度
gyro_y 真是角速度
输出：
PWM值
*/
int Vertical_PD(float Mid_angle, float Real_angle, float gyro_y)
{
int PWM_out;
PWM_out = Vertical_Kp * (Real_angle - Mid_angle) + Vertical_Kd * (gyro_y - 0);
return PWM_out;
}
//转向环 P控制
int Turn_P(int gyro_z)
{
int PWM_out;
PWM_out = Turn_Kp * gyro_z;
return PWM_out;
}

复制代码