普冉PY32系列(十二) 基于PY32F002A的6+1通道遥控小车III - 驱动篇 - 码农知识堂

普冉PY32系列(十二) 基于PY32F002A的6+1通道遥控小车III - 驱动篇
目录
基于PY32F002A的6+1通道遥控小车III - 驱动篇

驱动端, 即电机驱动控制板, 包含信号接收, IO扩展和运动控制计算等.

驱动控制板实物

 PCB板

 贴片中

 成品

 电机驱动模块

 软件设计

 整体结构

驱动部分的工作流程, 就是通过无线模块接收数据, CRC校验, 再将接收到的数据转换为电机的驱动参数, 调整电机运动状态, 不断循环.

因为XL2400和XN297LBW都是通过轮询的方式接收数据, 可以直接串接在main循环中. 最外层的代码的流程是
- uint8_t xbuf[XL2400_PL_WIDTH_MAX + 1]存储接收到的数据
- uint8_t pad_state[7]存储6+1通道的数据
- uint8_t servo_pwm_channel[8]存储各PWM通道的占空比
- 在main循环中
  - 读取数据接收状态, 如果无数据则等待10毫秒再次读取
  - 如果读取到数据
    CRC校验, 与第八个字节做比较, 不一致则丢弃, 一致则赋值到 pad_state
    根据 pad_state 的值, 计算4个电机的运动方向和速度
    根据电机的运动方向和速度, 计算电机对应的两路PWM占空比
    调用扩展IO接口更新各路PWM占空比
main循环的代码为
```
while(1)
{
  j++;
  if (XL2400_Rx() & XL2400_FLAG_RX_DR)
  {
    // 计算CRC
    crc = 0;
    for (i = 0; i < XL2400_PLOAD_WIDTH - 1; i++)
    {
      crc += *(xbuf + i);
    }
    if (crc != *(xbuf + XL2400_PLOAD_WIDTH - 1))
    {
      DEBUG_PRINT_STRING("CRC Error\r\n");
    }
    else
    {
      DEBUG_PRINT_STRING("CRC OK\r\n");
      // CRC校验正确时, 更新数值到 pad_state
      memcpy(pad_state, xbuf, 7);
    }

    // Key1 ~ Key8 对应 CH1 ~ CH8, 占空比0xFF和0x00分别代表按键未按下和按下
    for (i = 8; i--;)
    {
      if (*(pad_state + 6) & (1 << i))
      {
        DRV_LSPWM_SetDuty(i, 0xFF, 0xFF);
      }
      else
      {
        DRV_LSPWM_SetDuty(i, 0, 0xFF);
      }
    }
    // 将模拟信号数据 (X:A1, Y:A0, Z:A2) 转换为 PWM, 写入 servo_pwm_channel
    DRV_SERVO_AnalogConvert(*(xbuf + 1), *(xbuf), *(xbuf + 2), (uint8_t *)DIRECTION, servo_pwm_channel);
    // 更新低速 PWM 通道
    for (i = 0; i < 8; i++)
    {
      DRV_LSPWM_SetDuty(8 + i, *(servo_pwm_channel + i), 0xFF);
    }
    j = 0;
  }
  LL_mDelay(10);
}
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
```
74HC595扩展IO

使用串行数据控制 74HC595 的并行输出, 支持多个 74HC595. 通过这个方式, 可以仅用3个PIN脚扩展出8个, 16个甚至24个IO.

代码中增加了多处 NOP 以适配运行在 48MHz 时钟下的 PY32F002A. 用 NOP 比用 LL_mDelay() 可以更精确地控制延迟.
```
void HC595_WriteBytes(uint8_t *data, uint8_t size)
{
    uint8_t i;

    HC595_STCP_LOW;
    /* Add nops to accommodate 74hc595 speed */
    HC595_NOP;
    while(size--)
    {
        i = 8;
        // iterate through the bits in each byte
        while(i--)
        {
            HC595_SRCLK_LOW;
            HC595_NOP;
            if (*(data + size) & (1 << i))
            {
                HC595_DS_HIGH;
            }
            else
            {
                HC595_DS_LOW;
            }
            HC595_NOP;HC595_NOP;
            HC595_SRCLK_HIGH;
            // 拉高 SRCLK 后, 要等待足够长的时间使其生效
            HC595_NOP;HC595_NOP;HC595_NOP;HC595_NOP;HC595_NOP;
        }
    }
    HC595_STCP_HIGH;
    HC595_NOP;
}
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
```
使用扩展IO输出PWM

