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

LCKFB

送药小车代码仓库：https://gitee.com/lcsc/medical_car更好的观看体验请去：https://dri8c0qdfb.feishu.cn/wiki/UjwwwO0KZii5bykPcE4cJZafnAg
送药小车立创开源平台资料：https://oshwhub.com/li-chuang-kai-fa-ban/21-dian-sai-f-ti-zhi-neng-song-yao-xiao-che




直流减速电机PID速度环与位置环调试什么是PID？PID（Proportional-Integral-Derivative）控制是一种广泛应用于工业和嵌入式系统的控制算法。它通过比较期望的设定点和实际的过程变量之间的差异（称为误差）来调整控制系统的输出。PID控制器结合了比例（P）、积分（I）和微分（D）三种控制方式，以实现对系统的精确控制。
PID控制的三个主要组成部分：

• 比例控制（P）：比例控制是根据误差的大小来调整控制器输出。误差越大，输出调整幅度越大。这种控制方式有助于快速地减小误差，但可能导致系统在设定点附近来回波动，无法消除静差。
• 积分控制（I）：积分控制是根据误差累积的总量来调整控制器输出。当系统存在持续的静差时，积分控制有助于消除这种误差，使系统最终达到设定点。但过度的积分控制可能导致系统响应变慢或发生过冲。
• 微分控制（D）：微分控制是根据误差变化的速度来调整控制器输出。当误差变化速度较快时，微分控制可以抑制系统过冲和振荡。但微分控制对噪声敏感，可能导致输出不稳定。
  

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

[img=22618,2659]https://lceda001.feishu.cn/space/api/box/stream/download/asynccode/?code=YzNiZTQxYTVmMGVmMmU3ZDBiNWU1MGJlZDAyYjc2NmFfYkFYUTdOZ0wzQUs3azhzUm5LSHFxWERXTmRNOFQ3VGNfVG9rZW46V0Y5U2JvY1NDb3U4dGV4SmJsSmNPN0JtbmViXzE2OTAyNTAwMDA6MTY5MDI1MzYwMF9WNA[/img]

其中

• Kp是比例控制
• Ki是积分控制
• Kd是微分控制
• e是误差
• t:目前时间
• 丆：是积分变数
  

上面的式子是连续型的PID公式，但是在嵌入式系统中是不可能连续的，所有的都是数字的离散量。针对PID计算来说，都是间隔一定时间去计算的，改变的只是间隔时间的大小。
离散型的位置式PID和增量式PID公式是将连续时间域的PID公式转换为离散时间域，以便于在数字控制系统中实现。以下是离散型的位置式PID和增量式PID公式以及解释。
离散型位置式PID公式离散型位置式PID（绝对式PID）公式如下：

1. 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）公式如下：

1. Δ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，来达到所需的速度。
简要步骤如下：

• 初始化参数：首先将Kp，Ki，Kd全部设置为0，这样PID控制器不产生任何控制作用。
• 调节比例增益Kp：逐渐增加Kp的值，直到系统开始出现持续的振荡（速度在设定值附近上下波动）。此时记录下Kp的值。
• 调节积分增益Ki：将Kp设置为刚刚Kp值的一半，然后逐渐增加Ki的值，直到系统的静态误差（速度与设定值的差距）变得很小。过大的Ki值可能导致系统响应过慢或振荡，此时需要适当减小Ki值。
• 调节微分增益Kd：保持当前的Kp和Ki值不变，逐渐增加Kd的值，直到系统的动态性能满足要求（例如，要求速度在设定值附近的波动范围和响应时间）。过大的Kd值可能导致系统响应过于敏感或噪声敏感，此时需要适当减小Kd值。
• 微调参数：以上步骤得到的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控制可以分为两个层级：

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

