ROS学习笔记之八:尝试在FreeRTOS中使用ROSSerial

我理解 ROSSerial只是为MCU提供了一个和基于ROS主机(上位机)通讯的方式,而MCU要实现的功能还是要自己编写实现,这部分和以往需求一样,那么采用什么程序框架就是不可回避的问题。

现在由于STM32以极优的性价比提供了可以运行RTOS的硬件平台,使用RTOS已经不是一件奢侈的事情了,如果能在RTOS框架下使用ROSSerial应该是编写嵌入式控制程序的福音,故做此尝试。

为了便于实施,也便于围观者理解,还是基于前面的程序修改。

具体构思如下:

  1、构建以下任务:

     A)定时检测 rosserial 信息,并控制 LED 闪烁

     B)自检任务,执行一次后消除

     C)执行命令任务,等待ROS消息,根据消息执行。

  2、创建队列消息:

     在 rosserial 的 Callback 函数中产生队列消息,将收到的动作命令送给执行命令任务。

     因为 Callback 函数应该是在中断中产生的,故使用 xQueueSendFromISR()

     消息队列设计为 2个单元,每个单元 3 字节 :电机命令 PWM  舵机角度

按此定义消息单元数据类型:

typedefstruct

{

      uint8_t u8MotorCmd;

      uint8_t u8Pwm;

      uint8_t u8ServoVal;

}command_type;

 

三个任务如下:

// 任务 A:定时检测 rosserial 消息,控制 LED  闪烁

static voidtaskA(void* arg)

{

      UNUSED(arg);

  pinMode(LED_BUILTIN,OUTPUT);

  g_bLED_On = true;

  digitalWrite(LED_BUILTIN,HIGH);

  gc_i16DisplayTimeCnt = DISPLAY_TIME;

     

  while (1)

  {

    // Sleep for 10 milliseconds.

    vTaskDelay((10L * configTICK_RATE_HZ) /1000L);

 

    if(g_bSelfTest == false)

    {

         nh.spinOnce();                    // 检测rosserial

         gc_i16DisplayTimeCnt--;                       // LED Display

      if(gc_i16DisplayTimeCnt <= 0)

      {

        gc_i16DisplayTimeCnt = DISPLAY_TIME;

        g_bLED_On = !g_bLED_On;

        digitalWrite(LED_BUILTIN,g_bLED_On);

      }

    }

  }

}

这样编写逻辑似乎更清晰了。

// 任务 B:自检

static voidtaskB(void* arg)

{

      UNUSED(arg);

      uint8_t i,j;

     

      g_bSelfTest = true;

 

      for(i = 0; i< MAX_CMD_NO; i++)

      {

             j = 0;

             myServo.writeMicroseconds(correctServo(ga_u8TestCommand[i*4+ j]));

    j++;

   

    vTaskDelay((1000L* configTICK_RATE_HZ) / 1000L);   // 延时  1000ms, 等待舵机操作完成

   

    YM3.setRunDistance(ga_u8TestCommand[i*4+ j]);

    j++;

    YM3.run(ga_u8TestCommand[i*4 +j],ga_u8TestCommand[i*4 + j+1]);

   

    while(YM3.getMotorStat() ==ga_u8TestCommand[i*4 + j])

    {

         vTaskDelay((10L* configTICK_RATE_HZ) / 1000L);   // 延时  10ms  ,每10ms查询一下电机执行是否完成

    }

      }

  g_bSelfTest = false;

 

  vTaskDelete(NULL);

}

看客可以比较一下原来的自检处理逻辑,是不是这个直观了许多?

// 任务 C:执行命令

static voidtaskC(void* arg)

{

      UNUSED(arg);

 

  while(1)

  {

    if(g_bSelfTest== false)

    {

           xQueueReceive(xQueueCommand,&g_sCommand,portMAX_DELAY);

          

           if(g_sCommand.u8ServoVal!= g_ucServoVal)

           {

                  g_ucServoVal= g_sCommand.u8ServoVal;

                  myServo.writeMicroseconds(correctServo(g_ucServoVal));

                  vTaskDelay((1000L* configTICK_RATE_HZ) / 1000L);   // 延时  1000ms, 等待舵机操作完成

           }

           driveMotor(g_sCommand.u8MotorCmd,g_sCommand.u8Pwm);

          

           vTaskDelay((3000L* configTICK_RATE_HZ) / 1000L);   // 运行  3000ms,

          

           driveMotor(BRAKE,0);                 // 停止运行

    }

    else

    {

           vTaskDelay((1000L* configTICK_RATE_HZ) / 1000L);   // 延时  1000ms

    }

  }

}