SG90 MG90 MG995 MG996 这些常见舵机的PWM频率是50Hz, 而普通的直流有刷电机, 调速PWM频率建议在50Hz以内. 如果调节分辨率设为90, 那么IO的翻转速度只需要 50 * 90 * 2 = 9KHz, 扩展IO速度比原生IO慢一些, 但是用于输出驱动舵机和电机的PWM已经足够了.

下面的代码用于驱动 74HC595 输出PWM信号
```
/**
 * hc595_state 是根据 pwm_duty 计算得到的, 本次传输给 74HC595的开关量
 * pwm_duty 对应每个通道当前的占空比
 * pwm_duty_pre 对应每个通道下一个PWM周期的占空比, 在每个PWM周期开始时将 pwm_duty_pre 的值赋给 pwm_duty, 这种机制可以保证PWM在一个周期内是固定的, 避免占空比发生突变
 * pwm_counter 是PWM时钟计数器, 当前所有的模拟PWM通道共用一个计数器, 在计数器到达PWM_PERIOD后会从0重新计数
 */
uint8_t hc595_state[HC595_SIZE], pwm_duty[PWM_CH_SIZE], pwm_duty_pre[PWM_CH_SIZE], pwm_counter = 0;

/**
 * 设置占空比
 * channel: 0 ~ PWM_CH_SIZE - 1
 * numerator/denominator: duty
*/
void DRV_LSPWM_SetDuty(uint8_t channel, uint8_t numerator, uint8_t denominator)
{
  pwm_duty_pre[channel] = (uint8_t)(((uint16_t)numerator * PWM_PERIOD) / denominator);
}

/**
 * 模拟PWM的时钟处理方法
*/
void DRV_LSPWM_Tick(void)
{
  uint8_t i, hc595_idx, mask;
  // 对每一个通道, 将计数器与pwm_duty对比得到当前通道的电平状态
  for (i = 0; i < PWM_CH_SIZE; i++)
  {
    hc595_idx = i / 8;
    mask = 1 << (i % 8);

    if (pwm_duty[i] <= pwm_counter)
    {
      hc595_state[hc595_idx] = hc595_state[hc595_idx] & (~mask);
    }
    else
    {
      hc595_state[hc595_idx] = hc595_state[hc595_idx] | mask;
    }
  }
  // 将电平状态通过 74hc595 输出
  HC595_WriteBytes(hc595_state, HC595_SIZE);

  pwm_counter++;
  // 当计数器到预设周期时, 重置计数器
  if (pwm_counter == PWM_PERIOD)
  {
    pwm_counter = 0;
    // 从 pwm_duty_pre 载入新的占空比设置
    memcpy(pwm_duty, pwm_duty_pre, PWM_CH_SIZE);
  }
}
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
```
运动控制

这里不介绍麦克纳姆轮的运动机制, 只说明如何将输入转换为电机的输出. 假定电机的位置如下, 俯视图, 斜杆表示麦克纳姆轮的抓地轮轴方向, 从上面看到的轮轴是米字形, 着地部分为菱形.
```
      Forward
      /      \ 
     / M1  M3 \
Left             Right
     \ M2  M4 /
      \      / 
      Backward
1
2
3
4
5
6
7
```
在控制小车运动时用到了三个通道, 分别控制小车的 1)左右平移, 2)前后平移, 3)原地顺时针和逆时针旋转. 定义三个输入通道的变量为 $X, Y, Z$ , 取值范围为

$\in [-1, 1], Y \in [-1, 1], Z \in [-1, 1]$

对应的输出为4个直流电机, 因为要控制正反向, 所以每个电机需要2个PWM通道, 一共8个PWM通道, 每一对PWM通道同时只会有一个输出PWM(模拟功率强度), 另一个保持低电平(占空比为0), 转换关系只需要输出4个, 令输出为 $M 1, M 2, M 3, M 4$ , 输入与输出的对应关系可以用行列式表示