在平衡小车的串级PID控制中，首先需要调整内环控制器的参数，以保证车体能够迅速地恢复到竖直状态。接下来，调整外环控制器的参数，以实现对车体位置的控制。在调整参数的过程中，需要关注小车的稳定性、响应速度、过冲等性能指标。
串级PID控制可以提高系统的稳定性，他可以确保外环期望的值可以稳定执行，比如说如果平衡小车只有外环控制的话，他输出的PWM值太小的话很有可能会导致小车根本无法克服摩擦力运动，如果他把这个速度值给到内环控制后，就由内环来确保小车以这个速度来执行。在送药小车的电机控制中，速度环就是内环，外环可以是位置环，寻红线环，角度环等。
实现PID的程序代码在这里实现了位置式PID的程序，它在仓库里的2_code->applications->module->pid里面。
.H文件

1. #ifndef _POSITIONAL_PID_H
   
2. #define _POSITIONAL_PID_H
   
3. 
   
4. #include "stdint.h"
   
5. 
   
6. typedef enum {
   
7.     PID_DISABLE, /* PID失能 */
   
8.     PID_ENABLE, /* PID使能 */
   
9. } positional_pid_status;
   
10. 
    
11. typedef struct _positional_pid_params_t{
    
12. 
    
13.     char control;
    
14. 
    
15.     float kp;
    
16.     float ki;
    
17.     float kd;
    
18. 
    
19.     float target;
    
20.     float measure;
    
21.     float last_measure;
    
22. 
    
23.     float error;
    
24.     float last_error;
    
25. 
    
26.     float p_out;
    
27.     float i_out;
    
28.     float d_out;
    
29. 
    
30.     float output;
    
31.     float last_output;
    
32. 
    
33.     float output_max;
    
34.     float output_min;
    
35. 
    
36.     float integral_limit;
    
37. 
    
38.     float dead_zone;
    
39. 
    
40.     void (*positional_pid_params_init)(
    
41.         struct _positional_pid_params_t *positional_pid, float kp, float ki,
    
42.         float kd, float dead_zone, float output_max, float output_min);
    
43.     void (*positional_pid_set_value)(
    
44.         struct _positional_pid_params_t *positional_pid, float kp, float ki,
    
45.         float kd);
    
46.     void (*positional_pid_control)(
    
47.         struct _positional_pid_params_t *positional_pid,
    
48.         positional_pid_status status);
    
49. } positional_pid_params_t;
    
50. 
    
51. void positional_pid_init(positional_pid_params_t *positional_pid, float kp,
    
52.                          float ki, float kd,
    
53. 
    
54.                          float dead_zone, float output_max, float output_min);
    
55. float positional_pid_compute(positional_pid_params_t *positional_pid,
    
56.                             float target, float measure);
    
57. 
    
58. #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文件

1. #include "positional_pid.h"
   
2. #include <math.h>
   
3. 
   
4. #define ABS(x) ((x > 0) ? x : -x)
   
5. 
   
6. static void positional_pid_params_init(positional_pid_params_t *positional_pid,
   
7.                                   float kp, float ki, float kd,
   
8. 
   
9.                                   float dead_zone, float output_max,
   
10.                                   float output_min)
    
11. {
    
12.     // 初始化 PID 参数
    
13.     positional_pid->kp = kp;
    
14.     positional_pid->ki = ki;
    
15.     positional_pid->kd = kd;
    
16. 
    
17.     // 初始化死区、输出上限和输出下限
    
18.     positional_pid->dead_zone = dead_zone;
    
19.     positional_pid->output_max = output_max;
    
20.     positional_pid->output_min = output_min;
    
21. 
    
22.     // 初始化目标值和输出值
    
23.     positional_pid->target = 0;
    
24.     positional_pid->output = 0;
    
25. }
    
26. 
    
27. static void positional_pid_set_value(positional_pid_params_t *positional_pid,
    
28.                                  float kp, float ki, float kd)
    
29. {
    
30.     // 设置 PID 参数
    
31.     positional_pid->kp = kp;
    
32.     positional_pid->ki = ki;
    
33.     positional_pid->kd = kd;
    
34. }
    
