#include?“control.h“	
#include?“stm32f10x.h“
#include?“sys.h“
??/**************************************************************************
作者：平衡小車之家
我的淘寶小店：http://shop114407458.taobao.com/
**************************************************************************/
int?Balance_PwmVelocity_PwmTurn_Pwm;
u8?Flag_Target;
u32?Flash_R_Count;
int?Voltage_TempVoltage_CountVoltage_All;
float?Last_TargetXLast_TargetY;
#define?l0?0.1635f
#define?l1?0.151f
#define?l2?0.076f??
#define?Ratio?5.111f

/**************************************************************************
函數功能：數學模型
入口參數：末端執(zhí)行器位姿態(tài)
返回??值：無
**************************************************************************/
u8?Kinematic_Analysis（float?xfloat?yfloat?Betafloat?Alphafloat?Gamma）
{??
	??????float?mnkabctheta1theta2theta3s1ps2;
				m=l2*cos（Alpha）-x;???//中間變量
				n=l2*sin（Alpha）-y;???//中間變量
	??????//k=（l0*l0-l1*l1-m*m-n*n）/2/l1;?????//過程系數
	??????k=（l1*l1-l0*l0-m*m-n*n）/2/l0;//中間變量
	??????a=m*m+n*n;?????????????//解一元二次方程
	??????b=-2*n*k;
	??????c=k*k-m*m;
	???
	??????if（b*b-4*a*c<=0）?
????????{?
				TargetX=Last_TargetX;		
			??TargetY=Last_TargetY;		
				return?0;?//防止出現非實數解
				}
	
	??????theta1=（-b+sqrt（b*b-4*a*c））/2/a;??//得到一元二次方程的解，只取其中一個，另外一個解是（-b+sqrt（b*b-4*a*c））/2/a
	??????theta1=asin（theta1）*180/PI;???????//弧度換成角度
				Theta1_Temp?=?theta1;
				
	??????if（theta1>180）theta1=180;???????????//控制舵機的最大角度±90°
				if（theta1<-180）theta1=-180;
				
			???//k=（l1*l1-l0*l0-m*m-n*n）/2/l0;//中間變量
			??k=（l0*l0-l1*l1-m*m-n*n）/2/l1;
				a=m*m+n*n;????????????????????????//解一元二次方程
				b=-2*n*k;
				c=k*k-m*m;
	
	??????if（b*b-4*a*c<=0）??
				{	
				TargetX=Last_TargetX;		
			??TargetY=Last_TargetY;
				return?0;?????????//防止出現非實數解
				}
	??????s1ps2=（-b-sqrt（b*b-4*a*c））/2/a;??????//得到一元二次方程的解，只取其中一個，另外一個解是（-b+sqrt（b*b-4*a*c））/2/a
	??????s1ps2=asin（s1ps2）*180/PI;????????????//弧度換成角度
	
				if（s1ps2>180）?theta2=180;????
				if（s1ps2<-180）theta2=-180;??
				
	??????theta2=s1ps2-theta1;
				Theta2_Temp?=?theta2;
			??if（theta2>180）theta2=180;??????//控制舵機的最大角度±90°
				if（theta2<-180）theta2=-180;????//控制舵機的最大角度±90°
	
	??????theta3=Alpha*180/PI-theta1-theta2;???//求關節(jié)3角度
				Theta3_Temp?=?theta3;
				if（theta3>180）theta3=180;
				if（theta3<-180）theta3=-180;	????//控制舵機的最大角度±90°
				
				
				
				
				
???????//?Target1???=?750-（Beta）*Ratio;???//作用到輸出??
????????Target1???=?590-（theta1-90）*Ratio;??//400
	??????Target2???=?780-（theta2）*Ratio;???//730
	??????Target3???=?550-（theta3）*Ratio;??//增大值往下??//680
			//	Target4???=?800-（Gamma）*Ratio;???//6自由度額外增加的自由度??
			

				
				Last_TargetX=TargetX;
				Last_TargetY=TargetY;
				return?0;
}

