立创梁山派开发板-21年电赛F题-送药小车-直流减速电机PID速度环与位置环调试_小车pid需要位置环吗

直流减速电机PID速度环与位置环调试

什么是PID？

PID（Proportional-Integral-Derivative）控制是一种广泛应用于工业和嵌入式系统的控制算法。它通过比较期望的设定点和实际的过程变量之间的差异（称为误差）来调整控制系统的输出。PID控制器结合了比例（P）、积分（I）和微分（D）三种控制方式，以实现对系统的精确控制。

PID控制的三个主要组成部分：

1. 比例控制（P）：比例控制是根据误差的大小来调整控制器输出。误差越大，输出调整幅度越大。这种控制方式有助于快速地减小误差，但可能导致系统在设定点附近来回波动，无法消除静差。

2. 积分控制（I）：积分控制是根据误差累积的总量来调整控制器输出。当系统存在持续的静差时，积分控制有助于消除这种误差，使系统最终达到设定点。但过度的积分控制可能导致系统响应变慢或发生过冲。

3. 微分控制（D）：微分控制是根据误差变化的速度来调整控制器输出。当误差变化速度较快时，微分控制可以抑制系统过冲和振荡。但微分控制对噪声敏感，可能导致输出不稳定。

线性PID算法可以用下式表示：

其中

• Kp是比例控制

• Ki是积分控制

• Kd是微分控制

• e是误差

• t:目前时间

• 丆：是积分变数

上面的式子是连续型的PID公式，但是在嵌入式系统中是不可能连续的，所有的都是数字的离散量。针对PID计算来说，都是间隔一定时间去计算的，改变的只是间隔时间的大小。

离散型的位置式PID和增量式PID公式是将连续时间域的PID公式转换为离散时间域，以便于在数字控制系统中实现。以下是离散型的位置式PID和增量式PID公式以及解释。

离散型位置式PID公式

离散型位置式PID（绝对式PID）公式如下：

u[k] = Kp * e[k] + Ki * T * Σe[k] + Kd * (e[k] - e[k-1]) / T

其中，u[k]是第k个采样时刻的控制器输出，e[k]是第k个采样时刻的误差（设定点与过程变量之差），Kp、Ki和Kd分别是比例、积分和微分系数，T表示采样时间间隔。

在离散型位置式PID控制器中，比例项直接由当前误差计算，积分项由误差累加求和并乘以采样时间间隔，微分项由当前误差和上一个误差的差分除以采样时间间隔计算。

离散型增量式PID公式

离散型增量式PID（速度式PID）公式如下：

Δu[k] = Kp * (e[k] - e[k-1]) + Ki * T * e[k] + Kd * (e[k] - 2 * e[k-1] + e[k-2]) / T

其中，Δu[k]是第k个采样时刻的控制器输出增量，其他符号含义与上面的离散型位置式PID相同。

在离散型增量式PID控制器中，比例项由当前误差和上一个误差的差来计算，积分项由当前误差乘以采样时间间隔计算，微分项由当前误差、上一个误差和再上一个误差的差分除以采样时间间隔计算。这种控制器适用于需要避免输出突变和过冲的场景。

怎么调节PID呢？

如果某个 P，I，D 参数偏离了原点的 PID 值，控制效果就会变化。控制效果与各个参数的规律如图所示。截取自：链接​​​​​​

 

当然，如果你能记录PID的曲线的话，就可以导入matlab进行自动调节pid参数，网上有挺多教程，大家可以搜一搜。这里我们就只介绍靠经验来调节了（简称瞎鼓捣）。

比较常用的有试错法，这个是一种经验性的方法，通过尝试不同的PID参数值，找到合适的参数设置。基本步骤如下：

• 首先，将I和D参数设置为0，只保留P参数。

• 增加P参数，直到系统的响应时间变得合适。过高的P参数会导致系统振荡，过低的P参数会导致系统响应慢。

• 接着，增加I参数，以减小稳态误差。适当调整I参数，直到系统达到满意的性能。

• 最后，增加D参数，以降低系统的超调量和提高响应速度。适当调整D参数，直到系统达到满意的性能。

假设我们需要控制一个直流减速电机的速度，为了达到某个设定值。我们将使用PID控制器来调节电机的驱动PWM，来达到所需的速度。

简要步骤如下：

1. 初始化参数：首先将Kp，Ki，Kd全部设置为0，这样PID控制器不产生任何控制作用。

2. 调节比例增益Kp：逐渐增加Kp的值，直到系统开始出现持续的振荡（速度在设定值附近上下波动）。此时记录下Kp的值。

