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 惯性测量单元相关
  同时Robot从notify继承，实现了一个观察者模式实现对某一个或者多个事件的观察，这里是对消息的关注，即收到消息会通知到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
  

• 反解， 通过运动模型接口转换轮子的位移到机器人的里程