# 7.速度测量-学会使用编码器

你好，我是小鱼。上节做完小车，遥控时小车前进时你应该会发现，小车很难走一条直线，但明明我们给到两个电机的PWM占空比都是相同的，原因在于每一个电机的硬件参数并不能完全的保证一致，所以当我们采用开环控制时，即使我们给到每个电机相同的电压，也不能让两个电机保持相同的转速。

要解决这个问题我们就要把开环控制改成闭环控制，我们要实现的是速度闭环，所以第一步我们要实现的是对电机速度的测量。

## 一、编码器测速度理论

第一节中介绍过，我们采用的是AB磁编码器，编码器直接连接到了我们的单片机IO上，当电机转动时，IO上的电平高低就会产生变化，我们称这种电平从低到高再到低的过程称作一个脉冲。

## 1.1 轮速测量

因为有减速机的存在，当减速器的输出轴（轮胎）转动了一圈，我们会检测到多个脉冲。所以要想通过编码器得出轮子的速度，我们需要知道检测到一个脉冲时，轮子行走多远距离。

我们FishBot上的电机轮子直径为`65mm`，当轮子转一圈时产生N个脉冲，那么一个脉冲轮子前进的距离D可以这样计算，单位是mm。
$$
D = 65*PI/N
$$
下面我们将通过实际的测试确定D的值，已知D的情况下，我们测得，某一段时间$\Delta T$(ms)内测得脉冲数为$P_T$，则此时电机的转速为$V_T$(m/s)
$$
V_T = ((P_T*D)/1000)/(\Delta T/1000) \\
=(P_T*D)/\Delta T
$$


## 1.2 方向测量

你可能会好奇，为什么我们的电机后面有两个霍尔传感器，用一个不就可以对电机进行测速了吗？原因是使用两个会更精准，同时可以测量方向。

我们把磁铁看作小汽车，AB两个传感器是一条路上前后两个摄像头，如果汽车是正着行驶的，你会发现总是A摄像头先看到汽车，然后再是B，但如果反过来行驶，则是B摄像头先看到设备。

```
			[A]  		[B]
-------------------------------------------------------------
	[汽车-->]								[<--汽车]
-------------------------------------------------------------
```

为了更加直观小鱼也分别用逻辑分析仪测量了两段轮子正转和反转时，AB编码器上电平的变化。

![image-20230301234207461](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230301234207461.png)

放大正转时，当A（通道0）电平为高电平后（A摄像头先看到了汽车），过了一段时间B（通道1）才变为高电平（B摄像头看到了汽车）。

![image-20230301234246403](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230301234246403.png)

放大反转部分，当A（通道0）电平为高电平后（A摄像头看到了汽车），在A之前B（通道1）已经为高电平了（B摄像头先看到了汽车）。

![image-20230301234437633](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230301234437633.png)

所以在代码中我们可以检测到当A通道从低电平变成高电平时，B通道的电平值，如果为低则表示正转，为高则表示反转。

有了理论基础，我们尝试编码验证。

## 二、新建工程并导入开源库

新建`example25_encoder`

![image-20230301220741662](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230301220741662.png)

添加依赖

```ini
[env:featheresp32]  ; 这是一个环境配置标签，指定了代码将运行的硬件平台和框架
platform = espressif32  ; 指定了使用的平台为Espressif 32
board = featheresp32  ; 指定使用的硬件板为Feather ESP32
framework = arduino  ; 指定使用的框架为Arduino
lib_deps =  ; 列出所有依赖库的URL，这些库将被下载和安装
	https://github.com/fishros/Esp32PcntEncoder.git  ; ESP32 编码器驱动库
```

这里我们使用的是`Esp32PcntEncoder`开源库，这个库调用了`ESP32`的脉冲计算外设进行编码器脉冲的计算，使用非常简单。

## 三、代码实现

编写代码

```cpp
#include <Arduino.h>
#include <Esp32PcntEncoder.h>

Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器

void setup()
{
  // 1.初始化串口
  Serial.begin(115200); // 初始化串口通信，设置通信速率为115200

  // 2.设置编码器
  encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接
}

void loop()
{
  delay(10); // 等待10毫秒

  // 读取并打印两个编码器的计数器数值
  Serial.printf("tick1=%d,tick2=%d\n", encoders[0].getTicks(), encoders[1].getTicks());
}
```

上面这段代码使用了`ESP32PcntEncoder`库来读取两个旋转编码器的计数器数值。其中，函数`setup()`用于初始化串口和编码器；函数`loop()`用于读取并打印两个编码器的计数器数值。以下是代码的详细解释：

