PID控制(倒立摆程序)

【实例截图】

【核心代码】

倒立摆程序

/*倒立摆的调节上摆函数未成功：main函数的124行*/


#include "led.h"
#include "delay.h"
#include "sys.h"
#include "key.h"
#include "usart.h"
#include "wdg.h"
#include "timer.h"
#include "pwm.h"
#include "lcd.h"
#include "adc.h"
//#include "moto.h"
//#include "pid. h"


// #include "spi.h"
// #include "flash.h"
// #include "text.h"
// #include "fontupd.h"

extern volatile int AD[3];    //AD采集到的值
u8 start=1;
//u8 PID_flag;
u8 flag=0;
//u16 tmp;

struct PID { 
int SetPoint;  //设定目标 Desired Value 
int Proportion;//比例常数 Proportional Const 
int Integral;  //积分常数 Integral Const 
int Derivative;//微分常数 Derivative Const 
int LastError; //上次误差Error[-1] 
int PrevError; //当前误差Error[-2] 
int SumError;  //历史误差累计值Sums of Errors 
}; struct PID spid; //PID Control Structure 
 int rout;  //输出PID Response (Output) 
 int rin;   //反馈PID Feedback (Input) 

//PID核心算法(位置式)
int PIDCalc( struct PID *pp, unsigned int NextPoint ) 
{ 
	int dError,Error;//定义微分偏差和偏差
	Error = pp->SetPoint - NextPoint;//偏差=设定值-当前采样值
	pp->PrevError = pp->LastError;//保存，将最终误差赋值给前一个误差 
	pp->LastError = Error;//将当前偏差赋值给上次误差 
	pp->SumError  = Error; //积分，历史偏差累加 
	dError = pp->LastError - pp->PrevError;//当前微分，历史偏差相减
	
	//返回PID参数，输出控制参数
	return (pp->Proportion * Error//比例项
	  pp->Integral * pp->SumError//积分项
	  pp->Derivative * dError);// 微分项 
} 

//初始化PID各参数 
void PIDInit (struct PID *pp) 
{ 
	//（24,21）（24,20）（16,20动态模型）（12,24,380动态）（24,20,276）
	pp->SetPoint=2022;  //设定中心角度（±180°）为512（ADC）
	pp->Proportion=14;  //设定PID比例常数	  23
	pp->Integral=3;     //设定PID积分常数
	pp->Derivative=46; //设定PID微分常数
	pp->LastError=0;   //设定PID最终误差Error[-1]
	pp->PrevError=0;   //设定PID前一个误差Error[-2] 
	pp->SumError=0;    //设定历史误差累计值Sums of Errors 
}

void Speed_Adjust(uint16_t v,uint16_t t,uint8_t Direction)//调速(最大速度，时间，方向)
{
	uint16_t i=0;

	switch(Direction)
	{
	case 0:
											
						TIM_SetCompare2(TIM3,0);//启动加速
						TIM_SetCompare1(TIM3,8000);				 		
					break;
	case 1:
					
						TIM_SetCompare1(TIM3,0);
					  TIM_SetCompare2(TIM3,8000);								
					break;

	default: break;
	}
}

void Rock()
{
	if((AD[0]>=1822)&&(AD[0]<=2222))  //PID调节范围
	{
		rin=AD[0];//PID输入参数,ADC 的值
		rout=PIDCalc(&spid,rin);
	
		if(rout>0)
		{	
		 	TIM_SetCompare1(TIM3,0);
			TIM_SetCompare2(TIM3,rout 900);
		
		}
	   else if(rout<0) 
		{	 

			rout=-rout;
			TIM_SetCompare2(TIM3,0);
			TIM_SetCompare1(TIM3,rout 900);
				
		}

	}
	else
	{
	 TIM_SetCompare2(TIM3,0); TIM_SetCompare1(TIM3,0);  // start=1;delay_ms(1000);delay_ms(1000);delay_ms(1000);delay_ms(1000);delay_ms(1000);			  
	}
////////////////////////////////////////////////////////////////////////
	while((AD[0]>1728)&&(AD[0]<=1828))//上半圆不控制
	{
			TIM_SetCompare1(TIM3,0);
			TIM_SetCompare2(TIM3,900);	
	}
	while((AD[0]<2338)&&(AD[0]>2238))
	{
			TIM_SetCompare2(TIM3,0);
			TIM_SetCompare1(TIM3,900);
	}


////////////////////////////////////////////////////////////////////////
if(start==1)  //调节上摆程序
 {
	//	 Speed_Adjust(5000,50,0);//调速(速度，时间，方向)		   
	 // delay_ms(1000);delay_ms(1000);delay_ms(1000);
	   start=0;	
 }
	
} 
 int main(void)
 {
	SystemInit();
	delay_init(72);	     //延时初始化
	NVIC_Configuration();
    uart_init(9600);	 
 	LED_Init();
 	KEY_Init();
	LCD_Init();
	Adc_Init();
	POINT_COLOR=RED;
  PWM_Init(10000,0);	 //不分频。PWM频率20KHz
	PIDInit (&spid);//初始化PID算法
	Timerx_Init(100,7199);//10Khz的计数频率，计数到100 10ms	
	POINT_COLOR=RED;      					    	    
	LCD_ShowStr(120-7*8,50,0,7);//显示古诗《赋得古原草送别》
	POINT_COLOR=BLUE;
	LCD_ShowStr(120-3*8,50 16,55,58);//显示诗人：白居易
	POINT_COLOR=BLACK;
	LCD_ShowStr(32,50 32,8,55);//显示内容
	POINT_COLOR =RED;	
	LCD_ShowString(0,0,"ADC:");		 //显示一个字符串,16字体
	 while(1)
	{     
		 LCD_ShowNum(32,0,AD[0],4,16);//显示ADC的值
	   Rock();  
  // LCD_LED=0;//lcd背光关闭
    }
}