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

普冉PY32系列(一) PY32F0系列32位Cortex M0+ MCU简介
普冉PY32系列(二) Ubuntu GCC Toolchain和VSCode开发环境
普冉PY32系列(三) PY32F002A资源实测 - 这个型号不简单
普冉PY32系列(四) PY32F002A/003/030的时钟设置
普冉PY32系列(五) 使用JLink RTT代替串口输出日志
普冉PY32系列(六) 通过I2C接口驱动PCF8574扩展的1602LCD
普冉PY32系列(七) SOP8,SOP10,SOP16封装的PY32F002A/PY32F003管脚复用
普冉PY32系列(八) GPIO模拟和硬件SPI方式驱动无线收发芯片XN297LBW
普冉PY32系列(九) GPIO模拟和硬件SPI方式驱动无线收发芯片XL2400
普冉PY32系列(十) 基于PY32F002A的6+1通道遥控小车I - 综述篇
普冉PY32系列(十一) 基于PY32F002A的6+1通道遥控小车II - 控制篇
普冉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){// 计算CRCcrc = 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_statememcpy(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_channelDRV_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);
}

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 bytewhile(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;
}

使用扩展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);}
}

运动控制

这里不介绍麦克纳姆轮的运动机制, 只说明如何将输入转换为电机的输出. 假定电机的位置如下, 俯视图, 斜杆表示麦克纳姆轮的抓地轮轴方向, 从上面看到的轮轴是米字形, 着地部分为菱形.

      Forward/      \ / M1  M3 \
Left             Right\ M2  M4 /\      / Backward

在控制小车运动时用到了三个通道, 分别控制小车的 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{pmatrix} X & Y & Z \end{pmatrix} \times A_{ij} = \begin{pmatrix} X & Y & Z \end{pmatrix} \times \begin{pmatrix} 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{pmatrix} =\begin{pmatrix} M1 & M2 & M3 & M4 \end{pmatrix}$

根据已知的对应关系, 有

右平移, 摇杆输入 $X = 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (1, - 1, - 1, 1)$ 映射关系为

$\begin{pmatrix}1 & 0 & 0\end{pmatrix} \to \begin{pmatrix}1 & -1 & -1 & 1\end{pmatrix}$

左平移, 摇杆输入 $X = - 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (- 1, 1, 1, - 1)$ 映射关系为

$\begin{pmatrix}-1 & 0 & 0\end{pmatrix} \to \begin{pmatrix}-1 & 1 & 1 & -1\end{pmatrix}$

前平移, 摇杆输入 $Y = 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (1, 1, 1, 1)$ 映射关系为

$\begin{pmatrix}0 & 1 & 0\end{pmatrix} \to \begin{pmatrix}1 & 1 & 1 & 1\end{pmatrix}$

后平移, 摇杆输入 $Y = - 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (- 1, - 1, - 1, - 1)$ 映射关系为

$\begin{pmatrix}0 & -1 & 0\end{pmatrix} \to \begin{pmatrix}-1 & -1 & -1 & -1\end{pmatrix}$

顺时针, 摇杆输入 $Z = 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (1, 1, - 1, - 1)$ 映射关系为

$\begin{pmatrix}0 & 0 & 1\end{pmatrix} \to \begin{pmatrix}1 & 1 & -1 & -1\end{pmatrix}$

逆时针, 摇杆输入 $Z = - 1$ 直流电机输出 $(M 1, M 2, M 3, M 4) = (- 1, - 1, 1, 1)$ 映射关系为

$\begin{pmatrix}0 & 0 & -1\end{pmatrix} \to \begin{pmatrix}-1 & -1 & 1 & 1\end{pmatrix}$

汇总后就是

$\begin{pmatrix} 1 & 0 & 0 \\ -1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & -1 \end{pmatrix} \times \begin{pmatrix} 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{pmatrix} =\begin{pmatrix} 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{pmatrix}$

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

$A_{ij} = \begin{pmatrix} 1 & -1 & -1 & 1 \\ 1 & 1 & 1 & 1 \\ 1 & 1 & -1 & -1 \end{pmatrix}$

对应到代码中对输入的转换如下

/*** 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]);}}}

遇到的问题

遇到的唯一一个问题, 是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灯, 或者修改通信协议, 将驱动部分做成可配置的.