3. 调节积分增益Ki：将Kp设置为刚刚Kp值的一半，然后逐渐增加Ki的值，直到系统的静态误差（速度与设定值的差距）变得很小。过大的Ki值可能导致系统响应过慢或振荡，此时需要适当减小Ki值。

4. 调节微分增益Kd：保持当前的Kp和Ki值不变，逐渐增加Kd的值，直到系统的动态性能满足要求（例如，要求速度在设定值附近的波动范围和响应时间）。过大的Kd值可能导致系统响应过于敏感或噪声敏感，此时需要适当减小Kd值。

5. 微调参数：以上步骤得到的Kp，Ki，Kd值可能仍然不是最优解，因此还需要根据实际系统的性能要求进行微调。通常情况下，可以先调整Kp和Ki，然后再调整Kd。在调整过程中，需要关注系统的稳定性、阻尼、响应速度、过冲等性能指标。

这三个参数到底影响了什么？

这里就以恒温热水壶来举例说明了。

Kp（比例增益）

比例项是误差与控制器输出之间的线性关系。增大Kp会使系统响应速度更快，但可能会导致更大的过冲和振荡。减小Kp可以减少过冲和振荡，但会降低响应速度，甚至导致系统无法达到设定点（静态误差）。

以恒温热水壶为例，如果比例增益Kp设置得较大，当温度偏离设定点时，恒温热水壶会迅速地增加或减少加热功率。这可能导致温度在设定点附近发生过冲和振荡。而较小的Kp值可以减少过冲和振荡，但恒温器对温度偏差的响应速度会变慢。

Ki（积分增益）

积分项考虑了误差的累积效应，可以消除静态误差。增大Ki可以加快误差积分，使系统更快地达到设定点，但过大的Ki会导致积分过程过于敏感，引起系统的振荡和不稳定。

在上面恒温热水壶示例中，如果存在静态误差（即使加热持续了很久，温度仍无法达到设定点），可以通过增大积分增益Ki来消除这种静态误差，让水壶的加热功率再大一些。然而，过大的Ki值可能会导致恒温器对温度波动过于敏感，从而引起系统的振荡。

Kd（微分增益）

微分项关注误差的变化速度，可以预测系统的未来行为。增大Kd可以提高系统的阻尼性能，减小过冲和振荡，但过大的Kd会导致系统对噪声过于敏感，引入不稳定性。

在恒温热水壶示例中，如果系统在接近设定点时出现过冲和振荡，可以通过增大微分增益Kd来减小这些现象。微分项可以预测系统的未来行为，从而提前调整加热功率。然而，过大的Kd值可能会使恒温器对温度噪声过于敏感，从而影响系统的稳定性。

什么是串级PID？

串级PID控制是将多个PID控制器按照层级结构连接起来的控制策略。在串级PID控制中，一个PID控制器的输出被用作另一个PID控制器的输入。这种通常用于具有多个互相影响的控制变量的系统，可以提高系统的性能和稳定性。

以平衡小车为例，平衡小车一般是一个具有两个控制目标的系统：保持车体竖直和控制车体的位置。我们可以使用串级PID控制来实现这两个目标。

平衡小车的串级PID控制可以分为两个层级：

1. 内环控制：内环控制负责保持车体竖直。这个环节的PID控制器接收车体的倾角和倾角速度作为输入，并输出一个控制信号，用于调整驱动电机的转速，以达到保持车体竖直的目的。倾角控制是一个快速响应的过程，因此内环控制器需要具有较高的响应速度。

2. 外环控制：外环控制负责控制车体的位置。这个环节的PID控制器接收车体的实际位置和目标位置作为输入，并输出一个目标倾角。这个目标倾角被用作内环控制器的设定点。位置控制是一个较慢的过程，因此外环控制器的响应速度可以较低。

在平衡小车的串级PID控制中，首先需要调整内环控制器的参数，以保证车体能够迅速地恢复到竖直状态。接下来，调整外环控制器的参数，以实现对车体位置的控制。在调整参数的过程中，需要关注小车的稳定性、响应速度、过冲等性能指标。

串级PID控制可以提高系统的稳定性，他可以确保外环期望的值可以稳定执行，比如说如果平衡小车只有外环控制的话，他输出的PWM值太小的话很有可能会导致小车根本无法克服摩擦力运动，如果他把这个速度值给到内环控制后，就由内环来确保小车以这个速度来执行。在送药小车的电机控制中，速度环就是内环，外环可以是位置环，寻红线环，角度环等。

