二轮平衡小车3：PID速度环

使用芯片：STM32 F103 C8T6

今日继续我的二路平衡小车开发之路，今日编写的是二轮平衡小车的PID速度环，我准备了纸飞机串口助手软件来辅助测试调节PID。

本文主要贴代码，之前的文章都有原理，代码中相应初始化驱动部分也有注释~~

文章提供源码，解释以及工程下载，测试效果视频。

PID基础概念：

 这里简单介绍一下PID算法是反馈调节的算法，只需输入期望值与传感器反馈值即可实现自动调节电机PWM控制速度始终在期望值附近，即：反馈小了就加占空比，反馈大了就减占空比，但却不是简单的加减运算。

原理之前写过，这里直接贴出文章连接：

PID输出反馈回路调控算法原理_NULL指向我的博客-CSDN博客

编码器测速逻辑：

此处贴出函数，相关逻辑在之前的文章讲过：

MSP432自主开发笔记1:编码器测速_外部中断捕获法测速\测正反转_msp432编码器_NULL指向我的博客-CSDN博客

对于速度单位的理解与计算有各种各样，有喜欢算到  （cm/s） （m / s）  (rad / second )等等，需要通过不同电机转速，需求来选定。

这里我是用的电机减速比比较大，扭矩与载重大，但因此转速就慢，因此我采用每25ms采样的脉冲数作为速度来比较，使速度环闭合。

//定时器3中断服务程序	  (编码器捕获脉冲数)
void TIM3_IRQHandler(void)
{ 
	if(TIM_GetITStatus(TIM3, TIM_IT_CC1)) //通道1发生捕获事件
		{	
			Wheel[2].CAPTURE++;
			TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);}	  //每次进入中断都要清空中断标志，否则主函数将无法正常执行
		
	if(TIM_GetITStatus(TIM3, TIM_IT_CC2)) //通道2发生捕获事件
		{Wheel[2].CAPTURE++;
		TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);}		//每次进入中断都要清空中断标志，否则主函数将无法正常执行	
		
	if(TIM_GetITStatus(TIM3, TIM_IT_CC3)) //通道3发生捕获事件
		{Wheel[1].CAPTURE++;
		TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);}		//每次进入中断都要清空中断标志，否则主函数将无法正常执行	
		
	if(TIM_GetITStatus(TIM3, TIM_IT_CC4)) //通道4发生捕获事件
		{Wheel[1].CAPTURE++;
		TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);}	  //每次进入中断都要清空中断标志，否则主函数将无法正常执行
}

void calculate_speed(void)
{
	uint16_t tt;
	tt=50;
	
	if(SPEED_flag==1)
	{
		SPEED_flag=0;
		Wheel[1].SPEED=Wheel[1].CAPTURE;
		Wheel[2].SPEED=Wheel[2].CAPTURE;
		
//		printf("V1=%d,V2=%d\r\n",Wheel[1].SPEED,Wheel[2].SPEED);
		printf("P1=%d,P2=%d\r\n",Wheel[1].PWM_DIV,Wheel[2].PWM_DIV);
	  PRINT(plotter, "%d, %d, %d",Wheel[1].SPEED,Wheel[2].SPEED,tt);				
		
		PID_guide_peed(tt,tt);
		set_wheels(Wheel[1].PWM_DIV,Wheel[2].PWM_DIV,1,1);
		Wheel[1].CAPTURE=0; Wheel[2].CAPTURE=0;
	}
}

PID算法贴出：

 参数需要自己调，玄学调参......

#include "PID.h"


PID_TYPE suduhuan1;
PID_TYPE suduhuan2;


//PID 1~4号轮设置期望速度
void PID_guide_peed(uint16_t w1,uint16_t w2)
{
				Pid_increment_Cal(&suduhuan1,w1,Wheel[1].SPEED);
				Pid_increment_Cal(&suduhuan2,w2,Wheel[2].SPEED);	
				Wheel[1].PWM_DIV=suduhuan1.OutPut;
				Wheel[2].PWM_DIV=suduhuan2.OutPut;
}

PID_结构体  target_目标  measure_当前值
void Pid_increment_Cal(PID_TYPE *PID, int target, int measure)
{
    PID->Error = target - measure;                                            // 误差
    PID->Pout = PID->P * (PID->Error - PID->PreError);                        // 比例控制
    PID->Iout = PID->I * PID->Error;                                          // 积分控制
    PID->Dout = PID->D * (PID->Error - 2 * PID->PreError + PID->PrePreError); // 微分控制
     // 比例 + 积分 + 微分总控制

    if (PID->Iout > PID->Irang) // 积分限幅
        PID->Iout = PID->Irang;
    if (PID->Iout < -PID->Irang) // 积分限幅
        PID->Iout = -PID->Irang;
  PID->OutPut += PID->Pout + PID->Iout + PID->Dout;
  PID->PrePreError = PID->PreError;  // 记忆e(k-2)
  PID->PreError = PID->Error;       // 记忆e(k-1)

}



void PidParameter_init(void)
{
    suduhuan1.P =38;
    suduhuan1.I=18;
    suduhuan1.D=0;
    suduhuan1.PreError=0;
    suduhuan1.PrePreError=0;
		suduhuan1.Irang=12;
    suduhuan1.OutPut=0;
	
    suduhuan2.P =38;
    suduhuan2.I=18;
    suduhuan2.D=0;
    suduhuan2.PreError=0;
    suduhuan2.PrePreError=0;
    suduhuan2.Irang=12;
		suduhuan2.OutPut=0;
}

#ifndef _PID_H_
#define _PID_H_

#include "headfire.h"

typedef struct PID
{
  int P;         //参数
  int I;
  int D;
  float Error;     //比例项e(k)
  float Integral;  //积分项
  int Differ;    //微分项
  int PreError;   //e(k-1)
  int PrePreError;//e(k-2)
  float Ilimit;
  float Irang;
  int Pout;
  int Iout;
  int Dout;
  int OutPut;
  uint8_t Ilimit_flag;    //积分分离
}PID_TYPE;

extern PID_TYPE suduhuan1;
extern PID_TYPE suduhuan2;

PID_结构体  target_目标  measure_当前值
void Pid_increment_Cal(PID_TYPE *PID, int target, int measure);
void PidParameter_init(void); //PID参数初始化 

//PID 设置期望速度
void PID_guide_peed(uint16_t w1,uint16_t w2);

#endif