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

Milton2023-11-22 8:22

目录

基于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循环的代码为

c 复制代码 
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() 可以更精确地控制延迟.

c 复制代码 
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信号

c 复制代码 
/**
 * 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} \]

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

c 复制代码 
/**
 * 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仓库里的代码是调整过的代码, 如果你使用仓库代码发现一侧轮子转向不符合预期, 按上面的矩阵修改一下就可以.

项目资源

最后

设计已经经过实物验证, 遥控功能符合预期, 机械部分凑合能跑, 但是轮子太吵, 速度不够快(48:1电机), 对地面平整度要求比较高. 实测无线控制距离大约20米. 在当前的基础上还可以做一些优化, 例如增加对XN297LBW的支持, 现在的收发地址, 收发速率都是固定的, 可以利用现在的按键做成可调节或自动对码. 测试不同天线是否能提升控制距离. 驱动部分目前只使用了扩展IO, 原生IO没用上, 可以增加这部分的PWM设置, 可以增加一两个按钮和对应的LED灯, 或者修改通信协议, 将驱动部分做成可配置的.

相关推荐
Milton1 年前
普冉PY32系列(十四) 从XL2400迁移到XL2400P
py32f002a·xl2400·xl2400p
Milton1 年前
普冉PY32系列(十) 基于PY32F002A的6+1通道遥控小车I - 综述篇
py32f002a·xl2400·74hc595·74hc165·yx-1818
Milton1 年前
普冉PY32系列(九) GPIO模拟和硬件SPI方式驱动无线收发芯片XL2400
py32·py32f002a·xn297lbw·xl2400
IOsetting1 年前
普冉PY32系列(八) GPIO模拟和硬件SPI方式驱动无线收发芯片XN297LBW
py32f002a·xn297l·xn297lbw·py32f003
Milton1 年前
普冉PY32系列(八) GPIO模拟和硬件SPI方式驱动无线收发芯片XN297LBW
py32·py32f002a·xn297l·xn297lbw
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO07【目标跟踪】相机运动补偿08Unity中PICO实现 隔空取物 和 接触抓取物体09RAG 实践- Ollama+RagFlow 部署本地知识库10量化方法怎么选？如何评估量化后的大模型LLM？