4zhou 舵机

四轴舵机解算

#include "math.h"
#include 
int main()
{
	
	float a1,a2,a3,a4;                  //a1为底部圆台高度 剩下三个为三个机械臂长度 
	float j1,j4,j2,j3;                      //四个姿态角
	float L,H,P;	             //L =a2*sin(j2) + a3*sin(j2 + j3);H = a2*cos(j2) + a3*cos(j2 + j3); P为底部圆盘半径R
	float j_all;	                             //j2,j3,j4之和
	float len,high;                       //总长度,总高度
	float Cosj3,Sinj3;                   //用来存储cosj3,sinj3数值
	float Cosj2,Sinj2;
	float K1,K2;
	float X,Y,Z;  	             //输入 （X,Y,Z）坐标
	int i;
	float n,m,q;
	n = 0;
	m = 0;
	q = 0;
	//目标点坐标（X，Y，Z）
	X = 0;
	Y = 18;
	Z = 0;

	P = 14;     //底部圆盘半径
	a1 = 12; 	//底部圆盘高度	            
	a2 = 12;    //机械臂长度
	a3 = 12;
	a4 = 12;
	
	if (X == 0) 
	    j1=90;
	else 
	    j1 = atan((Y+P)/X)*(57.3);

	for(i=0;i<=180;i++)
	{	
		j_all = 3.1415927*i/180;

		len = sqrt((Y+P)*(Y+P)+X*X);
		high = Z;
			
		L = len	- a4*sin(j_all);
		H = high - a4*cos(j_all) - a1;
		
		Cosj3 = ((L*L)+(H*H)-((a2)*(a2))-((a3)*(a3)))/(2*(a2)*(a3));
		Sinj3 = (sqrt(1-(Cosj3)*(Cosj3)));
		
		j3 = atan((Sinj3)/(Cosj3))*(57.3);
		
		K2 = a3*sin(j3/57.3);
		K1 = a2+a3*cos(j3/57.3);
		
		Cosj2 = (K2*L+K1*H)/(K1*K1+K2*K2);
		Sinj2 = (sqrt(1-(Cosj2)*(Cosj2)));
		
		j2 = atan((Sinj2)/(Cosj2))*57.3;
		j4 = j_all*57.3- j2 - j3;
		
		if(j2>=0&&j3>=0&&j4>=-90&&j2<=180&&j3<=180&&j4<=90)
		{
			n=n+1;
		}
    } 
   
   
   	for(i=0;i<=180;i++)
	{
		j_all = 3.1415927*i/180;
		
		len = sqrt((Y+P)*(Y+P)+X*X);
		high = Z;

		L = len	- a4*sin(j_all);
		H = high - a4*cos(j_all) - a1;
		
		Cosj3 = ((L*L)+(H*H)-((a2)*(a2))-((a3)*(a3)))/(2*(a2)*(a3));
		Sinj3 = (sqrt(1-(Cosj3)*(Cosj3)));
		
		j3 = atan((Sinj3)/(Cosj3))*(57.3);
		
		K2 = a3*sin(j3/57.3);
		K1 = a2+a3*cos(j3/57.3);
		
		Cosj2 = (K2*L+K1*H)/(K1*K1+K2*K2);
		Sinj2 = (sqrt(1-(Cosj2)*(Cosj2)));
		
		j2 = atan((Sinj2)/(Cosj2))*57.3;
		j4 = j_all*57.3- j2 - j3;
		
	    if(j2>=0&&j3>=0&&j4>=-90&&j2<=180&&j3<=180&&j4<=90)
		{
			m=m+1;
			if(m==n/2||m==(n+1)/2)
			{	
				break;
			}
		}
    }
   
   	printf("j1:%f,j2:%f\nj3:%f,j4:%f\n",j1,j2,j3,j4);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

https://www.bilibili.com/video/BV1Zr4y157s8/?spm_id_from=autoNext&vd_source=ab50fb2cad935bc4d9deeb7f9401fe48