2021-09-09第一篇 PID

STM32+PID

一、姿态传感器

1.使用HW905测量X、Y、Z三轴的角度值。俯仰角：PITCH，横滚角： ROLL，偏航角：YAW

二、PID

1.PID公式（位置式）：value=KpEk+Kiintegral+Kd*(Ek-Ekk);

2.代码
pid.h:

#ifndef __PID_H
#define __PID_H

extern float PITCH_X;  //俯仰角
extern float ROLL_Y;   //横滚角
extern float HEADING_Z;//航向角
extern float DEPTH;//深度
extern float HEADING_targe,DEPTH_targe;		

extern int steer_change_x,steer_change_y,steer_change_z,steer_change_d;//定义3轴的转向变量
extern int pitch_pwm_Z,pitch_pwm_F,roll_pwm_Z,roll_pwm_F,head_pwm_Z,head_pwm_F,depth_pwm_Z,depth_pwm_F,forward_pwm;//直行变量为-500~500

//定义结构体
struct pid{
    float SetOut;           //定义设定输出值
	  float CurrentOut;				//定义当前输出值
    float Ek;               //定义偏差值
    float Ekk;           //定义上一个偏差值
    float Kp,Ki,Kd;           //定义比例、积分、微分系数
    float value;         //定义PID值（控制执行器的变量）
    float integral;           //定义积分值
		float integral_limit; //积分上限
};

extern void PID_init(struct pid *PID_angle);
float PID_count(struct pid *PID_angle,float output,float angle);
void PID_control(void);
//float uniformization(float a);


#endif

pid.c：

/********************************************************************************	 
 * PID代码	
********************************************************************************/
float PITCH_X;  //俯仰角
float ROLL_Y;   //横滚角
float HEADING_Z;//航向角
float DEPTH;//深度
//PID初始化
void PID_init(struct pid *PID_angle)
{
    printf("PID_init begin \n");
    PID_angle->SetOut=0.0;
	  PID_angle->CurrentOut=0.0;
    PID_angle->Ek=0.0;
    PID_angle->Ekk=0.0;
    PID_angle->integral=0.0;
	  PID_angle->integral_limit=100.0;
    PID_angle->Kp=0.2;
    PID_angle->Ki=0.001;
    PID_angle->Kd=0.2;
    printf("PID_init end \n");
}
float PID_count(struct pid *PID_angle,float output,float angle)
{
    PID_angle->SetOut=output;//设定输出值
    PID_angle->Ek=PID_angle->SetOut-PID_angle->CurrentOut;//偏差值
    PID_angle->integral+=PID_angle->Ek;//积分
		//积分上限
	if(PID_angle->integral>PID_angle->integral_limit) PID_angle->integral=100;
	if(PID_angle->integral<-PID_angle->integral_limit)PID_angle->integral=-100;	
    PID_angle->value=PID_angle->Kp*PID_angle->Ek+PID_angle->Ki*PID_angle->integral+PID_angle->Kd*(PID_angle->Ek-PID_angle->Ekk);//位置式公式
    PID_angle->Ekk=PID_angle->Ek;		
//	  PID_PITCH.CurrentOut+=PID_PITCH.value;
		PID_angle->CurrentOut=angle;
    return PID_angle->value;
}



#endif