普冉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)
{
// 计算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);
}
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;
}
使用扩展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\) , 取值范围为
\[X \in [-1, 1],
Y \in [-1, 1],
Z \in [-1, 1]\]
对应的输出为4个直流电机, 因为要控制正反向, 所以每个电机需要2个PWM通道, 一共8个PWM通道, 每一对PWM通道同时只会有一个输出PWM(模拟功率强度), 另一个保持低电平(占空比为0), 转换关系只需要输出4个, 令输出为 \(M1, M2, M3, M4\) , 输入与输出的对应关系可以用行列式表示
\[\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\) 直流电机输出 \((M1, M2, M3, M4) = (1, -1, -1, 1)\) 映射关系为
\[\begin{pmatrix}1 & 0 & 0\end{pmatrix} \to \begin{pmatrix}1 & -1 & -1 & 1\end{pmatrix}
\]
- 左平移, 摇杆输入 \(X = -1\) 直流电机输出 \((M1, M2, M3, M4) = (-1, 1, 1, -1)\) 映射关系为
\[\begin{pmatrix}-1 & 0 & 0\end{pmatrix} \to \begin{pmatrix}-1 & 1 & 1 & -1\end{pmatrix}
\]
- 前平移, 摇杆输入 \(Y = 1\) 直流电机输出 \((M1, M2, M3, M4) = (1, 1, 1, 1)\) 映射关系为
\[\begin{pmatrix}0 & 1 & 0\end{pmatrix} \to \begin{pmatrix}1 & 1 & 1 & 1\end{pmatrix}
\]
- 后平移, 摇杆输入 \(Y = -1\) 直流电机输出 \((M1, M2, M3, M4) = (-1, -1, -1, -1)\) 映射关系为
\[\begin{pmatrix}0 & -1 & 0\end{pmatrix} \to \begin{pmatrix}-1 & -1 & -1 & -1\end{pmatrix}
\]
- 顺时针, 摇杆输入 \(Z = 1\) 直流电机输出 \((M1, M2, M3, M4) = (1, 1, -1, -1)\) 映射关系为
\[\begin{pmatrix}0 & 0 & 1\end{pmatrix} \to \begin{pmatrix}1 & 1 & -1 & -1\end{pmatrix}
\]
- 逆时针, 摇杆输入 \(Z = -1\) 直流电机输出 \((M1, M2, M3, M4) = (-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灯, 或者修改通信协议, 将驱动部分做成可配置的.
