ROS机器人底盘(24)-嵌入式部分框架设计与实现

Robot类

先来张类关系图


ClassDiagram.png

在贴个main的代码

#include "robot.h"

int main(void)
{
    Robot::get()->init();

    while (1)
    {
      Robot::get()->check_command();
          Robot::get()->do_kinmatics();
          Robot::get()->calc_odom();
          Robot::get()->get_imu_data();
          Robot::get()->check_joystick();
    }                                   
}

Robot为一个单例模式类,通过他完成大部分工作, main函数中可以看到其功能:

  • 初始化
  • 检测命令
  • 运动解算
  • 计算里程
  • 获取imu数据
  • 检查遥控

与之关联的类有:

  • Model 这是个运动模型接口类,可以看到该类有几个派生类Differential Omni3 Mecanum 分别代表对不同机器人模型的解算
  • Dataframe 协议接口类,通过继承该类可以使用不同的协议进行通讯,这里实现了一个Simple_dataframe的派生类
  • Transport 通讯接口类,通过继承该类可以使用不同的通讯口接口通讯这里实现了一个USART_transport的派生类,显然这个USART_transport是使用串口通讯
  • MotorController 电机控制接口,派生出一个名为CommonMotorController类控制
  • PID 显然是PID控制类
  • Encoder 是编码器读取类
  • Joystick ps2遥控器
  • IMU 惯性测量单元相关
    同时Robotnotify继承,实现了一个观察者模式实现对某一个或者多个事件的观察,这里是对消息的关注,即收到消息会通知到Robot,通过Dataframe::register_notify注册

另外有另外几个单例类

  • Board 该类维护bsp相关的接口
  • Queue 实现了一个简单的队列
  • Data_holder该类维护这多个与通讯业务相关的类对象

Robot::init


这里主要完成对各个部件的初始化

Robot中的接口实例化


Robot中维护的多为接口类的指针, 可以看到在下面的代码中完成对象的实例化

根据配置的宏使用不同的运动模型, 其他接口的实例化也类似

运动解算

这里主要根据发送的速度得到各个轮子的转速,这部分工作是在Robot::update_velocity 而该函数为作为

对设置速度消息的关注的回调,该回调函数中

void Robot::update_velocity(){
    short vx = min(max(Data_holder::get()->velocity.v_liner_x, -(short(Data_holder::get()->parameter.max_v_liner_x))), short(Data_holder::get()->parameter.max_v_liner_x));
    short vy = min(max(Data_holder::get()->velocity.v_liner_y, -(short(Data_holder::get()->parameter.max_v_liner_y))), short(Data_holder::get()->parameter.max_v_liner_y));
    short vz = min(max(Data_holder::get()->velocity.v_angular_z, -(short(Data_holder::get()->parameter.max_v_angular_z))), short(Data_holder::get()->parameter.max_v_angular_z));

    float vel[3]={vx/100.0, vy/100.0, vz/100.0};
    float motor_speed[MOTOR_COUNT]={0};
    model->motion_solver(vel, motor_speed);


    for(int i=0;i<MOTOR_COUNT;i++){
        input[i] = motor_speed[i]*short(Data_holder::get()->parameter.encoder_resolution)/(2*__PI)*short(Data_holder::get()->parameter.do_pid_interval)*0.001;
    }

#if DEBUG_ENABLE
    printf("vx=%d %d motor_speed=%ld %ld\r\n", vx, vz, long(motor_speed[0]*1000), long(motor_speed[1]*1000));
#endif

    last_velocity_command_time = Board::get()->get_tick_count();
    do_kinmatics_flag = true;
}
  • 首先对速度限制做判断


    judge
  • 然后通过运动模型类解算得到轮子的速度motor_speed

    solve

  • 在转换到pid间隔时间(Data_holder::get()->parameter.do_pid_interval)内各个电机行径的编码器值,得到一组input(多个电机)

    input

PID

  • 采集编码器在pid间隔时间(Data_holder::get()->parameter.do_pid_interval)走了多少,即一组 feedback(多个电机)
    feedback
  • 有了输入input,反馈feedback我们既可以做pid运算了, 得到的一组output即为控制电机的pwm(负为反向)
    do pid
  • 控制电机输出


    control

wheel odom

  • 计算一定间隔时间内(CALC_ODOM_INTERVAL这里固定为100ms)编码器改变值,得到一组dis
  • 反解, 通过运动模型接口转换轮子的位移到机器人的里程


推荐阅读更多精彩内容