STM32机器人控制开发教程No.3 使用遥控控制电机/舵机(基于HAL库)

01 前言

本教程使用的机器人控制板拥有4个带编码器的电机接口,4个舵机接口,串口通信接口、SWD下载调试接口、航模遥控接口、USB5V输出接口以及方便与树莓派直接连接的40PIN接口等,板载资源丰富,方便调试!可以控制两轮、四轮差速及阿克曼转向机器人/小车。

控制板上的遥控接口:

02 机器人小车电机驱动开发——使用PPM信号遥控

在上一篇文章《STM32机器人控制开发教程No.2 霍尔编码器电机测速以及增量式PID控制(基于HAL库)》里介绍了如何使用霍尔编码器电机进行速度测量以及使用增量式PID算法对速度进行控制。在本篇文章中继续为你介绍控制板上遥控接口的使用方法!一般的遥控器接收机拥有SBUS模式、IBUS模式、PPM模式以及普通的PWM输出模式。使用PPM模式的理由是可以利用其能够产生多路PWM信号的特点,控制不同的外设,创造更多有意思的玩法!

PPM信号的介绍

  • PPM信号可以简单理解为由多个PWM信号综合成为一帧信号的形式,即多个PWM信号通过组合成PPM信号串行输出。

  • 一般一帧的PPM信号里最多包含10个PWM信号,但实际应用中由于同步帧的存在,所以一帧的PPM信号里最多有9个PWM信号数据。

  • 每帧PPM信号的数据总长度固定不变,周期为20ms。

  • 每一帧PPM信号中的PWM信号周期为0~2ms,而其中低电平脉宽固定为0.5ms,高电平的脉宽为0.5ms ~ 1.5ms。

03 使用STM32读取PPM的数据

根据PPM信号的格式,可以通过两种方法读取到其中每个PWM的信息:

  • 使用STM32的外部中断,当触发外部中断定时器进行计数,读取定时器的计数值便可以获得每个PWM的脉宽

  • 使用STM32的定时器,利用定时器的输入捕获功能,测量每个PWM的脉冲宽度

其实两种读取PPM数据的方法本质上相同,都是通过统计下降沿或上升沿的方法统计脉冲,并利用定时器的计数功能获得高低电平的脉宽。

注意:若使用定时器的输入捕获功能读取PPM信号数据,则需要留意其他使用同一定时器外设将不能正常工作,例如在控制板上遥控接收器接口使用了定时器3的输入捕获功能,则同样使用到定时器3作为编码器模式的电机将不能正常读取编码器的脉冲值,需选择使用其他电机接口或改变PPM的读取方式。

以下展示的配置为使用外部中断的方式进行读取PPM数据,使用定时器输入捕获方式的代码可参考例程文件中的注释内容

STM32CubeMX中的配置将遥控对应的STM32引脚设置为外部中断模式,并选择定时器7作为计数,同时使能定时器的中断

读取PPM具体实现代码:

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)  //外部中断的回调函数
{
  if(GPIO_Pin == GPIO_PIN_0)  //遥控接口->PB.0
  {
    PPM_Time = __HAL_TIM_GET_COUNTER(&htim7); //获得定时器7的计数值
    __HAL_TIM_GET_COUNTER(&htim7) = 0;   //计数值清零
    if(PPM_Okay == 1)
    {
            PPM_Databuf[PPM_Sample_Cnt] = PPM_Time;
      PPM_Sample_Cnt++;
      if(PPM_Sample_Cnt > 8) PPM_Okay = 0;
    }
    if(PPM_Time>=3000)
    {
      PPM_Okay = 1;
      PPM_Sample_Cnt = 0;
    }
  }
}

在主函数的循环中通过串口调试助手可以查看PPM_DataBuf[]数组中存储的各个通道的值

while(1)
{
        printf("PPM_Data[0] is %d\r\n",PPM_Databuf[0]);
    printf("PPM_Data[1] is %d\r\n",PPM_Databuf[1]);
    printf("PPM_Data[2] is %d\r\n",PPM_Databuf[2]);
    printf("PPM_Data[3] is %d\r\n",PPM_Databuf[3]);
       printf("PPM_Data[4] is %d\r\n",PPM_Databuf[4]);
    printf("PPM_Data[5] is %d\r\n",PPM_Databuf[5]);
}

将程序通过SWD下载到控制板,使用乐迪T8S遥控器配套的接收机,连接到板上的遥控接口,注意使用接收机时使用的是接收机上的PPM输出接口,然后通过乐迪遥控的配置方式,将接收器配置为PPM模式,然后打开遥控器开关进行对码,成功连接后打开串口调试助手,就可以看到各个通道的值啦!

