目录
- 普冉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 Errorrn"); } else { DEBUG_PRINT_STRING("CRC OKrn"); // 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 & 0end{pmatrix} to begin{pmatrix}1 & -1 & -1 & 1end{pmatrix} ]
- 左平移, 摇杆输入 (X = -1) 直流电机输出 ((M1, M2, M3, M4) = (-1, 1, 1, -1)) 映射关系为
[begin{pmatrix}-1 & 0 & 0end{pmatrix} to begin{pmatrix}-1 & 1 & 1 & -1end{pmatrix} ]
- 前平移, 摇杆输入 (Y = 1) 直流电机输出 ((M1, M2, M3, M4) = (1, 1, 1, 1)) 映射关系为
[begin{pmatrix}0 & 1 & 0end{pmatrix} to begin{pmatrix}1 & 1 & 1 & 1end{pmatrix} ]
- 后平移, 摇杆输入 (Y = -1) 直流电机输出 ((M1, M2, M3, M4) = (-1, -1, -1, -1)) 映射关系为
[begin{pmatrix}0 & -1 & 0end{pmatrix} to begin{pmatrix}-1 & -1 & -1 & -1end{pmatrix} ]
- 顺时针, 摇杆输入 (Z = 1) 直流电机输出 ((M1, M2, M3, M4) = (1, 1, -1, -1)) 映射关系为
[begin{pmatrix}0 & 0 & 1end{pmatrix} to begin{pmatrix}1 & 1 & -1 & -1end{pmatrix} ]
- 逆时针, 摇杆输入 (Z = -1) 直流电机输出 ((M1, M2, M3, M4) = (-1, -1, 1, 1)) 映射关系为
[begin{pmatrix}0 & 0 & -1end{pmatrix} to begin{pmatrix}-1 & -1 & 1 & 1end{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灯, 或者修改通信协议, 将驱动部分做成可配置的.
![洛谷 P11345 [KTSC 2023 R2] 基地简化 题解](http://www.itfaba.com/wp-content/themes/kemi/timthumb.php?src=http://www.itfaba.com/wp-content/themes/kemi/img/random/2.jpg&w=218&h=124&zc=1)