$(\begin{matrix} X & Y & Z \end{matrix})$
\times A_{ij} =
$(\begin{matrix} X & Y & Z \end{matrix})$
\times
$(\begin{matrix} a_{x, 1} & a_{x, 2} & a_{x, 3} & a_{x, 4} \\ a_{y, 1} & a_{y, 2} & a_{y, 3} & a_{y, 4} \\ a_{z, 1} & a_{z, 2} & a_{z, 3} & a_{z, 4} \end{matrix})$
=
$(\begin{matrix} M 1 & M 2 & M 3 & M 4 \end{matrix})$
(XYZ)×Aij=(XYZ)× ax,1ay,1az,1ax,2ay,2az,2ax,3ay,3az,3ax,4ay,4az,4 =(M1M2M3M4)

根据已知的对应关系, 有
- 右平移, 摇杆输入 $X = 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (1, - 1, - 1, 1)$ 映射关系为
$(\begin{matrix} 1 & 0 & 0 \end{matrix})$
\to
$(\begin{matrix} 1 & - 1 & - 1 & 1 \end{matrix})$
(100)→(1−1−11)
- 左平移, 摇杆输入 $X = - 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (- 1, 1, 1, - 1)$ 映射关系为
$(\begin{matrix} - 1 & 0 & 0 \end{matrix})$
\to
$(\begin{matrix} - 1 & 1 & 1 & - 1 \end{matrix})$
(−100)→(−111−1)
- 前平移, 摇杆输入 $Y = 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (1, 1, 1, 1)$ 映射关系为
$(\begin{matrix} 0 & 1 & 0 \end{matrix})$
\to
$(\begin{matrix} 1 & 1 & 1 & 1 \end{matrix})$
(010)→(1111)
- 后平移, 摇杆输入 $Y = - 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (- 1, - 1, - 1, - 1)$ 映射关系为
$(\begin{matrix} 0 & - 1 & 0 \end{matrix})$
\to
$(\begin{matrix} - 1 & - 1 & - 1 & - 1 \end{matrix})$
(0−10)→(−1−1−1−1)
- 顺时针, 摇杆输入 $Z = 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (1, 1, - 1, - 1)$ 映射关系为
$(\begin{matrix} 0 & 0 & 1 \end{matrix})$
\to
$(\begin{matrix} 1 & 1 & - 1 & - 1 \end{matrix})$
(001)→(11−1−1)
- 逆时针, 摇杆输入 $Z = - 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (- 1, - 1, 1, 1)$ 映射关系为
$(\begin{matrix} 0 & 0 & - 1 \end{matrix})$
\to
$(\begin{matrix} - 1 & - 1 & 1 & 1 \end{matrix})$
(00−1)→(−1−111)

汇总后就是

$(\begin{matrix} 1 & 0 & 0 \\ - 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & - 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & - 1 \end{matrix})$
\times
$(\begin{matrix} a_{x, 1} & a_{x, 2} & a_{x, 3} & a_{x, 4} \\ a_{y, 1} & a_{y, 2} & a_{y, 3} & a_{y, 4} \\ a_{z, 1} & a_{z, 2} & a_{z, 3} & a_{z, 4} \end{matrix})$
=
$(\begin{matrix} 1 & - 1 & - 1 & 1 \\ - 1 & 1 & 1 & - 1 \\ 1 & 1 & 1 & 1 \\ - 1 & - 1 & - 1 & - 1 \\ 1 & 1 & - 1 & - 1 \\ - 1 & - 1 & 1 & 1 \end{matrix})$
1−10000001−10000001−1 × ax,1ay,1az,1ax,2ay,2az,2ax,3ay,3az,3ax,4ay,4az,4 = 1−11−11−1−111−11−1−111−1−111−11−1−11

解这个式子可以得到 $A_{ij}$ 的值为

$A_{ij} =$
$(\begin{matrix} 1 & - 1 & - 1 & 1 \\ 1 & 1 & 1 & 1 \\ 1 & 1 & - 1 & - 1 \end{matrix})$
Aij= 111−111−11−111−1