1. 首先包含了两个头文件`Arduino.h`和`Esp32PcntEncoder.h`，用于编写`Arduino`程序和使用`ESP32PcntEncoder`库。
2. 在全局变量中创建了一个长度为2的`Esp32PcntEncoder`数组，用于存储两个编码器。
3. 函数setup()用于初始化串口和编码器。在本代码中，首先通过`Serial.begin()`函数初始化串口，设置通信速率为`115200`。然后通过`encoders[0].init()`和`encoders[1].init()`函数分别初始化了两个编码器。其中，函数init()需要传入三个参数，分别是编码器的`ID`、引脚A的`GPIO`编号和引脚B的`GPIO`编号。在本代码中，第一个编码器的`ID`为`0`，引脚A连接的`GPIO`为`32`，引脚B连接的`GPIO`为`33`；第二个编码器的`ID`为`1`，引脚A连接的`GPIO`为`26`，引脚B连接的`GPIO`为`25`。
4. 函数loop()用于读取并打印两个编码器的计数器数值。在本代码中，首先通过delay()函数等待10毫秒。然后通过`encoders[0].getTicks()`和`encoders[1].getTicks()`函数分别读取了两个编码器的计数器数值。最后通过`Serial.printf()`函数将这两个数值打印。

## 四、下载测试

将代码下载进入开发板，打开串口监视器，查看输出。

![image-20230302013218396](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230302013218396.png)

为了计算一个脉冲轮子前进的距离，我们可以通过手动将轮子旋转10圈，然后利用前面的公式进行计算。

这里小鱼将轮子转动10圈后得到脉冲数为`19419`

![image-20230302013559003](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230302013559003.png)

根据公式可以算出，一个脉冲轮子前进的距离为
$$
D = 65*PI/(19419/10)\\
=0.1051566
$$
接着我们可以利用公式计算速度。

## 五、计算速度

编写代码

```cpp
#include <Arduino.h>
#include <Esp32PcntEncoder.h>

Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器
int64_t last_ticks[2]; // 记录上一次读取的计数器数值
int32_t pt[2]; // 记录两次读取之间的计数器差值
int64_t last_update_time; // 记录上一次更新时间
float speeds[2]; // 记录两个电机的速度

void setup()
{
  // 1.初始化串口
  Serial.begin(115200); // 初始化串口通信，设置通信速率为115200

  // 2.设置编码器
  encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接

  // 3.让电机1以最大速度转起来
  pinMode(23, OUTPUT);
  digitalWrite(23, HIGH);
}

void loop()
{
  delay(10); // 等待10毫秒

  // 4.计算两个电机的速度
  uint64_t dt = millis() - last_update_time; // 计算两次读取之间的时间差
  pt[0] = encoders[0].getTicks() - last_ticks[0]; // 计算第一个编码器两次读取之间的计数器差值
  pt[1] = encoders[1].getTicks() - last_ticks[1]; // 计算第二个编码器两次读取之间的计数器差值

  speeds[0] = float(pt[0] * 0.1051566) / dt; // 计算第一个电机的速度
  speeds[1] = float(pt[1] * 0.1051566) / dt; // 计算第二个电机的速度

  // 5.更新记录
  last_update_time = millis(); // 更新上一次更新时间
  last_ticks[0] = encoders[0].getTicks(); // 更新第一个编码器的计数器数值
  last_ticks[1] = encoders[1].getTicks(); // 更新第二个编码器的计数器数值

  // 6.打印信息
  Serial.printf("tick1=%d,tick2=%d\n", encoders[0].getTicks(), encoders[1].getTicks()); // 打印两个编码器的计数器数值
  Serial.printf("spped1=%f,spped2=%f\n", speeds[0], speeds[1]); // 打印两个电机的速度
}
```

在`loop()`函数中，首先等待10毫秒，然后读取两个编码器的计数器数值，并且计算出它们的旋转速度。

其中，`last_ticks`数组用于存储上一次读取的计数器数值，`pt`数组存储两次读取之间的计数器增量，`last_update_time`变量存储上一次读取的时间，`speeds`数组存储两个编码器的旋转速度。

最后，通过串口打印出两个编码器的计数器数值和旋转速度。此外，还让GPIO 23输出高电平，使电机1以最大速度转动。

下载代码，观察串口打印

![image-20230302021823741](7.%E9%80%9F%E5%BA%A6%E6%B5%8B%E9%87%8F-%E5%AD%A6%E4%BC%9A%E4%BD%BF%E7%94%A8%E7%BC%96%E7%A0%81%E5%99%A8/imgs/image-20230302021823741.png)

速度为-0.389079m/s

## 六、总结

本节我们完成了对电机速度的测量，下一节我们尝试利用PID动态的控制电机保持在某个转速。