/**************************************************************************
函數功能：定時中斷函數?所有的控制代碼都在這里面		?
**************************************************************************/
int?TIM3_IRQHandler（void）
{????
	u8?temp;
	if?（TIM_GetITStatus（TIM3?TIM_IT_Update）?!=?R

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

?????文件????????399??2011-05-09?18:17??機器人機械臂最終版\keilkill.bat

?????文件??????13368??2018-08-10?14:47??機器人機械臂最終版\MiniBalance\CONTROL\control.c

?????文件???????1083??2017-08-25?07:45??機器人機械臂最終版\MiniBalance\CONTROL\control.h

?????文件???????2891??2015-07-09?14:05??機器人機械臂最終版\MiniBalance\DataScope_DP\DataScope_DP.C

?????文件????????586??2015-07-09?14:05??機器人機械臂最終版\MiniBalance\DataScope_DP\DataScope_DP.h

?????文件???????6276??2018-07-31?00:13??機器人機械臂最終版\MiniBalance\show\show.c

?????文件????????354??2016-01-06?14:35??機器人機械臂最終版\MiniBalance\show\show.h

?????文件??????17273??2010-06-07?10:25??機器人機械臂最終版\MiniBalance_COER\core_cm3.c

?????文件??????85714??2011-02-09?14:59??機器人機械臂最終版\MiniBalance_COER\core_cm3.h

?????文件??????12765??2011-03-10?10:52??機器人機械臂最終版\MiniBalance_COER\startup_stm32f10x_md.s

?????文件???????2935??2017-08-25?12:12??機器人機械臂最終版\MiniBalance_HARDWARE\ADC\adc.c

?????文件????????482??2017-07-12?22:49??機器人機械臂最終版\MiniBalance_HARDWARE\ADC\adc.h

?????文件???????2887??2015-06-07?00:48??機器人機械臂最終版\MiniBalance_HARDWARE\DataScope_DP\DataScope_DP.C

?????文件????????581??2015-05-04?12:37??機器人機械臂最終版\MiniBalance_HARDWARE\DataScope_DP\DataScope_DP.h

?????文件??????11040??2016-09-27?07:59??機器人機械臂最終版\MiniBalance_HARDWARE\IIC\IOI2C.c

?????文件???????3118??2016-03-05?15:33??機器人機械臂最終版\MiniBalance_HARDWARE\IIC\IOI2C.h

?????文件???????3761??2017-07-07?08:38??機器人機械臂最終版\MiniBalance_HARDWARE\KEY\key.c

?????文件????????605??2017-07-07?08:27??機器人機械臂最終版\MiniBalance_HARDWARE\KEY\key.h

?????文件???????1196??2016-06-05?14:52??機器人機械臂最終版\MiniBalance_HARDWARE\LED\LED.C

?????文件????????383??2016-06-05?14:52??機器人機械臂最終版\MiniBalance_HARDWARE\LED\LED.H

?????文件???????7636??2018-08-05?20:51??機器人機械臂最終版\MiniBalance_HARDWARE\MOTOR\motor.c

?????文件????????406??2018-08-05?19:31??機器人機械臂最終版\MiniBalance_HARDWARE\MOTOR\motor.h

?????文件???????6227??2016-06-05?14:52??機器人機械臂最終版\MiniBalance_HARDWARE\OLED\oled.c

?????文件???????1179??2016-06-05?14:52??機器人機械臂最終版\MiniBalance_HARDWARE\OLED\oled.h

?????文件??????16174??2013-09-13?11:45??機器人機械臂最終版\MiniBalance_HARDWARE\OLED\oledfont.h

?????文件???????4747??2017-10-20?15:25??機器人機械臂最終版\MiniBalance_HARDWARE\PS2\pstwo.c

?????文件???????1959??2017-10-20?15:25??機器人機械臂最終版\MiniBalance_HARDWARE\PS2\pstwo.h

?????文件???????6222??2017-02-21?21:32??機器人機械臂最終版\MiniBalance_HARDWARE\STMFLASH\stmflash.c

?????文件???????1550??2017-02-19?17:22??機器人機械臂最終版\MiniBalance_HARDWARE\STMFLASH\stmflash.h

?????文件???????1454??2018-08-05?21:18??機器人機械臂最終版\MiniBalance_HARDWARE\TIMER\timer.c

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