# XY串联型机械臂算法实现

**Repository Path**: jianping724/harry-xy-robot

## Basic Information

- **Project Name**: XY串联型机械臂算法实现
- **Description**: 只使用两个电机实现在纸上画任意图形
- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 8
- **Created**: 2021-10-21
- **Last Updated**: 2021-10-21

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 双联杆型画图XY机械臂

# 联系方式
Harryzhang 503433013@qq.com/harryzhangpro@gmail.com

# 硬件实现代码
[传送链接](https://gitee.com/harryzhangabc/ji-lin-province-electronic-design-pattern)

# 介绍
突然有了一个想法：    
**如果不用第三个电机，只用两个电机、两根硬联杆实现画正弦波、三角波和任意图形？**(不用第三个电机也能做出来)   
于是，有了以下思路：   
- 在两个电机上安装两个活动联杆，然后联杆之间插入一支笔，如图所示：
![结构图片](https://gitee.com/harryzhangabc/harry-xy-robot/raw/master/pic/1-1.png)   
A点：放笔的地方  
L1、L2:硬质联杆   
绿色框框：A点的执行范围，笛卡尔坐标系，H、L点的Y已知，分别为H和0，这个是由你机器决定的。  
H:电机控制的滑块位置A(可以通过编码器得到) L：电机控制的滑块位置B(可以通过编码器得到) A:输入的坐标值
根据上图描述，我们要通过输入A点目标值(L1、L2已知，设定固定L1原点)，解算出H、L的位置。  

那么，通过这个思路，我只要输入A的值，然后解算出H、L的坐标，不就可以画正玄波、画三角波、画方波、画任意图形了吗！

# 机械臂正解思路
![思路图片](https://gitee.com/harryzhangabc/harry-xy-robot/raw/master/pic/2-1.png)

# 正解算法设计
个人感觉挺麻烦的，请对着上图看代码。
```cpp
static solution invx(solution solu2)
{	
	solution solu = solu2;
	float L1_U,L2_U;	

	if(solu.CACL_A > solu.CACL_B){
		L1_U = L1;
		L2_U = L2;
	}else{
		L1_U = L2;
		L2_U = L1;
	}

	float ax = solu.CACL_A-solu.CACL_B;

	float A = pow(H,2)+pow(L1_U,2)-pow(ax,2)-pow(L2_U,2);
	float B = 2*H*L1_U;
	float C = 2*ax*L2_U;

	float a = pow(B,2)*pow(L2_U,2)+pow(C,2)*pow(L1_U,2);
	float b = -2*L1_U*A*B*pow(L2_U,2)-2*H*pow(L1_U,2)*pow(C,2);
	float c = pow(A,2)*pow(L1_U,2)*pow(L2_U,2)-pow(C,2)*pow(L1_U,2)*pow(L2_U,2)+pow(C,2)*pow(L1_U,2)*pow(H,2);

	solution solu3 = caclduo(a,b,c);
	
	if(solu.CACL_A > solu.CACL_B){
		solu.solved.x = solu.CACL_B + sqrt(pow(L1,2)-pow(solu3.DeltaX,2));
		solu.solved.y = solu3.DeltaX;
	}else{
		solu.solved.x = solu.CACL_A + sqrt(pow(L2,2)-pow(solu3.DeltaX,2));
		solu.solved.y = H-solu3.DeltaX;
	}

	// cout << sqrt(pow(L2,2)-pow(solu.DeltaX,2)) << "," << sqrt(pow(L1,2)-pow(solu.DeltaX,2)) << "," << org_a << "," << org_b << "," << ax << endl;

	return solu;
}
```

# 机械臂逆解思路
![思路图片](https://gitee.com/harryzhangabc/harry-xy-robot/raw/master/pic/1.png)

# 逆解算法设计

假设，我们第一步输入了A(x,y)值，这个值是不能直接带入公式计算的，因为L1固定到原点以后，A点的活动范围只有以L1为半径的半圆，A(x,y)不能保证直接落到半圆上，否则会无解。此时我们需要把它转换成思路图片的相对坐标系，也就是让这个xy落到这个半圆上，才能够进行逆解计算。那么怎么才能找到这个"差值"呢？，这里我们设L点为(X0,0),则向量LA=(x-X0,y);则该向量的模为L1。利用这个关系，我们可以列出一个包含X0/x/y/L1的一个二元一次方程，然后计算它的根，并取较小(靠近原点)那个。
  
实现代码：  
```cpp
static solution getx(float x,float y)   //计算
{
	solution solu = caclduo(1,-2*x,pow(x,2)+pow(y,2)-pow(L1,2));    //计算二元一次函数，并取较小的那个根

	if(solu.success)
	{
		x -= solu.DeltaX;   //在这一步我们得到了最小的那个根，也就是L(X0,0),然后把他转换到相对坐标轴上，把原点通过X轴平移到L点。
		float a1 = atan(y/x);   //请结合上图，理解下面的代码
		float sina2 = (H-L1*sin(a1))/L2;    //请结合上图，理解下面的代码 
		float cosa2 = sqrt(1-pow(sina2,2));      //请结合上图，理解下面的代码
		float ax = L2*cosa2;

	    // cout << "a1:" << a1 << "  sina1:" << sin(a1) << "  sina2:" << sina2 << "  cosa2:" << cosa2 << "  ax:" << ax <<"  DeltaX:" << solu.DeltaX << endl;

		solu.CACL_A = solu.DeltaX + (x-ax);     //解算完成，由于是在相对坐标系上计算了，需要把原点移回绝对坐标系
		solu.CACL_B = solu.DeltaX;
	}
	return solu;
}

//ax^2+bx+c=0,返回较小方程的根
static solution caclduo(float a,float b,float c)    //二元一次方程计算函数，输入a,b,c
{
	solution solu;
	float root = pow(b,2)-4*a*c;    //计算根
	solu.success = true;
	if(root>=0 || a!=0) //二次函数不存在
	{
		if(root == 0)   //二次函数只有一个解
		{
			solu.DeltaX = -b/2*a;   //返回解
			solu.success = true;
			goto end;
		}
		float x1 = (-b+sqrt(root))/(2*a);   //通过求根公式计算方程的解
		float x2 = (-b-sqrt(root))/(2*a);   

		// cout << "a:" << a << "  b:" << b << "  c:" << c << "  root:" << root << "  Dx1:" << x1 << "  Dx2:" << x2 << endl;

		if(x1>x2)
			solu.DeltaX = x2;          //由于安装方式，取最靠近原点的值作为增量值。
		else
			solu.DeltaX = x1;

	}
	else 
		solu.success = false;
	end:return solu;
}

```

# 算法编译与使用

### 输入坐标

```cpp
...(约200行左右)
#define Ax 218
#define Ay 32
...
```

### 根据你自己的机器改L1、L2、H参数

```cpp
...(约160行左右)
const float L1 = 110.0;
const float L2 = 110.0;
const float H = 110.0;
...
```

### 编译代码
```bash
g++ plot.c -lgdi32 -o plot.exe
./plot.exe
``` 

# 算法效果

按照上面的方法，输入A(218,32)并执行：  

![效果图1](pic/2.png)  

按照上面的方法，输入A(218,97)并执行：  

![效果图2](pic/3.png)  

![效果图2](pic/4.png)  

![效果图2](pic/5.png)  
 
# 结语
至此，我们实现了输入一个坐标，就能够自动计算出相对于的H、L的坐标值，通过加入直线插补、圆弧插补算法，然后通过编码器和电机去执行就可以实现只用俩个电机画图的功能。
# 算法纯原创！请点个赞亲们！