实现PID的程序代码

在这里实现了位置式PID的程序，它在仓库里的2_code->applications->module->pid里面。

.H文件

#ifndef _POSITIONAL_PID_H
#define _POSITIONAL_PID_H
 
#include "stdint.h"
 
typedef enum {
    PID_DISABLE, /* PID失能 */
    PID_ENABLE, /* PID使能 */
} positional_pid_status;
 
typedef struct _positional_pid_params_t{
 
    char control;
 
    float kp;
    float ki;
    float kd;
 
    float target;
    float measure;
    float last_measure;
 
    float error;
    float last_error;
 
    float p_out;
    float i_out;
    float d_out;
 
    float output;
    float last_output;
 
    float output_max;
    float output_min;
 
    float integral_limit;
 
    float dead_zone;
 
    void (*positional_pid_params_init)(
        struct _positional_pid_params_t *positional_pid, float kp, float ki,
        float kd, float dead_zone, float output_max, float output_min);
    void (*positional_pid_set_value)(
        struct _positional_pid_params_t *positional_pid, float kp, float ki,
        float kd);
    void (*positional_pid_control)(
        struct _positional_pid_params_t *positional_pid,
        positional_pid_status status);
} positional_pid_params_t;
 
void positional_pid_init(positional_pid_params_t *positional_pid, float kp,
                         float ki, float kd,
 
                         float dead_zone, float output_max, float output_min);
float positional_pid_compute(positional_pid_params_t *positional_pid,
                            float target, float measure);
 
#endif

这个是位置式PID的C语言实现。上面定义了一个名为positional_pid_params_t的结构体，用于存储 PID 控制器的参数和状态信息。具体包括：

• 控制器使能标志（control）当其值为PID_ENABLE时，表示控制器处于启用状态，会根据目标值和测量值计算输出；当其值为PID_DISABLE时，表示控制器处于禁用状态，输出为 0。

• 比例、积分、微分增益（kp、ki、kd）分别表示 PID 控制器的比例增益、积分增益和微分增益。这些增益参数决定了控制器对误差的反应程度。

• 目标值和实际测量值（target、measure、last_measure）target表示期望的目标值，measure表示实际测量到的当前值，last_measure表示上一次测量到的值。这些值用于计算误差，误差是PID想要减小的值。

• 当前误差和上一次误差（error、last_error）error表示当前误差，即目标值与测量值之差，last_error表示上一次的误差。这些误差值用于计算 PID 控制器的比例、积分、微分输出。

• 比例、积分、微分输出（p_out、i_out、d_out）分别表示 PID 控制器的比例输出、积分输出和微分输出。这些输出值是根据当前误差、增益参数以及之前的误差计算得到的。

• 控制器输出和上一次输出（output、last_output）output表示控制器的总输出，它是比例输出、积分输出和微分输出的和；last_output表示上一次的总输出。这些值在控制器使能时会返回给调用函数。

• 输出最大值和最小值（output_max、output_min）分别表示控制器输出的最大值和最小值。这些值用于限制控制器的输出范围，防止因过大的输出导致系统不稳定。

• 积分限幅（integral_limit）表示积分限幅，用于避免积分饱和现象，在c程序中还没使用到，

• 死区（dead_zone）表示死区大小。当误差的绝对值小于死区大小时，控制器输出为 0。这有助于减小微小误差对控制器输出的影响。

• 初始化函数指针（positional_pid_params_init）

• 设定 PID 参数的函数指针（positional_pid_set_value）

• 控制 PID 的函数指针（positional_pid_control）

.C文件

 
#include "positional_pid.h"
#include <math.h>
 
#define ABS(x) ((x > 0) ? x : -x)
 
static void positional_pid_params_init(positional_pid_params_t *positional_pid,
                                  float kp, float ki, float kd,
 
                                  float dead_zone, float output_max,
                                  float output_min)
{
    // 初始化 PID 参数
    positional_pid->kp = kp;
    positional_pid->ki = ki;
    positional_pid->kd = kd;
 
    // 初始化死区、输出上限和输出下限
    positional_pid->dead_zone = dead_zone;
    positional_pid->output_max = output_max;
    positional_pid->output_min = output_min;
 
    // 初始化目标值和输出值
    positional_pid->target = 0;
    positional_pid->output = 0;
}
 
