Hi3861 OpenHarmony 机械臂 （四）
3轴舵机控制机械臂，成品的样子，用个夹子给夹在桌子边，就成了一个伴随工作的辅助工具。
最初的目标是做个5轴驱动的机械臂，是这个样子的：
但是我买的的舵机是20KG的，头太重，举起来很抖，很不稳定，然后就退而求其次，改为3舵机结构。
在这里分享个经验，玩金属多轴机械臂，50块钱以下的舵机就不要买了，真是一分钱一分货，可以买个30多块钱的做为机械夹的控制部分。
刚装好6轴结构，真的很酷，很重很有机械感，像头金属兽。但是驱动起来需要很大的动力，一个稍微好点的40KG舵机很贵，我一直想用最低成本做个耐用好用的机械臂，我尝试了多款舵机，低性能的舵机不仅控制精度不行，而且很容易坏，炸一次坏一个。
舵机尸体
所以就别浪费钱了，我最开始买的松甲舵机一直随我用到最后，炸过好几次，虽然轴已经歪了，但是还能用，不是做广告，良心推荐。
安装过程，耐心、细致点，然后一些必要的工具，尖嘴钳，镊子，螺丝刀。
前面已经写了必要的代码，方便调试，在这个基础上，继续添加所需的功能，自己写代码就是好，可以随便改。
再写代码之前先复习一下高中的数学知识，勾股定理：
正弦函数 sin a/c ∠A的对边比斜边
余弦函数 cos b/c ∠A的邻边比斜边
正切函数 tan a/b ∠A的对边比邻边
余切函数 cot b/a ∠A的邻边比对边
正割函数 sec c/b ∠A的斜边比邻边
余割函数 csc c/a ∠A的斜边比对边
三角函数最重要的作用是能通过角度算出边长，通过边长算出角度。
两条机械臂构成一个三角形，我们已知2条机械臂的长度，a = 后段机械臂的长度，b = 前段机械臂的长度，我们要到达的D点，设定D点相对于舵机的坐标dx，dy。
通过D点的坐标求出c边的长度，然后知道了c的长度和a、b的长度就能求出，A、B、C的角度。
我在这里列出一些公式算法：
// 已知直角三角形abc中 C=90度，斜边c、a, 求 A 角度
public int Solve_RT_AC_Angel(int al, int cl)
{
double l1 = al;
double l2 = cl;
double rad = Math.Sin(l1 / l2);
double ang = rad * (180 / pai);
int A = (int)Math.Round(ang);
return A;
}
// 已知直角三角形abc中 C=90度，斜边c、b, 求 A 角度
public int Solve_RT_BC_Angel(int bl, int cl)
{
double l1 = bl;
double l2 = cl;
double rad = Math.Cos(l1 / l2);
double ang = rad * (180 / pai);
int A = (int)Math.Round(ang);
return A;
}
// 已知直角三角形abc中 C=90度，斜边c、b, 求 A 角度
public int Solve_RT_AB_Angel(int al, int bl)
{
double l1 = al;
double l2 = bl;
double rad = Math.Tan(l1 / l2);
double ang = rad * (180 / pai);
int A = (int)Math.Round(ang);
return A;
}
// 已知直角三角形abc中 C=90度，斜边c、A , 求 b 长度
public int Solve_RT_B_Side(int cl, int A)
{
double rad = pai / 180 * A;
double len = cl * Math.Cos(rad);
int bl = (int)Math.Round(len);
return bl;
}
// 已知直角三角形abc中 C=90度，斜边c、A , 求 a 长度
public int Solve_RT_A_Side(int cl, int A)
{
double rad = pai / 180 * A;
double len = cl * Math.Sin(rad);
int al = (int)Math.Round(len);
return al;
}
// 已知直角三角形 abc 已知 a、b、长度, C = 90 , 求 c 的长度
public int Solve_RT_C_Side(int al, int bl)
{
double l1 = al;
double l2 = bl;
double len = Math.Sqrt(l1 * l1 + l2 * l2);
int cl = (int)Math.Round(len);
return cl;
}
// 任意三角形 abc 已知 a、b、C , 求 c 的长度
public int Solve_C_Length(int al, int bl, int C)
{
double rad = pai / 180 * C;
double len = Math.Sqrt(al * al + bl * bl - 2 * al * bl * Math.Cos(rad));
int cl = (int)Math.Round(len);
return cl;
}
// 任意三角形 abc 已知 a、b、c 的长度， 求 A 角度
public int Solve_A_Angel(int al, int bl, int cl)
{
double l1 = al;
double l2 = bl;
double l3 = cl;
double cos_a = (l2 * l2 + l3 * l3 - l1 * l1) / (2 * l2 * l3);
double rad_a = Math.Acos(cos_a);
double ang_a = rad_a * (180 / pai);
int A = (int)Math.Round(ang_a);
return A;
}
// 任意三角形 abc 已知 a、b、c 的长度， 求 B 角度
public int Solve_B_Angel(int al, int bl, int cl)
{
double l1 = al;
double l2 = bl;
double l3 = cl;
double cos_b = (l1 * l1 + l3 * l3 - l2 * l2) / (2 * l1 * l3);
double rad_b = Math.Acos(cos_b);
double ang_b = rad_b * (180 / pai);
int B = (int)Math.Round(ang_b);
return B;
}
// 任意三角形 abc 已知 a、b、c 的长度， 求 C 角度
public int Solve_C_Angel(int al, int bl, int cl)
{
double l1 = al;
double l2 = bl;
double l3 = cl;
double cos_c = (l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2);
double rad_c = Math.Acos(cos_c);
double ang_c = rad_c * (180 / pai);
int C = (int)Math.Round(ang_c);
return C;
}知识有限，只能通过一步一步推导出结果，想到最近很火的姜萍，如果团队里有这样一个人她就能帮我们简化成一条简单的公式。
side_x = e.X;
side_y = e.Y;
int rt_al;
int rt_bl;
int o_x = 200;
int o_y = s_height - h_len;
rt_al = Math.Abs(side_y - o_y);
rt_bl = Math.Abs(side_x - o_x);
if(side_x < o_x)
{
Print_Msg("输入超出范围");
return;
}
c_len = solve.Solve_RT_C_Side(rt_al, rt_bl);
int rt_an;
int rt_bn;
int rt_cn;
rt_an = solve.Solve_A_Angel(rt_al, rt_bl, c_len);
rt_bn = solve.Solve_B_Angel(rt_al, rt_bl, c_len);
rt_cn = solve.Solve_C_Angel(rt_al, rt_bl, c_len);
ang_a = solve.Solve_A_Angel(a_len, b_len, c_len);
ang_b = solve.Solve_B_Angel(a_len, b_len, c_len);
ang_c = solve.Solve_C_Angel(a_len, b_len, c_len);
if (ang_a < 0 || ang_b < 0 || ang_c < 0)
{
Print_Msg("输入不满足长度要求");
return;
}
int an;
int bn = 0;
if (side_y < o_y)
{
an = rt_an + ang_b;
if (an < 90)
{
bn = 90 - an;
c_dot_y = solve.Solve_RT_B_Side(a_len, bn);
c_dot_x = solve.Solve_RT_A_Side(a_len, bn);
c_dot_x = o_x + c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = 90 + bn;
}
if (an > 90 && an < 180)
{
bn = 180 - an;
c_dot_x = solve.Solve_RT_B_Side(a_len, bn);
c_dot_y = solve.Solve_RT_A_Side(a_len, bn);
c_dot_x = o_x - c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = bn;
}
if(an > 180)
{
Print_Msg("超出范围");
return;
}
}
if (side_y > o_y)
{
an = rt_bn + ang_b;
if (an < 90)
{
Print_Msg("超出范围");
return;
}
if (an > 90 && an < 180)
{
bn = 180 - an;
c_dot_x = solve.Solve_RT_A_Side(a_len, bn);
c_dot_y = solve.Solve_RT_B_Side(a_len, bn);
c_dot_x = o_x + c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = 90 + bn;
}
if (an > 180)
{
bn = 270 - an;
c_dot_x = solve.Solve_RT_B_Side(a_len, bn);
c_dot_y = solve.Solve_RT_A_Side(a_len, bn);
c_dot_x = o_x - c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = bn;
}
}
label_len_c.Text = "c边：" + c_len.ToString();
if (ang_a > 0)
{
label_ang_a.Text = "a角：" + ang_a.ToString();
}
else
{
label_ang_a.Text = "a角：超范围";
}
if (ang_b > 0)
{
label_ang_b.Text = "b角：" + ang_d.ToString();
}
else
{
label_ang_b.Text = "b角：超范围";
}
if (ang_c > 0)
{
label_ang_c.Text = "c角：" + ang_c.ToString();
}
else
{
label_ang_c.Text = "c角：超范围";
}图像化：
演示视频
舵机操作演示
最后一点总结：
我现在的控制依然是一步一步进行，炸机真的炸怕了，再往后，可以把单片机的控制部分全部放到上位机上运行，减少hi3861处理负担，可以把多个角度数据一次发到舵机控制板，同时运行。
舵机的精度问题，一般好的舵机控制精度单位都是几个微秒，500us到2500us范围，PCA9685的控制寄存器只有12位，最大4096个数值，所以它注定很难实现很高的控制精度，要换一个好的舵机控制板也很贵，毕竟PCA9685才10几块钱。PCA9685跟精度不高的SG90小舵机搭配的挺好的，建议入手几个，一个只有几块钱，先在小舵机机上调试程序，程序通了再上大舵机，炸坏一个就没一个。舵机的数值角度跟实际的角度有误差，我加了一个补偿，进行实际的微调。
外国的5轴机械臂运行的那么好，是因为有好多舵机，好的算法，还有经验的积累。我们也可以慢慢实现，未来都不是问题，天下大事莫做于细，天下难事莫做于易，把大问题化成一个个简单的小问题，一个个解决。
社区规范：仅讨论OpenHarmony相关问题。
更多推荐
6
0- 0
已为社区贡献10条内容
所有评论(0)