RM学校代码分析

Freertos任务

总共执行四个任务，三个底盘的任务和一个测试的任务，分别为chasis_task.cpp,my_test_task.cpp,communicate_task.cpp,detect_task.cpp。下面分别从每个任务入手分析代码，先从chasis_task.cpp入手，在文件chasis.cpp种声明了类Chasis的全局实例对象，即chasis, 在任务chasis_task中先执行初始化即init():

chasis::init()函数：

先获取遥控器指针，通过代码：

chassis_RC = remote_control.get_remote_control_point();
last_chassis_RC = remote_control.get_last_remote_control_point();

然后设置模式，设置初始的模式为CHASSIS_ZERO_FORCE，控制模式设置为CHASSIS_VECTOR_RAW，即直接输出控制，然后通过for循环初始化四个电机，在chasis.h文件中对Motor.cpp文件中的类M3508进行了实例化：

M3508_motor chassis_motive_motor[4];

初始化中即传入由函数get_chassis_motive_motor_measure_point(i) 获取到都电机数据指针，然后创建一个数组**motive_speed_pid_parm[5]**，存入pid相关数据：

{MOTIVE_MOTOR_SPEED_PID_KP, MOTIVE_MOTOR_SPEED_PID_KI, MOTIVE_MOTOR_SPEED_PID_KD, MOTIVE_MOTOR_SPEED_PID_MAX_IOUT, MOTIVE_MOTOR_SPEED_PID_MAX_OUT};

赋值完成后调用pid::init()函数：

void Pid::init(pid_mode_e mode_, const fp32 *pid_parm, fp32 *ref_, fp32 *set_, fp32 erro_delta_)
{
    mode = mode_;
    data.Kp = pid_parm[0];
    data.Ki = pid_parm[1];
    data.Kd = pid_parm[2];
    data.kf = pid_parm[3];
    data.max_iout = pid_parm[3];
    data.max_out = pid_parm[4];
    data.ref_last = *ref_;
    data.set = set_;
    data.ref = ref_;
    data.error = *set_ - *ref_;

    if (data.mode == PID_ANGLE)
        data.error_delta = erro_delta_;
}

使用：

chassis_motive_motor[i].speed_pid.init(PID_SPEED, motive_speed_pid_parm, &chassis_motive_motor[i].speed, &chassis_motive_motor[i].speed_set, NULL);

即开启速度pid，传入数组地址motive_speed_pid_parm，电机当前速度，和设定速度。

使用一阶低通滤波器，即：

chassis_cmd_slow_set_vx.init(CHASSIS_CONTROL_TIME, chassis_x_order_filter);
chassis_cmd_slow_set_vy.init(CHASSIS_CONTROL_TIME, chassis_y_order_filter);

原理为滤波器输出 = 系数 × 当前输入指令 + (1 - 系数) × 上一次滤波器输出，使速度指令缓慢变化。

pid.clear()的作用：即通过清除所有内部状态量（尤其是积分项），确保控制器正常工作。