static void positional_pid_set_value(positional_pid_params_t *positional_pid,
                                 float kp, float ki, float kd)
{
    // 设置 PID 参数
    positional_pid->kp = kp;
    positional_pid->ki = ki;
    positional_pid->kd = kd;
}
 
float positional_pid_compute(positional_pid_params_t *positional_pid,
                             float target, float measure)
{
    if (positional_pid->control == PID_ENABLE)
    {
        // 设置目标值和测量值
        positional_pid->target = target;
        positional_pid->measure = measure;
 
        // 计算误差
        positional_pid->error =
            positional_pid->target - positional_pid->measure;
 
        if (ABS(positional_pid->error) > positional_pid->dead_zone)
        {
            // 计算比例项
            positional_pid->p_out = positional_pid->kp * positional_pid->error;
            // 计算积分项
            positional_pid->i_out += positional_pid->ki * positional_pid->error;
            // 计算微分项
            positional_pid->d_out =
                positional_pid->kd
                * (positional_pid->error - positional_pid->last_error);
 
            // 计算总输出
            positional_pid->output = positional_pid->p_out
                                     + positional_pid->i_out
                                     + positional_pid->d_out;
        }
 
        // 限制输出在输出上限和输出下限之间
        if (positional_pid->output > positional_pid->output_max)
        {
            positional_pid->output = positional_pid->output_max;
        }
        if (positional_pid->output < (positional_pid->output_min))
        {
            positional_pid->output = positional_pid->output_min;
        }
 
        // 更新上一次测量值、输出值和误差值
        positional_pid->last_measure = positional_pid->measure;
        positional_pid->last_output = positional_pid->output;
        positional_pid->last_error = positional_pid->error;
 
        return positional_pid->output;
    }
    else
    {
        return 0.0f;
    }
}
 
void positional_pid_control(positional_pid_params_t *positional_pid,positional_pid_status status)
{
    // 控制 PID 的使能状态
    positional_pid->control = status;
}
 
void positional_pid_init(positional_pid_params_t *positional_pid, float kp,
                         float ki, float kd,
 
                         float dead_zone, float output_max, float output_min)
{
    // 初始化 PID 控制器
    positional_pid->positional_pid_params_init = positional_pid_params_init;
    positional_pid->positional_pid_set_value = positional_pid_set_value;
    positional_pid->positional_pid_control = positional_pid_control;
    // 调用初始化函数设置参数
    positional_pid->positional_pid_params_init(
        positional_pid, kp, ki, kd, dead_zone, output_max, output_min);
    // 默认使能 PID 控制器
    positional_pid->control = PID_ENABLE;
}

1. positional_pid_params_init 初始化 PID 控制器的参数.传入的参数值会被分别赋值到相应的结构体成员变量中，包括增益值、死区大小、输出上限和下限等。

2. positional_pid_set_value 设置 PID 控制器的增益参数。传入的参数值（kp、ki、kd）会被分别赋值到相应的结构体成员变量中。

3. positional_pid_compute 计算 PID 控制器的输出。是 PID 控制器的核心计算逻辑。首先，根据传入的目标值 target 和实际测量值 measure，计算当前误差 error 和上一次误差 last_error。然后，根据误差和增益参数，分别计算比例输出、积分输出和微分输出。接下来，将这三个输出相加得到总输出，再根据输出限制范围（output_max 和 output_min）对输出进行限幅。最后，将一些必要的值存储到结构体中，以便下次计算使用，并返回当前输出。

4. positional_pid_control 用于控制 PID 控制器的启用和禁用状态。传入的参数 control 表示控制器的状态。如果设置为 PID_DISABLE，则将输出设置为 0。

5. positional_pid_init 初始化 PID 控制器，设置函数指针和调用初始化函数（将函数指针分别指向相应的函数实现）并默认使能PID控制器。

如何实现单个电机的速度和位置环

这个问题是一个非常关键的问题。这是因为小车电机的速度直接影响了小车的行驶速度和精度，而行驶速度和精度又是小车比赛中的关键因素之一。如果小车电机的速度不稳定，那么小车的行驶速度和精度就会受到影响，导致小车无法在比赛中达到最佳表现。因此，在进行智能车比赛之前，需要先解决小车电机速度问题，保证小车电机的速度稳定、准确，并且能够根据需要进行调整。并不是说给一个确定的电压或者确定的PWM值就能让电机保持到一个准确的转速，比如每个电机的绕线，轴承，机械性能，负载能力，各个接触点的摩擦力这些都有差别，小车轮子所受到的摩擦力的不同也会影响小车的前进速度。

