首先是导入 相应的 .h文件

#include "car_ros2.h"

下面是宏定义和全局变量

// 限制值在范围内
#define LIMIT_VALUE(value, min, max)    (value>max?max:(value<min?min:value))
//参数宏定义
// #define ENCODER_RESOLUTION      2160   //编码器分辨率, 轮子转一圈，编码器产生的脉冲数
#define ENCODER_RESOLUTION      2016   //编码器分辨率,实测这个值
#define WHEEL_DIAMETER          0.058    //轮子直径,单位：米
#define D_X                     0.30     //底盘X轴前后两轮中心的间距
#define D_Y                     0.28     //底盘Y轴左右两轮中心的间距
#define PID_RATE                100       //PID调节PWM值的频率速度变化1，编码器变化100
#define PI_ROS                      3.1415926
​
double pulse_time = 0.01;
double pulse_per_meter = 0;
float rx_plus_ry_cali = 0.3;
double angular_correction_factor = 1.0;
double linear_correction_factor = 1.0;
float ros_speed[3];
static int port_imu = 0;
static int imu_enable = 0;

• kinematics_init（void） ： 运动学解析参数初始化

float r_x = D_X/2;
float r_y = D_Y/2;
void kinematics_init(void)
{
    // 轮子转动一圈，移动的距离为轮子的周长WHEEL_DIAMETER*3.1415926，编码器产生的脉冲信号为ENCODER_RESOLUTION。
    // 则电机编码器转一圈产生的脉冲信号除以轮子周长可得轮子前进1m的距离所对应编码器计数的变化
    pulse_per_meter = (float)(ENCODER_RESOLUTION/(WHEEL_DIAMETER * PI_ROS)) / linear_correction_factor;
    
    r_x = D_X/2;
    r_y = D_Y/2;
    rx_plus_ry_cali = (r_x + r_y)/angular_correction_factor;
}

• encoder2dis（）数作用：将编码器值转换为实际变化的距离（单位：米）。

float encoder2dis(int encoder_in)
{   
    return (float)encoder_in/pulse_per_meter;
}

• convert_sptrue（）将速度转换为实际速度。

​
void convert_sptrue(int* speed_in, float* speed_out)
{   
    for(int i=0;i<4;i++){
        speed_out[i] = speed_in[i] * PID_RATE / pulse_per_meter;
    }
}

• convert_spvirtual（） 将实际速度转换为控制器速度。

​
void convert_spvirtual(float* speed_in, int* speed_out){
    for(int i=0;i<4;i++){
        speed_out[i] = (int16_t)(speed_in[i] * pulse_per_meter / PID_RATE);
    }
}

• kinematics_forward（）正向运动学解析，四个轮子位置变化->车子状态 车子位置

void kinematics_forward(float* dis_dt, float time_dt, float* car_state, float* car_pos){
    static float car_dx[10], car_dy[10], car_dyaw[10], car_dtime[10];
    
    static float car_dx_sum, car_dy_sum, car_dyaw_sum, car_time_sum;
    static int car_dt_cnt = 0;
    
    car_dt_cnt = (car_dt_cnt+1)%10;
​
    car_time_sum = car_time_sum - car_dtime[car_dt_cnt];
    car_dx_sum = car_dx_sum - car_dx[car_dt_cnt];
    car_dy_sum = car_dy_sum - car_dy[car_dt_cnt];
    car_dyaw_sum = car_dyaw_sum - car_dyaw[car_dt_cnt];
​
    car_dtime[car_dt_cnt] = time_dt;
    car_time_sum += car_dtime[car_dt_cnt];
​
    car_dx[car_dt_cnt] = (dis_dt[0] + dis_dt[1] + dis_dt[2] + dis_dt[3])/4.0;  // 车x方向位移
    car_dx_sum += car_dx[car_dt_cnt];
    car_state[0] = car_dx_sum / car_time_sum;      // x轴瞬时速度, 10次平均值
​
    car_dy[car_dt_cnt] = (0 - dis_dt[0] + dis_dt[1] + dis_dt[2] - dis_dt[3])/4.0;  // 车y方向位移
    car_dy_sum += car_dy[car_dt_cnt];
    car_state[1] = car_dy_sum / car_time_sum;      // y轴瞬时速度，10次平均值
​
    //积分计算里程计坐标系(odom_frame)下的机器人X,Y,Yaw轴坐标
    //dx = ( vx*cos(theta) - vy*sin(theta) )*dt
    //dy = ( vx*sin(theta) + vy*cos(theta) )*dt
    car_pos[0] += (cos((double)car_pos[2])*car_dx[car_dt_cnt] - sin((double)car_pos[2])*car_dy[car_dt_cnt]);
    car_pos[1] += (sin((double)car_pos[2])*car_dx[car_dt_cnt] + cos((double)car_pos[2])*car_dy[car_dt_cnt]);
​
    if(imu_enable){
        car_state[2] = (imu_get_angle(port_imu) - car_pos[2]) / time_dt;
        car_pos[2] = imu_get_angle(port_imu);
    }
    else{
        car_dyaw[car_dt_cnt] = ( 0 - dis_dt[0] + dis_dt[1] - dis_dt[2] + dis_dt[3] )/4.0 /(r_y + r_x);  // 车yaw转向角
        car_dyaw_sum = car_dyaw_sum + car_dyaw[car_dt_cnt];
        car_state[2] = car_dyaw_sum / car_time_sum;      // omega瞬时速度
        car_pos[2] += (car_dyaw[car_dt_cnt]);
    }
​
}