任务执行也比原来更容易理解,尤其是原来定义的每个命令执行3s的处理,此处用RTOS的延时命令就很直观的处理了,看上去程序在此死等,实际上RTOS已经挂起此任务,执行别的任务(taskA),以保证控制逻辑正确。这就是使用RTOS的优势,调度交给系统了。

初始化(setup)中修改如下:

……..

  // ------ 190612 ---------------

  xQueueCommand =xQueueCreate(QUEUE_ITEM_NUM,QUEUE_ITEM_SIZE);

 

      // create taskA at priority 2

  h_taskA = xTaskCreate(taskA, NULL,configMINIMAL_STACK_SIZE, NULL, 2, NULL);

 

  // create taskB at priority 2

  h_taskB = xTaskCreate(taskB, NULL, configMINIMAL_STACK_SIZE,NULL, 2, NULL);

 

  // create taskC at priority 2

  h_taskC = xTaskCreate(taskC, NULL,configMINIMAL_STACK_SIZE, NULL, 2, NULL);

 

  // check for creation errors

  if (xQueueCommand == NULL || h_taskA !=pdPASS || h_taskB != pdPASS || h_taskC != pdPASS)

  {

    Serial.println(F("Creationproblem"));

    while(1);

  }

 

  // start scheduler

  vTaskStartScheduler();

  while(1); 

}

相应的消息接收处理回调函数修改为:

voidoneWheel_cb( const geometry_msgs::Twist& cmd_msg)

{

  int temp;

  static portBASE_TYPE  xHigherPriorityTaskWoken;

  command_type  sCommand;

 

  if(cmd_msg.linear.x == 0.0)

  {

    sCommand.u8MotorCmd= BRAKE;

    sCommand.u8Pwm= 0;

  }

  else

  {

       if(cmd_msg.linear.x > 0.0)

        {

           sCommand.u8MotorCmd = FORWARD;

           sCommand.u8Pwm = byte(cmd_msg.linear.x);

        }

       else

        {

           sCommand.u8MotorCmd = BACKWARD;

           sCommand.u8Pwm = byte(0 - cmd_msg.linear.x);

        }

      }

 

  temp = int(cmd_msg.angular.z);

      if(temp <0)

      {

       temp = 0;

      }

     

      if(temp >180)

      {

             temp = 180;

      }

     

      sCommand.u8ServoVal = byte(temp);

     

      xQueueSendFromISR(xQueueCommand,&sCommand,&xHigherPriorityTaskWoken);

      if(xHigherPriorityTaskWoken == pdTRUE)

      {

             // 激活任务切换

      }

}

上面那个“激活任务切换”部分还没吃透,暂时空着,其主要作用是在发送队列消息后,启动一次系统任务调度处理,以便等待此消息的任务可以立刻响应,这个在uCOS中是系统自己处理的,在系统的中断服务程序退出时,会自己启动一次任务切换处理。在 FreeRTOS中好像需要显性的操作一下,这个等实际编程时再琢磨。

此时原来Arduino的loop函数没有作用了:

// WARNINGidle loop has a very small stack (configMINIMAL_STACK_SIZE)

// loopmust never block

void loop(){

  // Not used.

}

程序运行全部由FreeRTOS管理了,在Arduino程序中只是体现在这一句:

#include<STM32FreeRTOS.h>     // 190612

应该说还是比在MDK中编写简化了很多。

上述程序编写完成后,只是修改了一些语法错误,程序就实现了原来的功能,编译后资源占用如下:


 

这比上一个程序占用的程序空间大了不少,用了约30k字节,即便这样,应该还是有足够的空间编写功能程序的。

如果真的遇到功能需求复杂,换一个MCU即可,STM32有充足的选择空间,支持STM32Duino的MCU也很多。

至此,已经验证了使用STM32+Arduino环境可以实现 ROS系统的外围功能硬件的构建,下一步可以回归到 ROS,看看如何使用程序实现通过 ROSSerial控制小车。现在测试使用的还是命令行发送。

不过这一关打通了,为后续ROS学习提供了足够的素材,否则我看网上很多人尝试ROS都是基于有限的几个平台,没有变化,关键是成本颇高,削弱了很多人的参与激情。

 

注:程序附在 QQ 群 32438885 的共享文件夹中,供参考。

(程序重点是测试这样是否可行,没有严格处理RTOS函数的返回值,请看客留意!)

评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

嵌入之梦

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值