35. 
    
36. float positional_pid_compute(positional_pid_params_t *positional_pid,
    
37.                              float target, float measure)
    
38. {
    
39.     if (positional_pid->control == PID_ENABLE)
    
40.     {
    
41.         // 设置目标值和测量值
    
42.         positional_pid->target = target;
    
43.         positional_pid->measure = measure;
    
44. 
    
45.         // 计算误差
    
46.         positional_pid->error =
    
47.             positional_pid->target - positional_pid->measure;
    
48. 
    
49.         if (ABS(positional_pid->error) > positional_pid->dead_zone)
    
50.         {
    
51.             // 计算比例项
    
52.             positional_pid->p_out = positional_pid->kp * positional_pid->error;
    
53.             // 计算积分项
    
54.             positional_pid->i_out += positional_pid->ki * positional_pid->error;
    
55.             // 计算微分项
    
56.             positional_pid->d_out =
    
57.                 positional_pid->kd
    
58.                 * (positional_pid->error - positional_pid->last_error);
    
59. 
    
60.             // 计算总输出
    
61.             positional_pid->output = positional_pid->p_out
    
62.                                      + positional_pid->i_out
    
63.                                      + positional_pid->d_out;
    
64.         }
    
65. 
    
66.         // 限制输出在输出上限和输出下限之间
    
67.         if (positional_pid->output > positional_pid->output_max)
    
68.         {
    
69.             positional_pid->output = positional_pid->output_max;
    
70.         }
    
71.         if (positional_pid->output < (positional_pid->output_min))
    
72.         {
    
73.             positional_pid->output = positional_pid->output_min;
    
74.         }
    
75. 
    
76.         // 更新上一次测量值、输出值和误差值
    
77.         positional_pid->last_measure = positional_pid->measure;
    
78.         positional_pid->last_output = positional_pid->output;
    
79.         positional_pid->last_error = positional_pid->error;
    
80. 
    
81.         return positional_pid->output;
    
82.     }
    
83.     else
    
84.     {
    
85.         return 0.0f;
    
86.     }
    
87. }
    
88. 
    
89. void positional_pid_control(positional_pid_params_t *positional_pid,positional_pid_status status)
    
90. {
    
91.     // 控制 PID 的使能状态
    
92.     positional_pid->control = status;
    
93. }
    
94. 
    
95. void positional_pid_init(positional_pid_params_t *positional_pid, float kp,
    
96.                          float ki, float kd,
    
97. 
    
98.                          float dead_zone, float output_max, float output_min)
    
99. {
    
100.     // 初始化 PID 控制器
     
101.     positional_pid->positional_pid_params_init = positional_pid_params_init;
     
102.     positional_pid->positional_pid_set_value = positional_pid_set_value;
     
103.     positional_pid->positional_pid_control = positional_pid_control;
     
104.     // 调用初始化函数设置参数
     
105.     positional_pid->positional_pid_params_init(
     
106.         positional_pid, kp, ki, kd, dead_zone, output_max, output_min);
     
107.     // 默认使能 PID 控制器
     
108.     positional_pid->control = PID_ENABLE;
     
109. }

复制代码

• positional_pid_params_init 初始化 PID 控制器的参数.传入的参数值会被分别赋值到相应的结构体成员变量中，包括增益值、死区大小、输出上限和下限等。
• positional_pid_set_value 设置 PID 控制器的增益参数。传入的参数值（kp、ki、kd）会被分别赋值到相应的结构体成员变量中。
• positional_pid_compute 计算 PID 控制器的输出。是 PID 控制器的核心计算逻辑。首先，根据传入的目标值 target 和实际测量值 measure，计算当前误差 error 和上一次误差 last_error。然后，根据误差和增益参数，分别计算比例输出、积分输出和微分输出。接下来，将这三个输出相加得到总输出，再根据输出限制范围（output_max 和 output_min）对输出进行限幅。最后，将一些必要的值存储到结构体中，以便下次计算使用，并返回当前输出。
• positional_pid_control 用于控制 PID 控制器的启用和禁用状态。传入的参数 control 表示控制器的状态。如果设置为 PID_DISABLE，则将输出设置为 0。
• positional_pid_init 初始化 PID 控制器，设置函数指针和调用初始化函数（将函数指针分别指向相应的函数实现）并默认使能PID控制器。
  

