Robot类

先来张类关系图

在贴个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;
}

首先对速度限制做判断
然后通过运动模型类解算得到轮子的速度motor_speed
在转换到pid间隔时间（Data_holder::get()->parameter.do_pid_interval）内各个电机行径的编码器值，得到一组input(多个电机)

PID

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

wheel odom

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

24.嵌入式部分框架设计与实现

Robot类

Robot::init

Robot中的接口实例化

运动解算

PID

wheel odom

猜你喜欢