#include?“DSP_calculate.h“
#include?“delay.h“
#include?“math.h“


#define?L0?138
#define?L1?105
#define?L2?100
#define?L3?132

struct?mechinehand?hand;
/*
caculate?the?angle?of?z--
*/

float?angle0_get（float?xfloat?y）
{
static?double?r2d=180.0/3.1415926;
float?angle;
		if（x>0）
		{
			angle=r2d*atan（y/x）;
		}
		else?if（x==0）
		{
				if（y>0）
					angle=90;
				else?if（y<0）
					angle=-90;
		}
		else?if（x<0）
		{
				if（y>0）
					angle=180+r2d*atan（y/x）;
				else?if（y<0）
					angle=-180+r2d*atan（y/x）;
		}
	return?angle;
}
/*
calculate?the?angle?of?all
then?it?will?save?in?the?hand?structure
*/
u8?hand_caculate（void）
{
	int?i;
static?double?r2d=180.0/3.1415926;
float?Alpha[4]={0000};
float?R2XYxyzS3C3S2C2Rxt1af1Cxtaxta;
X=（float）（hand.OBJX[0]*hand.OBJX[0]+hand.OBJX[1]*hand.OBJX[1]）;
//arm_sqrt_f32（X&X）;
X=sqrt（X）;
//arm_sin_cos_f32（hand.Handangle?&S3?&C3）;
S3=sin（r2d*hand.Handangle）;

x=X-L3*C3;
z=hand.OBJX[2]-（L3*S3+L0）;
R2=x*x+z*z;
//arm_sqrt_f32（R2&R）;
R=sqrt（R2）;
C2=（R2-（L1*L1+L2*L2））/（2*L2*L1）;
Alpha[2]=r2d*acos（C2）;
Cxta=（（L1*L1+R2）-L2*L2）/（2*R*L1）;
xta=r2d*acos（Cxta）;
af1=r2d*atan（-z/x）;
Alpha[1]=（90-xta）+af1;
Alpha[3]=（Alpha[1]+Alpha[2]）+hand.Handangle-90;
Alpha[0]=angle0_get（hand.OBJX[0]hand.OBJX[1]）;
for（i=0;i<4;i++）
{
?if（Alpha[i]>=0）
?{
	?hand.Angle[i]=（short）（Alpha[i]+0.5）;
?}
?else
?{
	?hand.Angle[i]=（short）（Alpha[i]-0.5）;
?}
}
		return?1;
}

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????1479??2019-01-15?10:35??四自由度機(jī)械臂逆解析\hand.c

?????文件????????295??2019-01-15?10:17??四自由度機(jī)械臂逆解析\hand.h

?????目錄??????????0??2019-04-23?14:19??四自由度機(jī)械臂逆解析

-----------?---------??----------?-----??----

?????????????????1774????????????????????3