如何实现单个电机的速度和位置环这个问题是一个非常关键的问题。这是因为小车电机的速度直接影响了小车的行驶速度和精度，而行驶速度和精度又是小车比赛中的关键因素之一。如果小车电机的速度不稳定，那么小车的行驶速度和精度就会受到影响，导致小车无法在比赛中达到最佳表现。因此，在进行智能车比赛之前，需要先解决小车电机速度问题，保证小车电机的速度稳定、准确，并且能够根据需要进行调整。并不是说给一个确定的电压或者确定的PWM值就能让电机保持到一个准确的转速，比如每个电机的绕线，轴承，机械性能，负载能力，各个接触点的摩擦力这些都有差别，小车轮子所受到的摩擦力的不同也会影响小车的前进速度。
结合之前的文档06_如何计算小车轮子转速.md和07_电机驱动-舵机-蜂鸣器PWM配置.md我们现在已经可以获取到轮子转速和改变PWM值从而驱动电机转动。通过编码器我们可以进行电机的转速测量，通过改变PWM值我们可以让电机提高或者降低转速。在这个条件下，我们的目标值是让小车电机稳定在一个转速，测量值是小车当前由编码器测量得到的实际速度，输出值是电机的PWM值。通过调节PID的三个参数，让目标值变化时，实际值可以又快又准的接近目标值。
简单来说，PID就是测量出实际值，设定一个目标值，输出一个驱动值，让实际值尽量和目标值靠近。实现单个电机速度环时，
具体实现在仓库里的2_code->applications-pid_thread.c里面。
简要步骤如下：

• 引入必要的头文件。
• 定义需要的速度调节PID变量：positional_pid_params_t motor2_speed_pid;和实际的电机速度float motor1_speed_target = 0.0f;
• 通过uMCN订阅发布机制申明编码器1的信息：MCN_DECLARE(encoder_m1_topic);
• 创建一个PID线程，初始化速度环的PID配置：
  

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

复制代码

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

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

复制代码

---

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

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

1. position_result_motor1_speed_target = (positional_pid_compute(
   
2.     &motor1_position_pid, (float)motor1_position_target,
   
3.     (float)position1_measurement));

复制代码

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

1. void pid_test(int argc, char **argv)
   
