首先是导入 相应的 .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]; } }