结合之前的文档06_如何计算小车轮子转速.md和07_电机驱动-舵机-蜂鸣器PWM配置.md我们现在已经可以获取到轮子转速和改变PWM值从而驱动电机转动。通过编码器我们可以进行电机的转速测量，通过改变PWM值我们可以让电机提高或者降低转速。在这个条件下，我们的目标值是让小车电机稳定在一个转速，测量值是小车当前由编码器测量得到的实际速度，输出值是电机的PWM值。通过调节PID的三个参数，让目标值变化时，实际值可以又快又准的接近目标值。

简单来说，PID就是测量出实际值，设定一个目标值，输出一个驱动值，让实际值尽量和目标值靠近。实现单个电机速度环时，

具体实现在仓库里的2_code->applications-pid_thread.c里面。

简要步骤如下：

1. 引入必要的头文件。

2. 定义需要的速度调节PID变量：positional_pid_params_t motor2_speed_pid;和实际的电机速度float motor1_speed_target = 0.0f;

3. 通过uMCN订阅发布机制申明编码器1的信息：MCN_DECLARE(encoder_m1_topic);

4. 创建一个PID线程，初始化速度环的PID配置：

// 速度环配置
//  motor1 速度环 pid 参数初始化
positional_pid_init(&motor1_speed_pid, 125, 25, 80, 0.01, 1000, -1000);
// 修改motor1 速度环 kp ki kd
motor1_speed_pid.positional_pid_set_value(&motor1_speed_pid, 125, 25, 80);

1. 开启一个软件定时器，用固定频率去计算PID的输出值，在送药小车中我用的间隔是20ms，也就是以50Hz的频率去计算。

//从uMCN获取编码器信息
if (mcn_poll(encoder1_nod))
{
    mcn_copy(MCN_HUB(encoder_m1_topic), encoder1_nod, &encoder1_value);
}
// 计算电机1的PWM输出值
pwm_value = (int16_t)positional_pid_compute(
    &motor1_speed_pid, motor1_speed_target, encoder1_value.speed);
// 应用电机1的PWM值，实际给电机驱动的PWM波
motor1_pwm_value_set(pwm_value);

---

速度环就到这里，位置环和上面的类似，也要定义相应的PID变量，初始化PID参数，在开启的软件定时器中需要再加入以下代码

1. 位置环依靠编码器累积下来的数值确定轮子具体转了多少个脉冲，根据这个脉冲来计算小车电机转过的位置。位置环的PID，测量值就实际的编码器脉冲，目标值是想要控制电机选择多少脉冲，输出值是速度环的速度。

position_result_motor1_speed_target = (positional_pid_compute(
    &motor1_position_pid, (float)motor1_position_target,
    (float)position1_measurement));

怎样进行调试呢？

可以做一个finsh控制台指令来在电机运行时改变PID的值：类似car_find_lines_pid.Kp = atof(argv[3]);下面这样：

