#include?“control.h“	
#include?“filter.h“	
#define?PI?3.14159265
/**************************************************************************
作者：Mini?Balance?
淘寶店鋪：http://shop114407458.taobao.com/
**************************************************************************/

/**************************************************************************
函數功能：5MS定時中斷函數?5MS控制周期
入口參數：無
返回??值：無
作????者：Mini?Balance
**************************************************************************/
void?TIM1_UP_IRQHandler（void）??
{????
	if（TIM1->SR&0X0001）//5ms定時中斷
	{???
		??TIM1->SR&=~（1<<0）;???????????????????????????????????????//===清除定時器1中斷標志位		?
??		Led_Flash（400）;??????????????????????????????????????????//===LED閃爍;	??????
			Key（）;??????????????????????????????????????????????????//===掃描按鍵狀態
?			Moto1=2000-5*Pitch+gyro[1]*8/100??-5*Roll+gyro[0]*8/100??;//平衡的PD控制?可自行加入Z軸P控制抑制自旋?
			Moto2=2000+5*Pitch-gyro[1]*8/100??+5*Roll-gyro[0]*8/100??;//////////////?
			Moto3=2000-5*Pitch+gyro[1]*8/100??+5*Roll-gyro[0]*8/100??;//////////////?
			Moto4=2000+5*Pitch-gyro[1]*8/100??-5*Roll+gyro[0]*8/100??;//////////////?
???		Xianfu_Pwm（）;????????????????????????????????????????????//===PWM限幅
??????if（Turn_Off（）==0）???????????????????//===如果不存在異常
?			Set_Pwm（Moto1Moto2Moto3Moto4）;????????????????????????//===賦值給PWM寄存器????		
	}???????
}?


/**************************************************************************
函數功能：賦值給PWM寄存器
入口參數：PWM
返回??值：無
作????者：Mini?Balance
**************************************************************************/
void?Set_Pwm（int?moto1int?moto2int?moto3int?moto4）
{
			PWMA=myabs（moto1）;
			PWMB=myabs（moto2）;	
	???	PWMC=myabs（moto3）;
			PWMD=myabs（moto4）;	
}

/**************************************************************************
函數功能：限制PWM賦值?
入口參數：無
返回??值：無
作????者：Mini?Balance
**************************************************************************/
void?Xianfu_Pwm（void）
{	
	??int?Amplitude=4000;?
????if（Moto1<0）?Moto1=0;	
		if（Moto1>Amplitude）??Moto1=Amplitude;	
	??if（Moto2<0）?Moto2=0;	
		if（Moto2>Amplitude）??Moto2=Amplitude;		
	??if（Moto3<0）?Moto3=0;	
		if（Moto3>Amplitude）??Moto3=Amplitude;	
	??if（Moto4<0）?Moto4=0;	
		if（Moto4>Amplitude）??Moto4=Amplitude;		
	
}

/**************************************************************************
函數功能：異常關閉電機
入口參數：傾角和電壓
返回??值：1：異常??0：正常
作????者：Mini?Balance
**************************************************************************/
u8?Turn_Off（void）
{
	????u8?temp;
			if（Pitch<-40||Pitch>40||1==Flag_Stop||Roll<-40||Roll>40）
			{	?????????????????????????????????????????????????//===傾角大于40度關閉電機
??????temp=1;????????????????????????????????????????????//===Flag_Stop置1關閉電機
??????}
			else
??????temp=0;
??????return?temp;			
}
	

/**************************************************************************
函數功能：絕對值函數
入口參數：int
返回??值：unsigned?int
**************************************************************************

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\BALANCE\
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\BALANCE\CONTROL\
?????文件????????3645??2015-10-17?19:30??6050?角度獲取\BALANCE\CONTROL\control.c
?????文件?????????532??2015-10-17?18:13??6050?角度獲取\BALANCE\CONTROL\control.h
?????目錄???????????0??2018-03-10?16:56??6050?角度獲取\BALANCE\DataScope_DP\
?????文件????????2667??2018-03-10?16:56??6050?角度獲取\BALANCE\DataScope_DP\DataScope_DP.C
?????文件?????????586??2015-07-09?14:05??6050?角度獲取\BALANCE\DataScope_DP\DataScope_DP.h
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\BALANCE\DMP\
?????文件???????19342??2012-12-14?11:16??6050?角度獲取\BALANCE\DMP\dmpKey.h
?????文件????????6765??2012-12-14?11:16??6050?角度獲取\BALANCE\DMP\dmpmap.h
?????文件???????84104??2015-07-09?14:05??6050?角度獲取\BALANCE\DMP\inv_mpu.c
?????文件????????4335??2015-07-09?14:05??6050?角度獲取\BALANCE\DMP\inv_mpu.h
?????文件???????57839??2015-07-09?14:05??6050?角度獲取\BALANCE\DMP\inv_mpu_dmp_motion_driver.c
?????文件????????3386??2015-07-09?14:05??6050?角度獲取\BALANCE\DMP\inv_mpu_dmp_motion_driver.h
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\BALANCE\filter\
?????文件????????2811??2015-07-09?19:48??6050?角度獲取\BALANCE\filter\filter.c
?????文件?????????931??2015-07-09?19:48??6050?角度獲取\BALANCE\filter\filter.h
?????目錄???????????0??2018-03-10?16:44??6050?角度獲取\BALANCE\MPU6050\
?????文件???????11157??2018-03-10?16:42??6050?角度獲取\BALANCE\MPU6050\MPU6050.c
?????文件???????14791??2018-03-10?16:44??6050?角度獲取\BALANCE\MPU6050\mpu6050.h
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\BALANCE\show\
?????文件????????2119??2015-10-17?19:23??6050?角度獲取\BALANCE\show\show.c
?????文件?????????331??2015-10-16?09:30??6050?角度獲取\BALANCE\show\show.h
?????目錄???????????0??2018-03-10?16:54??6050?角度獲取\HARDWARE\
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\HARDWARE\IIC\
?????文件???????10558??2015-10-07?22:15??6050?角度獲取\HARDWARE\IIC\IOI2C.c
?????文件????????3123??2015-10-07?22:15??6050?角度獲取\HARDWARE\IIC\IOI2C.h
?????目錄???????????0??2018-03-10?16:28??6050?角度獲取\HARDWARE\KEY\
?????文件????????1633??2015-10-07?21:37??6050?角度獲取\HARDWARE\KEY\key.c
?????文件?????????235??2015-10-16?13:57??6050?角度獲取\HARDWARE\KEY\key.h
?????目錄???????????0??2018-03-10?16:58??6050?角度獲取\HARDWARE\LED\
............此處省略118個文件信息