对应到代码中对输入的转换如下
```
/**
 * Convert 3-channel analog input to 8-channel PWM duties
 * 
 *           |  1 -1 -1  1 |
 * (X Y Z) * |  1  1  1  1 | 
 *           |  1  1 -1 -1 |
 * 
 * Change the determinant according to your motor direction
*/
void DRV_SERVO_AnalogConvert(uint8_t uvx, uint8_t uvy, uint8_t uvz, uint8_t *pwm_channel)
{
  uint8_t i;
  uint8_t *a, *b;
  // 将输入的 0~255 的无符号数转为有符号数
  int16_t denominator = 0, vx = uvx - 127, vy = uvy - 127, vz = uvz - 127, motor[4];
  // 忽略 (-7, 8) 区间的数值, 忽略电位器中间点阻值偏差
  vx = (vx < 8 && vx > -7)? 0 : vx;
  vy = (vy < 8 && vy > -7)? 0 : vy;
  vz = (vz < 8 && vz > -7)? 0 : vz;
  // 根据上面的行列式, 转换为电机向量
  motor[0] =  vx + vy + vz;
  motor[1] = -vx + vy + vz;
  motor[2] = -vx + vy - vz;
  motor[3] =  vx + vy - vz;

  // 因为计算中电机向量会超过255, 为保持方向正确需要等比例缩小到长度为255的范围内
  // 遍历得到最大的向量长度, 当最大向量长度大于1时, 各个方向可以用这个值等比例压缩
  for (i = 0; i < 4; i++)
  {
    if (motor[i] > denominator) denominator = motor[i];
    else if (-motor[i] > denominator) denominator = -motor[0];
  }

  // 将每个电机向量转换为两个PWM通道占空比
  for (i = 0; i < 4; i++)
  {
    a = pwm_channel + (i * 2);
    b = a + 1;
    *a = 0;
    *b = 0;
    if (motor[i] > 0)
    {
    	// 正向转动
      *a = (denominator > 255)? (uint8_t)(motor[i] * 255 / denominator) : (uint8_t)motor[i];
    }
    else
    {
    	// 反向转动
      *b = (denominator > 255)? (uint8_t)((-motor[i]) * 255 / denominator): (uint8_t)(-motor[i]);
    }

  }

}
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
```
遇到的问题

遇到的唯一一个问题, 是YX-1818的输入输出不一致. 两个YX-1818模块, 其中有一个存在问题, 在AB通道方向一致时, 电机方向正确, 但是在AB通道方向相反时, 电机方向是相反的. 这个问题排查了很久, 开始以为是代码问题, 后来最终排查到YX-1818模块, 将正常的一侧换上, 输出就正常了, 所以基本上能确定是YX-1818的问题.

因为只是方向错误, 在代码里将运动控制矩阵调整一下就行了, GitHub仓库里的代码是调整过的代码, 如果你使用仓库代码发现一侧轮子转向不符合预期, 按上面的矩阵修改一下就可以.

项目资源
- 电路和PCB: https://oshwhub.com/iosetting/py32f002a-remote-control
- 代码: https://github.com/IOsetting/py32f002a-remote-control
- 演示: https://www.bilibili.com/video/BV1Nu4y1w7FE
最后

设计已经经过实物验证, 遥控功能符合预期, 机械部分凑合能跑, 但是轮子太吵, 速度不够快(48:1电机), 对地面平整度要求比较高. 实测无线控制距离大约20米. 在当前的基础上还可以做一些优化, 例如增加对XN297LBW的支持, 现在的收发地址, 收发速率都是固定的, 可以利用现在的按键做成可调节或自动对码. 测试不同天线是否能提升控制距离. 驱动部分目前只使用了扩展IO, 原生IO没用上, 可以增加这部分的PWM设置, 可以增加一两个按钮和对应的LED灯, 或者修改通信协议, 将驱动部分做成可配置的.
相关阅读:
Spring源码系列学习
 常见树种（贵州省）：009楠木、樟木、桂木种类
 解决Redis分布式锁宕机出现不可靠问题-zookeeper分布式锁
 $route和$router的区别详解
 【前端设计模式】之发布订阅模式
 Flink系列之Flink中Broadcast和Counter整理和实战
 全能成熟稳定开源分布式存储Ceph破冰之旅-上
 python用cv2画图（line, rectangle, text等）
【C++笔记】多态的原理、单继承和多继承关系的虚函数表、 override 和 final、抽象类、重载、覆盖(重写)、隐藏(重定义)的对比
 Flutter 08 三棵树（Widgets、Elements和RenderObjects）
原文地址：https://blog.csdn.net/michaelchain/article/details/134544693

普冉PY32系列(十二) 基于PY32F002A的6+1通道遥控小车III - 驱动篇

目录

基于PY32F002A的6+1通道遥控小车III - 驱动篇

驱动控制板实物

PCB板

贴片中

成品

电机驱动模块

软件设计

整体结构

74HC595扩展IO

使用扩展IO输出PWM

运动控制

遇到的问题

项目资源

最后