void pid_test(int argc, char **argv)
{
    static float temp_speed;
    static uint32_t temp_delay_ms = 10;
    // pid_test speed forward|backward|left|right  time
    /* 检查输入的变量是否有四个 */
    if (argc < 4)
    {
        rt_kprintf("Please input'pid_test speed <forward|backward|left|right> "
                   "speed_value <time>'\n");
        return;
    }
    if (!rt_strcmp(argv[1], "speed"))
    {
        if (!rt_strcmp(argv[2], "forward"))
        {
            motor1_speed_target = atof(argv[3]);
            motor2_speed_target = atof(argv[3]);
            rt_thread_mdelay(atoi(argv[4]));
            motor1_speed_target = 0.0f;
            motor2_speed_target = 0.0f;
        }
        else if (!rt_strcmp(argv[2], "backward"))
        {
            motor1_speed_target = -atof(argv[3]);
            motor2_speed_target = -atof(argv[3]);
            rt_thread_mdelay(atoi(argv[4]));
            motor1_speed_target = 0.0f;
            motor2_speed_target = 0.0f;
        }
        else if (!rt_strcmp(argv[2], "left"))
        {
            motor1_speed_target = atof(argv[3]);
            motor2_speed_target = -atof(argv[3]);
            rt_thread_mdelay(atoi(argv[4]));
            motor1_speed_target = 0.0f;
            motor2_speed_target = 0.0f;
        }
        else if (!rt_strcmp(argv[2], "right"))
        {
            motor1_speed_target = -atof(argv[3]);
            motor2_speed_target = atof(argv[3]);
            rt_thread_mdelay(atoi(argv[4]));
            motor1_speed_target = 0.0f;
            motor2_speed_target = 0.0f;
        }
    }
    if (!rt_strcmp(argv[1], "position"))
    {
        if (!rt_strcmp(argv[2], "go"))
        {
            car_go_position(atoi(argv[3]));
 
            temp_speed = 0;
            while (temp_speed <= atof(argv[4]))
            {
                temp_speed += 0.01f;
                set_car_position_max_speed(temp_speed);
                rt_thread_mdelay(temp_delay_ms);
            }
        }
        else if (!rt_strcmp(argv[2], "spin"))
        {
            car_spin_position(atoi(argv[3]));
        }
    }
    if (!rt_strcmp(argv[1], "angle"))
    {
        if (!rt_strcmp(argv[2], "pid"))
        {
            car_find_lines_pid.Kp = atof(argv[3]);
            car_find_lines_pid.Ki = atof(argv[4]);
            car_find_lines_pid.Kd = atof(argv[5]);
            car_find_lines_pid.MaxOut = atof(argv[6]);
        }
        else if (!rt_strcmp(argv[2], "absolute"))
        {
            turn_target = atof(argv[3]);
            if (turn_target >= 180)
            {
                turn_target = 180;
            }
            else if (turn_target <= -180)
            {
                turn_target = -180;
            }
        }
        else if (!rt_strcmp(argv[2], "run"))
        {
            raw_target_speed = atof(argv[3]);
            rt_thread_mdelay(atoi(argv[4]));
            raw_target_speed = 0;
        }
    }
    else
    { /* 输入的是其他内容 */
        rt_kprintf("Please input'pid_test speed <forward|backward|left|right> "
                   "speed_value <time>'\n");
    }
}
MSH_CMD_EXPORT(pid_test, pid test sample
               : servo_test<servo1 | servo2><pulse(32 - 150)>);

当然，想快一点的话就直接用MDK进入硬件仿真，直接把具体的变量添加到Watch窗口进行修改，下面就直接用进入硬件调试直接修改变量的方法来调试

以调试单电机速度环为例：

这里的测量值是小车当前由编码器测量得到的实际速度，输出值是电机的PWM值。通过调节PID的三个参数，让目标值变化时，实际值可以又快又准的接近目标值。

我们可以在pid线程中加入如下代码，通过串口打印出轮子1的设定速度和正交编码器1测出来的轮子转速。

while (1)
{
    mcn_copy(MCN_HUB(encoder_m1_topic), encoder1_nod, &encoder1_value);
    rt_kprintf("motor1:%f,%f\n",raw_target_speed,encoder1_value.speed);
    rt_thread_mdelay(20);
}

在MDK中，把motor1_speed_pid,raw_target_speed添加到Watch变量中，这样我们就可以实时修改了。

打开在03_软件工具与调试工具介绍.md文档中介绍的软件VOFA+，选择数据引擎为FireWater，数据接口为串口，串口号选择你具体使用的串口号，波特率选115200,点击左上角的小蓝点就可以打开了

然后在左侧的控件中拖拽一个波形图到上面的tab里面，在控件上面右键选择如下图所示。

接下来就是通过MDK改变motor1_speed_pid里面的Kp，Ki，Kd和raw_target_speed这几个变量了。

下面的动图是我改变raw_target_speed的结果，I0是目标值，I1是测量值。

速度环调试完后再调位置环：

可以在pid线程中加入如下代码，通过串口打印出轮子1的目标位置和正交编码器1测出来的累积脉冲。

while (1)
{
    rt_kprintf("motor1:%d,%d\n",motor1_position_target,position1_measurement);
    rt_thread_mdelay(20);
}

在MDK中，把motor1_position_pid,motor1_position_target添加到Watch变量中，这样我们就可以实时修改了。

和上面调节速度环一样打开VOFA+开始调试。接下来就是通过MDK改变motor1_position_pid里面的Kp，Ki，Kd和raw_target_speed这几个变量了。

下面的动图是我改变raw_target_speed的结果，I0是目标值，I1是测量值。

可以看到他到达目标值的速度有点慢，这是因为电机转动快了会导致轮子打滑，这里我用了比较保守的值，设置了motor1_position_pid的output_max为3，也就是说这个轮子速度最快是3m/s，当然这个值在你自己调试的时候可以适量加大，目前我选择的这个电机，在PWM最高时可以达到八点几米每秒。当然这里我设置的Kp是偏小的，主要也是为了让轮子的速度变化小一点，平缓一些，防止打滑情况的出现。