#include?“PID.h“


extern?int?Encodercntscnts0;
extern?float?tdtitd1ti1;

PIDTypdDef?Pid;

float?ZHONGZHI=200;
float?D_BiasLast_BiasLast_Bias1;
float?Balance_Kp=0Balance_Ki=100Balance_Kd=0Speed_Kp=0Speed_Ki=0Speed_Kd=0;
int?balance_pwmspeed_pwmPWMPWM1;
u16?dirflag;?

//定時器14中斷
//定時器14中斷服務函數用于PID的處理2ms一次
void?TIM5_IRQHandler（void）
{
		if（TIM_GetITStatus（TIM5TIM_IT_Update）==SET）
		{		
		
			Encoder?=?TIM2?->?CNT;???????????????//===讀取編碼器的值
			Encoder?=?Encoder/10;????????????????//==10以內的誤差除去
			cnts0?=?cnts/100;
??????if（Encoder>165&&Encoder<235）
			{
				
				GPIO_ResetBits（GPIOBGPIO_Pin_14）;???????????//啟動電機
????????flag?=?1;????????????????????????????????????//電機啟動標志位置1
				
				balance_pwm?=?Balance_angle（Encoder）;????????//角度環控制
				speed_pwm?=?Speed_control（cnts0）;?????????????//速度環控制
				
				Pid.CurrPWM?=?balance_pwm?-?speed_pwm;???????//===更新PWM
				Pid.CurrPWM?=?1000000/Pid.CurrPWM;???????????//時鐘分頻，得到ARR值
				
//				printf（“Pid.CurrPWM:?%d\n“Pid.CurrPWM）;	
				
				if（Pid.CurrPWM>=0）?
				{
					dir?=?1;?????????????????????????????????//轉向標志位置1
					GPIO_ResetBits（GPIOB?GPIO_Pin_15）;??????//電機線CW拉低正轉
				}
				else?if（Pid.CurrPWM<0）?
				{
					dir?=?0;?????????????????????????????????//轉向標志位置0
					GPIO_SetBits（GPIOB?GPIO_Pin_15）;????????//電機反轉???
					Pid.CurrPWM?=?-Pid.CurrPWM;???????????????//PID結果取絕對值
				}				
			}				
		??else?
			{
				GPIO_SetBits（GPIOBGPIO_Pin_14）;???????//關閉電機
				flag?=?0;??????????????????????????????//電機關閉標志位置0
				cnts?=?0;
				Pid.CurrPWM?=?0;
				balance_pwm?=?0;
				speed_pwm?=?0;
				PWM?=?0;
				Last_Bias?=?0;
				Last_Bias1?=?0;
			}
			
			TIM_SetAutoreload（TIM3Pid.CurrPWM）;?????//ARR賦值
			TIM_SetCompare1（TIM3Pid.CurrPWM/2）;?????//占空比設置50%
			TIM_SetCounter（TIM30）;??????????????????//因為APRE為設置為立即生效，為避免CNT計數器寄存器內存寫穿，每更新一次ARR值，就向CNT清零
		}
		
//		printf（“Pid.CurrPWMEncoder:?%d%d\n“Pid.CurrPWMEncoder）;
		
	??TIM_ClearITPendingBit（TIM5TIM_IT_Update）;???//清除中斷標志位
}

void?TIM3_IRQHandler（void）
{
	if（TIM_GetITStatus（TIM3TIM_IT_Update）==SET）?//溢出中斷
	{
		if（flag==1）
		{
			if（dir==1）
			{
				cnts++;
				if（cnts>25600）
				{
					cnts?=?0;
				}
			}
			else
			{
				cnts--;
				if（cnts<-25600）
				{?
					cnts?=?0;
				}
			}
		}

	}
	TIM_ClearITPendingBit（TIM3TIM_IT_Update）;??//清除中斷標志位
}

/*------------------------------------------
?函數功能:增量式PID計算
?函數說明:控制角度環
?步進電機需要用增量式PID
?用位置式的話，怎么調都是抖的?
------------------------------------------*/
int?Balance_angle（float?a_encode）?
{
	float?Bias;

	Bias?=?a_encode?-?ZHONGZHI;??????//===求出平衡的角度中值（和機械相關）得到與目標值的誤差?
	
	PWM?+=?Balance_Kp*（Bias-Last_Bias）?+?Balance_Ki*Bias?;????//===計算平衡控制的電機PWM
	
??Last_Bias?=?Bias;
	
	return?PWM;
}

/*------------------------------------------
??函數功能：位置式PID計算
??函數說明：控制速度環
------------------------------------------*/
int?Speed_control（float?cnt）
{
	float?Bias;
		
	Bias?=?cnts?-

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????5031??2018-03-23?20:55??倒立擺調試\BALENCE\PID.c

?????文件???????1010??2018-03-22?21:59??倒立擺調試\BALENCE\PID.h

?????文件?????109142??2016-04-15?21:57??倒立擺調試\CORE\core_cm4.h

?????文件??????22735??2016-04-15?21:57??倒立擺調試\CORE\core_cm4_simd.h

?????文件??????17146??2016-04-15?21:57??倒立擺調試\CORE\core_cmFunc.h

?????文件??????20513??2016-04-15?21:57??倒立擺調試\CORE\core_cmInstr.h

?????文件??????29605??2016-04-15?21:57??倒立擺調試\CORE\startup_stm32f40_41xxx.s

?????文件???????6924??2016-04-15?21:57??倒立擺調試\FWLIB\inc\misc.h

?????文件??????32880??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_adc.h

?????文件??????27318??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_can.h

?????文件???????2416??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_crc.h

?????文件??????14481??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_cryp.h

?????文件??????14946??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_dac.h

?????文件???????4296??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_dbgmcu.h

?????文件??????12977??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_dcmi.h

?????文件??????28882??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_dma.h

?????文件??????19692??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_dma2d.h

?????文件???????8012??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_exti.h

?????文件??????24467??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_flash.h

?????文件???????3275??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_flash_ramfunc.h

?????文件??????44924??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_fmc.h

?????文件??????27181??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_fsmc.h

?????文件??????23548??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_gpio.h

?????文件??????10084??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_hash.h

?????文件??????31939??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_i2c.h

?????文件???????4323??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_iwdg.h

?????文件??????21191??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_ltdc.h

?????文件???????7728??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_pwr.h

?????文件??????30063??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_rcc.h

?????文件???????3958??2016-04-15?21:57??倒立擺調試\FWLIB\inc\stm32f4xx_rng.h

............此處省略185個文件信息