#include?“control.h“		
??/**************************************************************************
作者：Sam
我的淘寶小店：客戶滿意使我們的服務宗旨
**************************************************************************/
int?Balance_PwmPosition_Pwm;
u8?Flag_TargetPosition_Target;
/**************************************************************************
函數功能：所有的控制代碼都在這里面
??????????TIM1控制的5ms定時中斷?
**************************************************************************/
int?TIM1_UP_IRQHandler（void）??
{????
	if（TIM1->SR&0X0001）//5ms定時中斷
	{???
		??TIM1->SR&=~（1<<0）;???????????????????????????????????????//===清除定時器1中斷標志位	?????????????????????
	?????if（delay_flag==1）
			?{
				?if（++delay_50==10）	?delay_50=0delay_flag=0;??????????//===給主函數提供50ms的精準延時
			?}		
????	Encoder=Read_Encoder（4）;?????????????	???????????????????//===更新編碼器位置信息
??????Angle_Balance=Get_Adc_Average（315）;?????????????????????//===更新姿態	
?????	Balance_Pwm?=balance（Angle_Balance）;??????????????????????????????????????????//===角度PD控制	
	????if（++Position_Target>4）	Position_Pwm=Position（Encoder）Position_Target=0;?????//===位置PD控制?25ms進行一次位置控制
??????Moto=Balance_Pwm-Position_Pwm;????????//===計算電機最終PWM
		??Xianfu_Pwm（）;?????????????????????????//===PWM限幅?反正占空比100%帶來的系統不穩定因素
		??if（Turn_Off（Voltage）==0）??????????????//===低壓和傾角過大保護
			Set_Pwm（Moto）;????????????????????????//===賦值給PWM寄存器
	??	Led_Flash（100）;???????????????????????//===LED閃爍指示系統正常運行?
	????Voltage=Get_battery_volt（）;???????????//===獲取電池電壓	??????
			Key（）;????????????????????????????????//===掃描按鍵變化
	}???????	
	?return?0;	??
}?

/**************************************************************************
函數功能：傾角PD控制
入口參數：角度
返回??值：傾角控制PWM
作????者：平衡小車之家
**************************************************************************/
int?balance（float?Angle）
{??
???float?Bias;???????????????????????//傾角偏差
	?static?float?Last_BiasD_Bias;????//PID相關變量
	?int?balance;??????????????????????//PWM返回值?
	?Bias=Angle-ZHONGZHI;??????????????//求出平衡的角度中值?和機械相關
	?D_Bias=Bias-Last_Bias;????????????//求出偏差的微分?進行微分控制
	?balance=-Balance_KP*Bias-D_Bias*Balance_KD;???//===計算傾角控制的電機PWM??PD控制
???Last_Bias=Bias;???????????????????//保持上一次的偏差
	?return?balance;
}

/**************************************************************************
函數功能：位置PD控制?
入口參數：編碼器
返回??值：位置控制PWM
作????者：平衡小車之家
**************************************************************************/
int?Position（int?Encoder）
{??
???static?float?Position_PWMLast_PositionPosition_BiasPosition_Differential;
	?static?float?Position_Least;
??	Position_Least?=Encoder-Position_Zero;?????????????//===
????Position_Bias?*=0.8;		???
????Position_Bias?+=?Position_Least*0.2;	?????????????//===一階低通濾波器??
	??Position_Differential=Position_Bias-Last_Position;
	??Last_Position=Position_Bias;
		Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD;?//===速度控制	
	??return?Position_PWM;
}

/***************************************************

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\
?????目錄???????????0??2018-04-20?09:27??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\CONTROL\
?????文件???????????0??2016-02-22?11:47??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\CONTROL\MFC10DB.tmp
?????文件???????????0??2016-02-23?08:58??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\CONTROL\MFC3A5E.tmp
?????文件????????5604??2018-04-20?09:27??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\CONTROL\control.c
?????文件???????11154??2016-02-22?15:13??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\CONTROL\control.c~RF18b9998.TMP
?????文件?????????574??2016-04-23?13:57??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\CONTROL\control.h
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\
?????文件???????19342??2012-12-14?11:16??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\dmpKey.h
?????文件????????6765??2012-12-14?11:16??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\dmpmap.h
?????文件???????84104??2015-07-09?14:05??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\inv_mpu.c
?????文件????????4335??2015-07-09?14:05??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\inv_mpu.h
?????文件???????57839??2015-07-09?14:05??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\inv_mpu_dmp_motion_driver.c
?????文件????????3386??2015-07-09?14:05??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DMP\inv_mpu_dmp_motion_driver.h
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DataScope_DP\
?????文件????????2891??2015-07-09?14:05??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DataScope_DP\DataScope_DP.C
?????文件?????????586??2015-07-09?14:05??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\DataScope_DP\DataScope_DP.h
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\MPU6050\
?????文件???????11099??2015-12-04?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\MPU6050\MPU6050.c
?????文件???????14787??2015-10-17?15:04??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\MPU6050\mpu6050.h
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\filter\
?????文件????????2105??2015-12-07?15:52??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\filter\filter.c
?????文件?????????399??2015-12-07?15:52??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\filter\filter.h
?????目錄???????????0??2018-04-20?09:27??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\show\
?????文件????????4978??2018-04-20?09:27??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\show\show.c
?????文件?????????332??2016-04-19?16:22??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\BALANCE\show\show.h
?????目錄???????????0??2017-03-22?10:34??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\HARDWARE\
?????目錄???????????0??2018-04-20?09:27??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\HARDWARE\ADC\
?????文件????????4587??2018-04-20?09:27??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\HARDWARE\ADC\adc.c
?????文件?????????487??2017-12-17?15:51??【平衡小車之家】Forest?D1?旋轉倒立擺源碼\HARDWARE\ADC\adc.h
............此處省略161個文件信息