2. {
   
3.     static float temp_speed;
   
4.     static uint32_t temp_delay_ms = 10;
   
5.     // pid_test speed forward|backward|left|right  time
   
6.     /* 检查输入的变量是否有四个 */
   
7.     if (argc < 4)
   
8.     {
   
9.         rt_kprintf("Please input'pid_test speed <forward|backward|left|right> "
   
10.                    "speed_value <time>'\n");
    
11.         return;
    
12.     }
    
13.     if (!rt_strcmp(argv[1], "speed"))
    
14.     {
    
15.         if (!rt_strcmp(argv[2], "forward"))
    
16.         {
    
17.             motor1_speed_target = atof(argv[3]);
    
18.             motor2_speed_target = atof(argv[3]);
    
19.             rt_thread_mdelay(atoi(argv[4]));
    
20.             motor1_speed_target = 0.0f;
    
21.             motor2_speed_target = 0.0f;
    
22.         }
    
23.         else if (!rt_strcmp(argv[2], "backward"))
    
24.         {
    
25.             motor1_speed_target = -atof(argv[3]);
    
26.             motor2_speed_target = -atof(argv[3]);
    
27.             rt_thread_mdelay(atoi(argv[4]));
    
28.             motor1_speed_target = 0.0f;
    
29.             motor2_speed_target = 0.0f;
    
30.         }
    
31.         else if (!rt_strcmp(argv[2], "left"))
    
32.         {
    
33.             motor1_speed_target = atof(argv[3]);
    
34.             motor2_speed_target = -atof(argv[3]);
    
35.             rt_thread_mdelay(atoi(argv[4]));
    
36.             motor1_speed_target = 0.0f;
    
37.             motor2_speed_target = 0.0f;
    
38.         }
    
39.         else if (!rt_strcmp(argv[2], "right"))
    
40.         {
    
41.             motor1_speed_target = -atof(argv[3]);
    
42.             motor2_speed_target = atof(argv[3]);
    
43.             rt_thread_mdelay(atoi(argv[4]));
    
44.             motor1_speed_target = 0.0f;
    
45.             motor2_speed_target = 0.0f;
    
46.         }
    
47.     }
    
48.     if (!rt_strcmp(argv[1], "position"))
    
49.     {
    
50.         if (!rt_strcmp(argv[2], "go"))
    
51.         {
    
52.             car_go_position(atoi(argv[3]));
    
53. 
    
54.             temp_speed = 0;
    
55.             while (temp_speed <= atof(argv[4]))
    
56.             {
    
57.                 temp_speed += 0.01f;
    
58.                 set_car_position_max_speed(temp_speed);
    
59.                 rt_thread_mdelay(temp_delay_ms);
    
60.             }
    
61.         }
    
62.         else if (!rt_strcmp(argv[2], "spin"))
    
63.         {
    
64.             car_spin_position(atoi(argv[3]));
    
65.         }
    
66.     }
    
67.     if (!rt_strcmp(argv[1], "angle"))
    
68.     {
    
69.         if (!rt_strcmp(argv[2], "pid"))
    
70.         {
    
71.             car_find_lines_pid.Kp = atof(argv[3]);
    
72.             car_find_lines_pid.Ki = atof(argv[4]);
    
73.             car_find_lines_pid.Kd = atof(argv[5]);
    
74.             car_find_lines_pid.MaxOut = atof(argv[6]);
    
75.         }
    
76.         else if (!rt_strcmp(argv[2], "absolute"))
    
77.         {
    
78.             turn_target = atof(argv[3]);
    
79.             if (turn_target >= 180)
    
80.             {
    
81.                 turn_target = 180;
    
82.             }
    
83.             else if (turn_target <= -180)
    
84.             {
    
85.                 turn_target = -180;
    
86.             }
    
87.         }
    
88.         else if (!rt_strcmp(argv[2], "run"))
    
89.         {
    
90.             raw_target_speed = atof(argv[3]);
    
91.             rt_thread_mdelay(atoi(argv[4]));
    
92.             raw_target_speed = 0;
    
93.         }
    
94.     }
    
95.     else
    
96.     { /* 输入的是其他内容 */
    
97.         rt_kprintf("Please input'pid_test speed <forward|backward|left|right> "
    
98.                    "speed_value <time>'\n");
    
99.     }
    
100. }
     
101. MSH_CMD_EXPORT(pid_test, pid test sample
     
102.                : servo_test<servo1 | servo2><pulse(32 - 150)>);

复制代码

当然，想快一点的话就直接用MDK进入硬件仿真，直接把具体的变量添加到Watch窗口进行修改，下面就直接用进入硬件调试直接修改变量的方法来调试
以调试单电机速度环为例：这里的测量值是小车当前由编码器测量得到的实际速度，输出值是电机的PWM值。通过调节PID的三个参数，让目标值变化时，实际值可以又快又准的接近目标值。
我们可以在pid线程中加入如下代码，通过串口打印出轮子1的设定速度和正交编码器1测出来的轮子转速。

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

复制代码

在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测出来的累积脉冲。

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

复制代码

在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是偏小的，主要也是为了让轮子的速度变化小一点，平缓一些，防止打滑情况的出现。