///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//上周末參加了北京的四軸沙龍，很有意思的活動。通過活動，我意識到，想讓跟很多的人來加入我們這個團隊，一起來研究四軸！
//我身邊玩四軸的人幾乎沒有，因此程序的移植和調試都是自己做，有時遇到不懂的，也不方便問。
//現在在EEPW這個論壇里，越來越多的人樂于奉獻出自己的代碼，這是個好的現象，
//因為只有不斷交流，才能取得更大的進步！三個臭皮匠，賽過諸葛亮~?
//????
//考慮再三，我決定把自己移植調試的代碼開源出來，供大家學習。希望大家能一起，來加入這個活動，來加入我們。
//因為咱們的套件都是一樣的，硬件上是一致的了，那就只需要研究軟件上的算法就行了。交流起來會方便的多！
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//四軸DIY活動詳情：http://www.eepw.com.cn/event/action/QuadCopter_DIY/
//
//四軸論壇：http://forum.eepw.com.cn/forum/368/1?
//
//我的四軸DIY進程貼：http://forum.eepw.com.cn/thread/248747/1
//
//淘寶店鋪：http://item.taobao.com/item.htm?spm=a230r.1.14.23.sYD4gY&id=35605621244
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//	BY:讓四軸飛，2014430
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include?“control.h“
#include?“BSP.H“
#include?“rc.h“
#include?“imu.h“

PID?PID_ROLPID_PITPID_YAW;
u8?ARMED?=?0;
extern?vs16?QHZYXZ;

float?Get_MxMi（float?numfloat?maxfloat?min）
{
	if（num>max）
		return?max;
	else?if（num		return?min;
	else
		return?num;
}

void?CONTROL（float?rol_now?float?pit_now?float?yaw_now?float?rol_tar?float?pit_tar?float?yaw_tar）	
{						//當前姿態角，，，目標姿態角
	u16?moto1=0moto2=0moto3=0moto4=0;
	vs16?throttle;
	
	float?rol?=?rol_tar?+?rol_now;
	float?pit?=?pit_tar?+?pit_now;
	float?yaw?=?yaw_tar?+?yaw_now;
	
	throttle?=?Rc_Get.THROTTLE?-?1000;	//1000<遙控油門值<2000
	if（throttle<0）	throttle=0;
	
	PID_ROL.IMAX?=?throttle/10;			//積分限幅，積分值不超過當前油門值的一半
	PID_ROL.IMAX?=?Get_MxMi（PID_ROL.IMAX10000）;	//限制積分結果為，0到1000	
	PID_PIT.IMAX?=?PID_ROL.IMAX;
	
	PID_ROL.pout?=?PID_ROL.P?*?rol;
	PID_PIT.pout?=?PID_PIT.P?*?pit;
//////////////////、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、	
	/////////////0.1??0.1???30??30
	if（rol_tar*rol_tar<1?&&?pit_tar*pit_tar<1?&&?rol_now*rol_now<100?&&?pit_now*pit_now<100?&&?throttle>300）
	{			//防止角度大了，積分超調//目標姿態角水平，姿態角幾乎水平，油門值不太低
		PID_ROL.iout?+=?PID_ROL.I?*?rol;
		PID_PIT.iout?+=?PID_PIT.I?*?pit;
		PID_ROL.iout?=?Get_MxMi（PID_ROL.ioutPID_ROL.IMAX-PID_ROL.IMAX）;		//對輸出的積分限幅
		PID_PIT.iout?=?Get_MxMi（PID_PIT.ioutPID_PIT.IMAX-PID_PIT.IMAX）;
	}
	else?if（throttle<200）
	{			//油門值較小時，積分項清零
		PID_ROL.iout?=?0;
		PID_PIT.iout?=?0;
	}
//////////////////、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、	

//	rc_roll_d?=?rol_tar?-?getlast_roll;
//	getlast_roll?=?rol_tar;
//	PID_ROL.dout?=?PID_ROL.D?*?（MPU6050_GYRO_LAST.X+rc_roll_d*300）;//角速度+控制誤差微分
//	
//	rc_pitch_d?=?pit_tar?-?getlast_pitch;
//	getlast_pitch?=?pit_tar;
//	PID_PIT.dout?=?PID_PIT.D?*?（MPU6050_GYRO_LAST.Y+rc_pitch_d*300）;//角速度+控制誤差微分
	
	PID_ROL.dout?=?PID_ROL.D?*?MPU6050_GYRO_LAST.X;		
	PID_PIT.dout?=?PID_PIT.D?*?MPU6050_GYRO_LAST.Y;
//////////

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2014-05-22?21:30??FeiKong\
?????文件???????76877??2014-05-22?21:30??FeiKong\ANO_FLY.uvgui.Administrator
?????文件??????146671??2013-08-11?16:29??FeiKong\ANO_FLY.uvgui.LG
?????文件??????146099??2013-07-21?11:26??FeiKong\ANO_FLY.uvgui.SHAO
?????文件??????140411??2014-04-30?16:24??FeiKong\ANO_FLY.uvgui.Yang
?????文件???????73765??2014-05-03?21:27??FeiKong\ANO_FLY.uvgui_Administrator.bak
?????文件???????31262??2014-05-22?21:30??FeiKong\ANO_FLY.uvopt
?????文件???????22986??2014-05-22?21:30??FeiKong\ANO_FLY.uvproj
?????文件???????52252??2014-05-22?21:20??FeiKong\ANO_FLY_ANO_FLY.dep
?????目錄???????????0??2014-04-02?17:03??FeiKong\APP\
?????目錄???????????0??2014-05-22?21:20??FeiKong\APP\CONTROL\
?????文件????????5767??2014-05-22?21:20??FeiKong\APP\CONTROL\Control.c
?????文件?????????302??2013-08-09?20:04??FeiKong\APP\CONTROL\Control.h
?????目錄???????????0??2014-05-22?21:19??FeiKong\APP\I2C_6050\
?????文件????????1451??2014-03-16?02:09??FeiKong\APP\I2C_6050\delay.c
?????文件?????????333??2013-06-04?11:33??FeiKong\APP\I2C_6050\delay.h
?????文件???????11413??2014-05-22?21:19??FeiKong\APP\I2C_6050\I2C_MPU6050.c
?????文件????????2980??2014-04-28?00:45??FeiKong\APP\I2C_6050\I2C_MPU6050.h
?????目錄???????????0??2014-05-22?21:20??FeiKong\APP\IMU\
?????文件????????5877??2014-05-22?21:20??FeiKong\APP\IMU\IMU.c
?????文件?????????355??2013-08-09?21:26??FeiKong\APP\IMU\IMU.h
?????目錄???????????0??2014-05-22?21:20??FeiKong\APP\RC\
?????文件???????10375??2014-05-22?21:20??FeiKong\APP\RC\Rc.c
?????文件?????????595??2013-08-08?19:57??FeiKong\APP\RC\rc.h
?????目錄???????????0??2014-05-22?21:18??FeiKong\APP\SYS\
?????文件????????3267??2014-05-22?21:18??FeiKong\APP\SYS\main.c
?????文件????????8944??2014-05-22?21:18??FeiKong\APP\SYS\stm32f10x_it.c
?????文件????????2084??2013-07-21?11:26??FeiKong\APP\SYS\stm32f10x_it.h
?????目錄???????????0??2014-05-22?21:20??FeiKong\APP\UART\
?????文件????????1389??2014-05-22?21:20??FeiKong\APP\UART\Uart1.c
?????文件?????????200??2014-05-22?21:19??FeiKong\APP\UART\uart1.h
............此處省略356個文件信息