#include?“control.h“	
??/**************************************************************************
作者：平衡小車之家
我的淘寶小店：http://shop114407458.taobao.com/
**************************************************************************/
int?Balance_PwmVelocity_PwmTurn_Pwm;
u8?Flag_TargetStep=0;
u32?Flash_R_Count;
int?Voltage_TempVoltage_CountVoltage_All;
float?Last_Position_ALast_Position_BLast_Position_C;
/***************************************************
**************************************************************************/
#define?a????????????（0.14f）???????????????????
#define?b????????????（0.16f）?
#define?h????????????（0.097f）?
#define?L????????????（0.06f）?
/**************************************************************************
函數(shù)功能：運(yùn)動數(shù)學(xué)模型
入口參數(shù)：X?Y?Z?三軸速度或者位置
返回??值：無
**************************************************************************/
void?Kinematic_Analysis（int?Pxint?Pyint?Pz）
{?
	??????float?target_xtarget_ytarget_z;
	??????float?cde;
	??????target_x=（float）Px/1000;
				target_y=（float）Py/1000;
				target_z=（float）Pz/180*PI;
	??????c=target_x-L;
				d=target_y-h;
	??????e=sqrt（pow（d2）+pow（c2））;??
		????Target_A???=acos（（a*a+e*e-b*b）/2/a/e）+atan（d/c）;
????????Target_B???=Target_A-PI+acos（（a*a+b*b-e*e）/2/a/b）;
				Target_C???=-target_z;

				Target_A*=-RATIO;
				Target_B*=-RATIO;
				Target_C*=-RATIO;
}

/**************************************************************************
函數(shù)功能：所有的控制代碼都在這里面
?????????5ms定時(shí)中斷由MPU6050的INT引腳觸發(fā)
?????????嚴(yán)格保證采樣和數(shù)據(jù)處理的時(shí)間同步				?
**************************************************************************/
int?EXTI15_10_IRQHandler（void）?
{????
	?if（INT==0）		
	{???
		???EXTI->PR=1<<15;??????????????????????????????????????????????????????//清除中斷標(biāo)志位???
		???Flag_Target=!Flag_Target;
			?	Key（）;????????????????????????????????????????//===掃描按鍵狀態(tài)?單擊雙擊可以改變小車運(yùn)行狀態(tài)
				Position_A=-Read_Encoder（3）;????????????	?		//===讀取編碼器的值
				Position_B=Read_Encoder（2）;???????????		?		//===讀取編碼器的值
				Position_C=Read_Encoder（4）;????????????	?	??	//===讀取編碼器的值
				Encoder_A=Position_A-Last_Position_A;?????????//===一介差分得到速度值
				Encoder_B=Position_B-Last_Position_B;?????????//===一介差分得到速度值
				Encoder_C=Position_C-Last_Position_C;?????????//===一介差分得到速度值
			?	if（Usart_Flag==0&&Usart_ON_Flag==1）??memcpy（rxbufUrxbuf8*sizeof（u8））;	//如果解鎖了串口控制標(biāo)志位，進(jìn)入串口控制模式
				Get_Angle（Way_Angle）;???????????????????????????????????????????????????//===更新姿態(tài)	
				if（CAN_ON_Flag==1||Usart_ON_Flag==1）?CAN_N_Usart_Control（）;??????????//接到串口或者CAN遙控解鎖指令之后，使能CAN和串口控制輸入
				if（Xianfu_XYZ（））??????????//不超過運(yùn)動范圍且不在拖動示教模式下，執(zhí)行運(yùn)動學(xué)分析
				Kinematic_Analysis（Target_XTarget_YTarget_Z）;?//根據(jù)給定坐標(biāo)，解算出每個(gè)電機(jī)的位置

				Motor_A=Position_PID_A（Position_ATarget_A）>>5;//A電機(jī)位置控制
				Motor_B=Position_PID_B（Position_BTarget_B）>>5;//B電機(jī)位置控制
				Motor_C=-Position_PID_C（Position_CTarget_C）>>5;//C電機(jī)位置
????????Count_Velocity（）;

				if（（Position_A-Position_B>-3120）&&myabs（Encoder_A）>myabs（Encoder_B））Moto

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-02-10?16:06??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\CONTROL\
?????文件???????13270??2017-08-28?20:01??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\CONTROL\control.c
?????文件????????1213??2017-08-22?19:13??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\CONTROL\control.h
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\
?????文件???????19342??2012-12-14?11:16??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\dmpKey.h
?????文件????????6765??2012-12-14?11:16??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\dmpmap.h
?????文件???????84104??2015-07-09?14:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\inv_mpu.c
?????文件????????4335??2015-07-09?14:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\inv_mpu.h
?????文件???????57839??2015-07-09?14:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\inv_mpu_dmp_motion_driver.c
?????文件????????3386??2015-07-09?14:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DMP\inv_mpu_dmp_motion_driver.h
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DataScope_DP\
?????文件????????2891??2015-07-09?14:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DataScope_DP\DataScope_DP.C
?????文件?????????586??2015-07-09?14:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\DataScope_DP\DataScope_DP.h
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\MPU6050\
?????文件???????11303??2017-08-28?14:57??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\MPU6050\MPU6050.c
?????文件???????14985??2017-08-28?14:57??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\MPU6050\mpu6050.h
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\filter\
?????文件????????2105??2015-12-07?15:52??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\filter\filter.c
?????文件?????????399??2016-04-06?14:10??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\filter\filter.h
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\show\
?????文件????????3982??2017-08-28?14:57??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\show\show.c
?????文件?????????354??2017-04-20?18:37??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance\show\show.h
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_COER\
?????文件???????17273??2010-06-07?10:25??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_COER\core_cm3.c
?????文件???????85714??2011-02-09?14:59??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_COER\core_cm3.h
?????文件???????15503??2011-03-10?10:52??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_COER\startup_stm32f10x_hd.s
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_HARDWARE\
?????目錄???????????0??2018-02-10?16:05??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_HARDWARE\ADC\
?????文件????????2932??2017-08-21?12:20??1.步進(jìn)電機(jī)版機(jī)械臂源碼（標(biāo)準(zhǔn)版）\MiniBalance_HARDWARE\ADC\adc.c
............此處省略293個(gè)文件信息