增量式 PID 控制demo

 记录一下,以后用得着

#include "sys.h"
#include "BLDC.h" 
#include "stm32f10x.h"


#define START_PWM_PUSSE		100
#define PWM_MAX_ARR	999
#define PWM_MIN_ARR	49


void MIX_PIN_Init(void)
{
 
    GPIO_InitTypeDef  GPIO_InitStructure;
 	
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);	 //使能PB,PE端口时钟
	
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_8;				 //LED0-->PB.5 端口配置
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		 //推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		 //IO口速度为50MHz
    GPIO_Init(GPIOC, &GPIO_InitStructure);					 //根据设定参数初始化GPIOB.5
	
    GPIO_ResetBits(GPIOC,GPIO_Pin_6|GPIO_Pin_8);						 //PB.5 输出高
}
 

/*20~30Khz    999 2 72000000/(1000*3)=24000*/
void TIM8_PWM_Init(void)
{ 
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);	//使能定时器3时钟
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC  | RCC_APB2Periph_AFIO, ENABLE);  //使能GPIO外设和AFIO复用功能模块时钟   
 
   //设置该引脚为复用输出功能,输出TIM3 CH2的PWM脉冲波形	GPIOB.5
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; //TIM_CH2
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOC, &GPIO_InitStructure);//初始化GPIO
 
   //初始化TIM3
	TIM_TimeBaseStructure.TIM_Period = 999; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
	TIM_TimeBaseStructure.TIM_Prescaler =2; //设置用来作为TIMx时钟频率除数的预分频值 
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
	
	//初始化TIM3 Channel2 PWM模式	 
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
	TIM_OCInitStructure.TIM_Pulse = 100; 
	TIM_OC2Init(TIM8, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC2

	TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);  //使能TIM3在CCR2上的预装载寄存器
 
	TIM_Cmd(TIM8, ENABLE);  //使能TIM3
}


/*
100ms
*/
void TIM7_BASE_Init(void)
{  

	NVIC_InitTypeDef        NVIC_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);

	
	TIM_TimeBaseStructure.TIM_Period = 999; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值	 80K
	TIM_TimeBaseStructure.TIM_Prescaler =7199; //设置用来作为TIMx时钟频率除数的预分频值  不分频
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
	
	NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;  //TIM3中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;  //先占优先级0级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;  //从优先级3级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
	NVIC_Init(&NVIC_InitStructure);  //初始化NVIC寄存器
	
  TIM_ARRPreloadConfig(TIM7, ENABLE); //使能TIMx在ARR上的预装载寄存器
	
	TIM_ITConfig(TIM7,TIM_IT_Update,ENABLE );
	TIM_Cmd(TIM7, DISABLE);  //使能TIMx外设
}


void TIM1_ERT_Init(void)
{  
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;



	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOE  | RCC_APB2Periph_AFIO, ENABLE);  //使能GPIO外设和AFIO复用功能模块时钟使能
	GPIO_PinRemapConfig(GPIO_FullRemap_TIM1, ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOE, &GPIO_InitStructure);
	
	TIM_TimeBaseStructure.TIM_Period = 9999;
	TIM_TimeBaseStructure.TIM_Prescaler =0;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

	TIM_ETRClockMode2Config(TIM1,TIM_ExtTRGPSC_OFF,TIM_ExtTRGPolarity_NonInverted,0);
	TIM_SetCounter(TIM1, 0); 
	TIM_Cmd(TIM1, DISABLE);
}

//增量式PID
typedef struct PID {
	float SetPoint;//目标 <速度   pps 每秒检测到的脉冲>
	
	float Proportion;//比例系数 
	float Integral;//积分系数
	float Derivative;//微分系数
	float LastError;上上次
	float PrevError;//上次
	float SumError;//累计
	
	uint16_t CCR;//当前控制量  PWM占空比
	int   Set_v;//设置的目标速度<单位 pps 每秒检测到的脉冲> 
	uint8_t stop_sta;
} PID;
PID   vPID={3.0,0.4,0.001,0,0,0,0,START_PWM_PUSSE,100,0};

float PIDCalc( PID *pp, float NextPoint )
{
	float dError,
	Error;
	Error = pp->SetPoint - NextPoint;//本次误差  目标-当前   比例
	pp->SumError += Error;//累计误差  积分
	dError = pp->LastError - pp->PrevError;//两次误差差值    微分
	pp->PrevError = pp->LastError; //更新
	pp->LastError = Error; //更新

    if(pp->SumError>900)  pp->SumError=900;//积分控制
    else if(pp->SumError<-900)	  pp->SumError=-900;//积分控制

	//输出增量
	return (pp->Proportion * Error
	+ pp->Integral * pp->SumError 
	+ pp->Derivative * dError
	);
}

float Vol(int vSet,int v3){

	float vOut; 
	float vIn;

	vPID.SetPoint = vSet;
	vIn = (float)v3;
	vOut = PIDCalc ( &vPID,vIn );	 
	return vOut;
}

//100ms
void TIM7_IRQHandler()
{
	int v;//每秒脉冲
	float Pv=0.0; 
	
	if(TIM_GetITStatus(TIM7,TIM_FLAG_Update))	
	{
		TIM_ClearITPendingBit(TIM7, TIM_FLAG_Update);

		v = TIM1->CNT *10;
		TIM1->CNT = 0;
		if(vPID.Set_v-v>=vPID.Set_v/6 || vPID.Set_v-v<=-vPID.Set_v/6)// 当前v 大于 ±设置v
		{	
			Pv=Vol(vPID.Set_v,v);
			vPID.CCR=vPID.CCR+Pv;//增量式
			if(vPID.CCR>=PWM_MAX_ARR)
			{
				vPID.CCR=PWM_MAX_ARR;
			}
			else if(vPID.CCR<=PWM_MIN_ARR)
			{
				vPID.CCR=PWM_MIN_ARR;
			}			
			TIM8->CCR2 = vPID.CCR;			
		}
		
		if(v==0)vPID.stop_sta=0;
		else vPID.stop_sta=1;
	}
}





void Mix_MT_Config()
{
	MIX_PIN_Init();
	
	TIM8_PWM_Init();
	TIM7_BASE_Init();
	TIM1_ERT_Init();
}


//v r/s   
// 300  0.05~5
void Mix_MT_Start(float v)
{
	
	if(v<0.05)v=0.05;
	else if(v>5)v=5;
	
	vPID.Set_v = v*10*6;
	GPIO_SetBits(GPIOC,GPIO_Pin_8);
	TIM_CtrlPWMOutputs(TIM8,ENABLE);
	TIM_SetCounter(TIM1, 0); 
	TIM_Cmd(TIM1, ENABLE);
	TIM_Cmd(TIM7, ENABLE);
	TIM_Cmd(TIM8, ENABLE);
}

//v   r/s 转/秒
//(0.05,5)
void Mix_MT_SetSpeed(float v)
{
	if(v<0.05)v=0.05;
	else if(v>5)v=5;
	vPID.Set_v = v;
}

void Mix_MT_Stop()
{
	GPIO_ResetBits(GPIOC,GPIO_Pin_8);
	TIM_CtrlPWMOutputs(TIM8,DISABLE);
	TIM_Cmd(TIM1, DISABLE);
	TIM_Cmd(TIM7, DISABLE);
	TIM_Cmd(TIM8, DISABLE);
}