四自由度机械臂逆解析程序

这个一个四自由度的机械臂逆解析算法程序。通过设置坐标位置可以输出各个舵机的转动角度。

#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;
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1479  2019-01-15 10:35  四自由度机械臂逆解析\hand.c

     文件        295  2019-01-15 10:17  四自由度机械臂逆解析\hand.h

     目录          0  2019-04-23 14:19  四自由度机械臂逆解析

----------- ---------  ---------- -----  ----

                 1774                    3