可以通过操控摇杆并观察串口调试助手中的各帧数据的变化,与遥控上的控制通道相对应,那样就可以通过摇杆来控制舵机电机等外设了。

04 通过遥控控制舵机
 

得到了PPM的信号数据,如何利用其值来控制舵机进行转动呢?

首先要将得到的PPM各通道值与舵机PWM的比较值进行对应,以设置舵机PWM的占空比来转动不同的角度。可以根据乐迪遥控器每个摇杆的舵量大小来匹配相对应的角度,建议将摇杆位于中间位置时对应舵机的90°,通过上推或下拉摇杆的方式增加或减少舵量,以控制舵机进行转动。

需要注意,舵机的转动角度范围为0°~180°,因此在进行匹配时不能超出舵机的转动范围。当前作者将每个通道的舵量设置为1000~2000,即当舵量大小为1000时对应舵机角度为0°,当舵量为2000时对应舵机角度为180°,同时将舵量为1500对应为舵机的90°。注意,在实际使用时应当设置一定的遥控“死区”,即当舵量位于“死区”范围内时舵机仍保持不动,避免因为过于灵敏造成舵机频繁动作而引起抖动的情况。

根据舵量与舵机角度的对应大小,可以得到以下函数关系:

y=0.18x-180

然后通过调用舵机角度设置的函数,对舵机进行控制,实现代码如下:

void ServoControl()
{
  if(PPM_Databuf[0]<1600&&PPM_Databuf[0]>1400){PPM_Databuf[0] = 1500;} //设置死区
  if(PPM_Databuf[1]<1600&&PPM_Databuf[1]>1400){PPM_Databuf[1] = 1500;}
  if(PPM_Databuf[2]<1600&&PPM_Databuf[2]>1400){PPM_Databuf[2] = 1500;}
  if(PPM_Databuf[3]<1600&&PPM_Databuf[3]>1400){PPM_Databuf[3] = 1500;}
  
    Set_Servo_angle(&htim2,TIM_CHANNEL_3,myabs(0.18*PPM_Databuf[0]-180)); 
  Set_Servo_angle(&htim2,TIM_CHANNEL_4,myabs(0.18*PPM_Databuf[1]-180));
  Set_Servo_angle(&htim4,TIM_CHANNEL_3,myabs(0.18*PPM_Databuf[2]-180));
  Set_Servo_angle(&htim4,TIM_CHANNEL_4,myabs(0.18*PPM_Databuf[3]-180));
}

将程序下载到机器人控制板上,按照上文中的方法连接好接收机,并接入四路舵机。然后打开遥控器,就可以看到使用遥控控制四路舵机的效果啦!

05 通过遥控控制电机
 

使用遥控对电机进行控制,本质上与控制舵机原理相同。都是通过接收遥控的PPM信号数据设置电机PWM的占空比,以达到控制速度的目的。

注意在电机的遥控控制中同样应当设置一定的“死区”,避免出现油门归零时电机仍在转动的情况。

控制两个电机实现前进后退的实现代码如下(两轮差速转向功能可参考例程文件):


void MOtorRemoteControl()
{
  if(PPM_Databuf[1]<1600&&PPM_Databuf[1]>1400){PPM_Databuf[1] = 1500;}
  MotorControl(PPM_Databuf[1]-1500,PPM_Databuf[1]-1500);//电机最大速度为50%,左右轮同速前进
}

 遥控控制电机演示效果:

关于遥控控制电机和舵机就介绍到这里啦,如果你对这块控制板有更多创意和新奇的玩法也可以与我们分享噢!当然,你也可以提出你想要的功能以及优化建议。这么炫酷的控制板,赶紧买一块带回家,发挥你天马行空般的想法,控制你想要控制设备,或者可以购买我们的小车套件配合使用,扩展更多玩法,开启你的机器人开发之路吧! 

欢迎加入我们的交流群,该群面向热爱机器人研发的朋友们,方便大家一起学习、分享、交流智能机器人创造,结识更多志同道合的小伙伴。更有不定期的社区专属福利哦!关注公众号(COONEO)即可获取入群方式。

创作不易,如果喜欢这篇内容,请您也转发给您的朋友,一起分享和交流创造的乐趣,也激励我们为大家创作更多的机器人研发攻略,让我们一起learning by doing!

猜你喜欢

转载自blog.csdn.net/COONEO/article/details/125911761