• kinematics_inverse （）逆向运动学解析，将底盘三轴速度转换为四个轮子的目标速度。

void kinematics_inverse(float v_tx, float v_ty, float omega, float* speed_out)
{
    speed_out[0] = v_tx - v_ty - (r_x + r_y)*omega;
    speed_out[1] = v_tx + v_ty + (r_x + r_y)*omega;
    speed_out[2] = v_tx + v_ty - (r_x + r_y)*omega;
    speed_out[3] = v_tx - v_ty + (r_x + r_y)*omega;
}

• mecanum_wheel（）控制麦克纳姆轮机器人的运动。

void mecanum_wheel(float v_tx, float v_ty, float omega)
{
    static int motor_speeds[4];
    static float wheel_speeds[4];
    kinematics_init();
    kinematics_inverse(v_tx, v_ty, omega, wheel_speeds);
    convert_spvirtual(wheel_speeds, motor_speeds);
    set_motor(1, motor_speeds[0]);
    set_motor(2, -motor_speeds[1]);
    set_motor(3, motor_speeds[2]);
    set_motor(4, -motor_speeds[3]);
}

• 与Ros2 相关的全局变量

static float car_pos[3];    // 位置值
static float car_state[3];  // 状态值
​
static float car_control[3];  // 控制值
static float time_last_run = 0;

• ros2_process （）ROS2 主要处理函数，包括控制机器人运动，编码器值转换，运动学解析等

void ros2_process()
{   
    static float dis_dt[4];
​
    static float time_last;
    static float time_dt;
    static int encoder_now[4], encoder_last[4], encoder_dt[4];
    static int encoder_p[4] = {1, -1, 1, -1};
    static float time_during = 0;
​
    time_during = seconds() - time_last_run;
    if(time_during < 0.2){
        mecanum_wheel(car_control[0], car_control[1], car_control[2]);
    }
    else if(time_during < 0.6 && time_during > 0.5){
        mecanum_wheel(0, 0, 0);
    }
​
    for(int i=0;i<4;i++){
        encoder_now[i] = motor_encoder(i+1) * encoder_p[i];
        encoder_dt[i] = encoder_now[i] - encoder_last[i];
        encoder_last[i] = encoder_now[i];
        dis_dt[i] = encoder2dis(encoder_dt[i]);
    }
    time_dt = seconds() - time_last;
    kinematics_forward(dis_dt, time_dt, car_state, car_pos);
    time_last = seconds();
}

• ros2_set_control（）设置控制值

void ros2_set_control(float* control_sp){
    car_control[0] = control_sp[0];
    car_control[1] = control_sp[1];
    car_control[2] = control_sp[2];
    time_last_run = seconds();
}

• show_odom（）显示机器人位置信息

void show_odom(){
    while(1){
        printf("car_pos\nx: %.2f y: %.2f \nyaw: %.2f\n", car_pos[0], car_pos[1], car_pos[2]);
        wait(0.1);
    }
}

下面几个关于IMU的是用来控制和设置IMU端口的。

void ros_imu_enable(){
    if(imu_enable){
        return;
    }
    imu_adjust_angle_init(port_imu, 50);
    imu_enable = 1;
}
​
void ros_imu_disable(){
    imu_enable = 0;
}
​
// 设置imu端口号
void ros_set_imu_port(int port_id){
    port_imu = port_id;
    ros_imu_enable();
}

• reset_odom_ros 重置位置信息

void reset_odom_ros(){
    if(imu_enable){
        imu_reset();
    }
    // 数据清零
    for(int i=0;i<3;i++){
        // 获取状态
        car_state[i] = 0;
        // 获取位置
        car_pos[i] = 0;
    }
}

• ros2_odom_start（）ROS2里程计初始化函数

void ros2_odom_start()
{
    static uint8_t ros_start_flag = -1;
    if(ros_start_flag != 1){
        ros_start_flag = 1;
        kinematics_init();
        setTimerHandle(ros2_process, 10);
    }
    else{
        reset_odom_ros();
    }
}

• get_odom_ros（）获取机器人状态和位置信息

void get_odom_ros(float* state_car, float* pos_car){
    for(int i=0;i<3;i++){
        // 获取状态
        state_car[i] = car_state[i];
        // 获取位置
        pos_car[i] = car_pos[i];
    }
}
​