嵌入式开发

本文基于 STM32F103CBT6 和 TB6612 驱动芯片驱动趣轮科技 MG513P30_12V 直流电机，并使用 PID 算法进行闭环控制

直流电机驱动

驱动芯片

具体芯片使用请看 这篇

驱动代码

驱动代码要讲的部分不多，注释里已经基本全了，Motor_SetSpeed 之所以输入范围是 $[-1, 1]$ 是对齐 PID 控制的输出

默认将 STBY 引脚直接接入 3.3v 所以不需要使能 / 失能函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
/**
 * @file motor.h
 * @brief 直流电机控制驱动
 * @author hanjin
 * @date 2024-10-14
 * @version v1.0
 */

#ifndef MOTOR_H
#define MOTOR_H

#include "main.h"
#include "tim.h"
#define MAX_COMPARE     (1000U - 1U)    ///< 其实就是自动重载值

#define ROTO_RADIO      (4 * 13)        ///< 倍频器*线数
#define REDUCTION_RADIO (30)            ///< 减速比 30:1
#define MAX_SPEED       (366)           ///< 最大转速(rpm)

#define SAMPLING_PERIOD (1e-2)          ///< 采样间隔(s)

/**
 * @def __SPEED2COMPARE
 * @brief 将速度转化为CCR
 * @attention 结果四舍五入
 * @param SPEED 速度 [0, 1]
 */
#define __SPEED2CCR(SPEED) (uint16_t)((SPEED) * (MAX_COMPARE) + 0.5)

typedef struct
{
    TIM_HandleTypeDef* const htim;
    const uint32_t channel;
} TIM_Channel_t;

typedef struct
{
    GPIO_TypeDef* const GPIOx;
    const uint16_t GPIO_Pin;
} GPIO_Out_t;

// TODO: 直接用speed正负来表示方向 < 但是也许从实用的角度不需要？
typedef struct
{
    const TIM_Channel_t pwm;
    // const TIM_Channel_t encoder1, encoder2;
    TIM_HandleTypeDef* const encoder;
    const GPIO_Out_t in1, in2;
    uint8_t direction; ///< 方向：0 正向 1 反向
    float speed; ///< 速度：0 <= speed <= 1

    float real_speed; ///< 由编码器测得的速度
    float real_round; ///< 由编码器测得的圈数
} Motor_t;

// void Motor_Enable();
// void Motor_Disable();

void Motor_Start(const Motor_t* hmotor);
void Motor_Stop(const Motor_t* hmotor);
void Motor_SetSpeed(Motor_t* hmotor, float speed);
void Motor_SetDirection(Motor_t* hmotor, uint8_t direction);
void Motor_ToggleDirection(Motor_t* hmotor);

void Encoder_Start(Motor_t* hmotor);
void Encoder_Stop(Motor_t* hmotor);
void Encoder_Progress(Motor_t* hmotor);
float Encoder_GetSpeed(const Motor_t* hmotor);
#endif

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/**
 * @file motor.c
 * @brief 直流电机控制驱动
 * @author hanjin
 * @date 2024-10-14
 * @version v1.0
 */

#include "motor.h"
#include <stdio.h>

/**
 * @brief 应用方向变更，也可以用于启动
 * @note 为啥是inline呢，因为我只是不想写两遍罢了
 * @param hmotor
 * @retval None
 */
inline void _Perform_SetDirection(const Motor_t* hmotor)
{
    if (hmotor->direction == 0) // 正向旋转
    {
        HAL_GPIO_WritePin(hmotor->in1.GPIOx, hmotor->in1.GPIO_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(hmotor->in2.GPIOx, hmotor->in2.GPIO_Pin, GPIO_PIN_SET);
    } else
    {
        HAL_GPIO_WritePin(hmotor->in1.GPIOx, hmotor->in1.GPIO_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(hmotor->in2.GPIOx, hmotor->in2.GPIO_Pin, GPIO_PIN_RESET);
    }
}

/**
 * @brief 使能驱动芯片
 * @note 打算不使用STBY引脚，想直接给他接3v3
 */
// void Motor_Enable()
// {
//     HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_SET);
// }

/**
 * @brief 失能驱动芯片
 */
// void Motor_Disable()
// {
//     HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_SET);
// }

/**
 * @brief 启动电机
 * @attention 本函数不会设置速度，所以会按照原本速度运行
 * @param hmotor 电机
 * @retval None
 */
void Motor_Start(const Motor_t* hmotor)
{
    _Perform_SetDirection(hmotor);
    __HAL_TIM_SET_COMPARE(hmotor->pwm.htim, hmotor->pwm.channel, __SPEED2CCR(hmotor->speed)); // 根据速度设置占空比
    HAL_TIM_PWM_Start(hmotor->pwm.htim, hmotor->pwm.channel);
}

/**
 * @brief 停止电机
 * @param hmotor
 */
void Motor_Stop(const Motor_t* hmotor)
{
    HAL_TIM_PWM_Stop(hmotor->pwm.htim, hmotor->pwm.channel);
    HAL_GPIO_WritePin(hmotor->in1.GPIOx, hmotor->in1.GPIO_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(hmotor->in2.GPIOx, hmotor->in2.GPIO_Pin, GPIO_PIN_RESET);
}

/**
 * @brief 设置速度
 * @param hmotor
 * @param speed -1<= speed <= 1
 */
inline void Motor_SetSpeed(Motor_t* hmotor, float speed)
{
    if (speed > 0 && hmotor->direction == 1)
        Motor_SetDirection(hmotor, 0);
    else if (speed < 0 && hmotor->direction == 0)
        Motor_SetDirection(hmotor, 1);
    if (speed < 0) speed = -speed;
    hmotor->speed = speed;
    __HAL_TIM_SET_COMPARE(hmotor->pwm.htim, hmotor->pwm.channel, __SPEED2CCR(speed));
}

/**
 * @brief 设置方向
 * @param hmotor
 * @param direction 0 正转  1 反转
 */
void Motor_SetDirection(Motor_t* hmotor, const uint8_t direction)
{
    hmotor->direction = direction;
    _Perform_SetDirection(hmotor);
}

/**
 * @brief 切换方向
 * @param hmotor
 */
inline void Motor_ToggleDirection(Motor_t* hmotor)
{
    Motor_SetDirection(hmotor, !hmotor->direction);
}

/**
 * @brief 启动编码器
 * @param hmotor
 */
inline void Encoder_Start(Motor_t* hmotor)
{
    HAL_TIM_Encoder_Start(hmotor->encoder, TIM_CHANNEL_ALL);
}

/**
 * @brief 关闭编码器
 * @param hmotor
 */
inline void Encoder_Stop(Motor_t* hmotor)
{
    HAL_TIM_Encoder_Stop(hmotor->encoder, TIM_CHANNEL_ALL);
}

/**
 * @brief 在定时器中处理编码器的数据
 * @note 双边沿计数，TI1 and TI2
 * @param hmotor
 */
void Encoder_Progress(Motor_t* hmotor)
{
    // 此处需要保证 counter < 32768 正常电机转不了那么快
    const int16_t counter = __HAL_TIM_GET_COUNTER(hmotor->encoder);
    // printf("%d", counter);
    hmotor->real_round += (float)counter / (ROTO_RADIO * REDUCTION_RADIO); // 转过的圈数
    hmotor->real_speed = (float)counter / (ROTO_RADIO * REDUCTION_RADIO) / SAMPLING_PERIOD * 60; // 实际转速(rpm)
    __HAL_TIM_SET_COUNTER(hmotor->encoder, 0);
}

/**
 * @brief 获取电机速度
 * @param hmotor
 * @return 电机速度(rpm)
 */
inline float Encoder_GetSpeed(const Motor_t* hmotor)
{
    return hmotor->real_speed;
}

Motor_t 类型变量定义示例

1
2
3
4
5
6
7
Motor_t motor = {
  {&htim4, TIM_CHANNEL_1}, ///< PWM信号 {定时器, 通道}
  &htim2, ///< 编码器使用的定时器
  {MOTORA_IN1_GPIO_Port, MOTORA_IN1_Pin}, ///< 电机输入IN1 {GPIOx, Pin}
  {MOTORA_IN2_GPIO_Port, MOTORA_IN2_Pin}, ///< 电机输入IN2 {GPIOx, Pin}
  0, 0, 0, 0 ///< 方向，速度（0~1)，编码器测得的速度，编码器测得的圈数
};

PID 控制

理论基础 / 参考资料

实践开始

要完成 PID 控制，我们需要有反馈输入 feedback，并拥有预设定的目标值 target，然后根据 PID 算法计算输出 output

首先，我们假定

  • 输入范围不限，因为 PID 的输入可能是 圈数、速度 等很多东西
  • 输出有效范围在 $[0,1]$，因为我们的 PID 输出应当作为电机控制的输入（在上面的电机控制驱动中指的是 Motor_SetSpeed 函数的输入）

由一大坨理论（从 SSC 的 PID 控制教程中）得到增量式 PID 计算公式

$$\Delta u(n) = u(n) - u(n-1) = K_P(e(n)-e(n-1)) + K_I e(n) + K_D(e(n)-2e(n-1)+e(n-2)) $$

可见增量 $\Delta u(n)$ 只与最近三次误差值有关，故我们只需记录最近三个误差值 error, error_last1, error_last2

注意：PID 计算函数应当是周期性的，因为单片机中使用的是离散型 PID 系统

离散 PID 系统存在控制频率要求，实际使用时需要在硬件定时器中使用，周期越小，要求系统抗噪声能力较强，同时参数值较小

根据上面一坨东西，我们可以得到初步的 PID 计算函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
float PID_Calculate(PID_t* hpid, const float feedback)
{
    /* 计算误差值 */
    hpid->error = hpid->target - feedback;
    
    /* 计算增量 */
    /* 比例部分 */
    float p = hpid->Kp * (hpid->error - hpid->error_last1);

    /* 积分部分 */
    float i = hpid->Ki * hpid->error;

    /* 微分部分 */
    float d = hpid->Kd * (hpid->error - 2 * hpid->error_last1 + hpid->error_last2);
    
    hpid->output += p + i + d;

    // 更新误差值
    hpid->error_last2 = hpid->error_last1;
    hpid->error_last1 = hpid->error;
    
    return hpid->output;
}

但是很快我们会发现一些奇奇怪怪的问题，比如

  • 我在设置目标值时系统一下子就跑飞了，油门轰轰地响，半天停不下来，都要开到华盛顿去了
  • 怎么输出老是在目标附近摇摆不定做类简谐运动，让我回忆起我美好的高中生活
  • 速度环为啥老是波动不能固定？这个位置环到位了能不能就别动来动去了
  • 它怎么总是先飞过去再飞回来
  • 臣卜木曹为啥它做简谐运动振幅还越来越大，我草要飞了！
  • 我趣它为啥非要先反着转然后再正着转过去？

显然，我们需要一定的优化，比如

  1. 限制输出范围，或者叫抗饱和

    将 output 的值限制在有效范围内，避免一直处于超调区，系统能够更快响应

    1
    2
    3
    4
    5
    /* 抗饱和：限制范围，防止跑飞，同时限制速度 */
    if (hpid->output > hpid->output_abs_max)
        hpid->output = hpid->output_abs_max;
    else if (hpid->output < -hpid->output_abs_max)
        hpid->output = -hpid->output_abs_max;

  2. 加一些低通滤波器器，避免高频噪声（同时也避免系统输出剧烈变化）

    1
    2
    3
    4
    5
    // 对微分部分进行低通滤波
    hpid->filtered_d = hpid->filtered_d * dfilter + d * (1 - dfilter);

    // 对输出进行低通滤波
    hpid->output += du * (1 - ufilter);

  3. 对积分部分添加死区

    通常在非常接近目标值时输出的波动都是由积分部分引起的，如果所谓的「非常接近」已经达到精度需求，则完全可以在这一精度范围内舍弃微分部分以避免波动

    1
    2
    float i = 0;
    if (hpid->error > hpid->ideadzone || hpid->error < -hpid->ideadzone) i = Ki * hpid->error;

  4. 使用模糊 PID 控制

    往往一组 PID 参数不能满足动态过程的所有需求，所以就有了动态改变 PID 参数的模糊 PID 算法。

    由于我还不会模糊 PID 控制，故采用下位方案：将控制过程分为两部分，以 $|error|$ 作为分界值 $cutoff$，使用两组不同的 PID 参数。这两组参数应当分别具有以下特性

    • 初始参数应当让响应速度尽可能快，以缩短调节时间
    • 末尾参数应当尽可能将系统控制在目标附近，以获得更高精度
    1
    2
    3
    4
    5
    float Kp, Ki, Kd, ufilter, dfilter;
    if (hpid->error < hpid->cutoff && hpid->error > -hpid->cutoff)
        Kp = hpid->KpE, Ki = hpid->KiE, Kd = hpid->KdE, ufilter = hpid->ufilterE, dfilter = hpid->dfilterE;
    else
        Kp = hpid->KpS, Ki = hpid->KiS, Kd = hpid->KdS, ufilter = hpid->ufilterS, dfilter = hpid->dfilterS;

另外，对于以上几个奇怪的问题的原因，可能要从参数入手

注意：以下参数方面的原因判断不一定准确，建议自己调试总结

  • 类简谐运动大概是由于积分部分和微分部分的超调，加大比例部分参数压住即可（或者减小积分部分）
  • 先反转可能是微分部分设大了，减小微分部分参数即可

速度环的波动是正常的，因为即使一直给电机供一样的电，电机的速度也会发生变化，速度环在一定范围内的波动是正常的。

驱动代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
/**
 * @file pid.h
 * @brief PID 驱动文件
 * @author hanjin
 * @date 2024-10-14
 * @version v0.3.2
 */

#ifndef PID_H
#define PID_H

#define PID_VERSION "0.3.2"

#include "main.h"


// 增量式PID
typedef struct
{
    uint8_t enable;
    float target; ///< PID控制要求输入值的目标
    float output_abs_max; ///< PID输出绝对值最大值，防止跑飞
    float cutoff; ///< 分界值
    /* v3.0 将pid控制分段 */
    // error < 5 时的参数
    float KpE, KiE, KdE; ///< 比例系数 积分系数 微分系数
    float dfilterE; ///< 微分部分低通滤波系数，越大越平滑
    float ufilterE; ///< 输出值滤波
    /* v3.1 增加死区大小，用于位置环稳定 */
    float ideadzone; ///< 积分部分死区大小
    // error >=5 时的参数
    float KpS, KiS, KdS;
    float ufilterS, dfilterS;
    // 运行过程中间量
    float error_last1, error_last2, error; ///< 前一次误差 前前次误差 本次误差
    float output; ///< PID控制输出值
    float filtered_d; ///< 微分部分经过滤波后的值
} PID_t;

/**
 * @brief 设置PID控制的目标
 * @param __PID_HANDLE__ PID对象的指针
 * @param __TARGET__ 目标
 */
#define __PID_SET_TARGET(__PID_HANDLE__, __TARGET__) ((__PID_HANDLE__)->target = (__TARGET__))

/**
 * @brief 重置PID过程量
 * @note added 0.3.2
 * @param __PID_HANDLE__
 */
#define __PID_RESET(__PID_HANDLE__) \
    do { \
        (__PID_HANDLE__)->error = 0; \
        (__PID_HANDLE__)->error_last1 = 0; \
        (__PID_HANDLE__)->error_last2 = 0; \
        (__PID_HANDLE__)->output = 0; \
    } while(0)


void PID_Init(PID_t* hpid,
              float KpS, float KiS, float KdS, float ufilterS, float dfilterS,
              float KpE, float KiE, float KdE, float ufilterE, float dfilterE, float ideadzone,
              float output_abs_max, float cutoff, float target
);
float PID_Calculate(PID_t* hpid, const float feedback);
#endif

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
/**
 * @file pid.c
 * @brief PID 驱动文件
 * @author hanjin
 * @date 2024-10-14
 * @version v0.3.2
 */

#include "pid.h"

/**
 * @brief PID初始化
 * @attention 由于输出在 -1 ~ 1 之间，所以参数需要 / MAX_INPUT 来对齐阶数 < 只是猜测，具体需要实践
 * @param hpid PID对讲指针
 * @param KpS 初始比例系数
 * @param KiS 初始积分系数
 * @param KdS 初始微分系数
 * @param ufilterS 初始输出滤波系数
 * @param dfilterS 初始微分部分滤波系数
 * @param KpE 末尾比例系数
 * @param KiE 末尾积分系数
 * @param KdE 末尾微分系数
 * @param ufilterE 末尾输出滤波系数
 * @param dfilterE 末尾微分部分滤波系数
 * @param ideadzone 积分部分死区大小
 * @param output_abs_max 输出绝对值最大限制
 * @param cutoff 初始与末尾分隔量 abs(error) < cutoff 视为末尾状态
 * @param target PID控制目标
 */
void PID_Init(PID_t* hpid,
              float KpS, float KiS, float KdS, float ufilterS, float dfilterS,
              float KpE, float KiE, float KdE, float ufilterE, float dfilterE, float ideadzone,
              float output_abs_max, float cutoff, float target
)
{
    hpid->target = target;
    hpid->output_abs_max = output_abs_max;
    hpid->cutoff = cutoff;

    hpid->KpS = KpS, hpid->KiS = KiS, hpid->KdS = KdS, hpid->ufilterS = ufilterS, hpid->dfilterS = dfilterS;
    hpid->KpE = KpE, hpid->KiE = KiE, hpid->KdE = KdE, hpid->ufilterE = ufilterE, hpid->dfilterE = dfilterE, hpid->ideadzone = ideadzone;
}


/**
 * @brief 计算pid输出
 * @note 理论上各种地方都能用不是么
 * @param hpid PID对象
 * @param feedback 输入值
 * @return -1 ~ 1
 */
float PID_Calculate(PID_t* hpid, const float feedback)
{
    // 计算误差值
    // hpid->error = input - hpid->target;
    hpid->error = hpid->target - feedback;
    // 计算增量
    float Kp, Ki, Kd, ufilter, dfilter;
    if (hpid->error < hpid->cutoff && hpid->error > -hpid->cutoff)
        Kp = hpid->KpE, Ki = hpid->KiE, Kd = hpid->KdE, ufilter = hpid->ufilterE, dfilter = hpid->dfilterE;
    else
        Kp = hpid->KpS, Ki = hpid->KiS, Kd = hpid->KdS, ufilter = hpid->ufilterS, dfilter = hpid->dfilterS;
    /* 比例部分 */
    float p = Kp * (hpid->error - hpid->error_last1);

    /* 积分部分 */
    float i = 0;
    if (hpid->error > hpid->ideadzone || hpid->error < -hpid->ideadzone) i = Ki * hpid->error;

    /* 微分部分 */
    float d = Kd * (hpid->error - 2 * hpid->error_last1 + hpid->error_last2);
    // 对微分部分进行低通滤波
    hpid->filtered_d = hpid->filtered_d * dfilter + d * (1 - dfilter);

    float du = p + i + hpid->filtered_d;

    hpid->output += du * (1 - ufilter);

    /* 抗饱和：限制范围，防止跑飞，同时限制速度 */
    if (hpid->output > hpid->output_abs_max)
        hpid->output = hpid->output_abs_max;
    else if (hpid->output < -hpid->output_abs_max)
        hpid->output = -hpid->output_abs_max;

    // 更新误差值
    hpid->error_last2 = hpid->error_last1;
    hpid->error_last1 = hpid->error;
    return hpid->output;
}

使用电机驱动和 PID 控制驱动

CubeMX 配置

时钟

HLCK 配置为 72 MHz

TIM1：预分配 72-1，自动重装载 10000-1。提供间隔 10ms 的定时中断

TIM2/TIM3：Combined Channels 配置为 Encoder Mode，并将 Encoder Mode 改为 Encoder Mode TI1 and TI2，其余默认

TIM4：预分频 72-1，自动重装载值 1000-1，并选择两个通道设为 PWM Generation 这里选择 CH1 和 CH2

记的打开定时器的中断

引脚

需要四个引脚，分别具有用户标签 MOTORA_IN1 MOTORA_IN2 MOTORB_IN1 MOTORB_IN2，即两个电机的四个输入

必要代码实现

  1. 定义 Motor_t 类型变量和 PID_t 类型变量

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    Motor_t motors[2] = {
      {
        {&htim4, TIM_CHANNEL_1}, ///< PWM信号 {定时器, 通道}
        &htim2, ///< 编码器使用的定时器
        {MOTORA_IN1_GPIO_Port, MOTORA_IN1_Pin}, ///< 电机输入IN1 {GPIOx, Pin}
        {MOTORA_IN2_GPIO_Port, MOTORA_IN2_Pin}, ///< 电机输入IN2 {GPIOx, Pin}
        0, 0, 0, 0 ///< 方向，速度（0~1)，编码器测得的速度，编码器测得的圈数
      }, {
        {&htim4, TIM_CHANNEL_2}, ///< PWM信号 {定时器, 通道}
        &htim3, ///< 编码器使用的定时器
        {MOTORB_IN1_GPIO_Port, MOTORB_IN1_Pin}, ///< 电机输入IN1 {GPIOx, Pin}
        {MOTORB_IN2_GPIO_Port, MOTORB_IN2_Pin}, ///< 电机输入IN2 {GPIOx, Pin}
        0, 0, 0, 0 ///< 方向，速度（0~1)，编码器测得的速度，编码器测得的圈数
      }
    };
    PID_t PIDs[2];

  2. 在 HAL_TIM_PeriodElapsedCallback 回调中处理编码器数据并执行 PID 计算

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
    {
      if (htim == &htim1)
      {
        /* 编码器采样 */
        Encoder_Progress(motors + 0);
        Encoder_Progress(motors + 1);
        /* PID计算 速度环 */
        if (PIDs[0].enable)
        {
          float pid_output = PID_Calculate(PIDs + 0, motors[0].real_speed);
          Motor_SetSpeed(motors + 0, pid_output);
        }
        if (PIDs[1].enable)
        {
          float pid_output = PID_Calculate(PIDs + 1, motors[1].real_speed);
          Motor_SetSpeed(motors + 1, pid_output);
        }
      }
    }

    此处执行的是速度环控制，如果想要使用位置环，你只需要换一套参数，并将 feedback 的实参改为 motors[x].real_round 即可

  3. 在 USER CODE BEGIN 2 中进行 PID 参数初始化（具体参数自己调去），并使能 TIM1 的定时器中断

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    PID_Init(&PIDs[0],
      , , , , ,
      , , , , , ,
      , , 
    );
    PID_Init(&PIDs[1],
      , , , , ,
      , , , , , ,
      , , 
    );

    HAL_TIM_Base_Start_IT(&htim1);

  4. 在恰当的地方使能编码器和电机

    1
    2
    3
    4
    5
    6
    7
    Motor_Start(motors + 0);
    Encoder_Start(motors + 0);
    PIDs[0].enable = 1;

    Motor_Start(motors + 1);
    Encoder_Start(motors + 1);
    PIDs[1].enable = 1;

注意事项

  1. 电机在 Start 之前或 Stop 之后处于断电状态，如果你希望就让那个电机保持在那里，你应当使用速度环控制并将目标设定为 0（或者使用位置环控制，并让电机位于目标值）
  2. 不要试图自己调用 Motor_SetSpeed 来设置速度，这是一个愚蠢的选择，因为这相当于完全舍弃了 PID 控制，正确的做法是修改 PID 控制的目标值，让 PID 来帮你实现

控制模式切换

「那我要是 TM 的既想要速度环又想要位置环怎么办？」 会有人这样想的

所以我们可以通过一些方法来实现两种控制的切换

  1. 定义一个集成类

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    typedef enum
    {
        SPEED_LOOP = 0U,
        POSITION_LOOP = 1U,
    } WheelState_t;

    typedef struct
    {
        Motor_t motor; ///< 电机类
        PID_t speed_loop, position_loop; ///< 速度环pid和位置环pid
        WheelState_t state; ///< 处于哪种控制模式下
        uint8_t enable; ///< 是否开启
    } Wheel_t;

  2. 计算的时候稍费些功夫

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    /* PID计算 速度环 & 位置环 */
    float pid_output;
    if (wheels[0].enable)
    {
      if (wheels[0].state == SPEED_LOOP)
        pid_output = PID_Calculate(&wheels[0].speed_loop, wheels[0].motor.real_speed);
      else // wheels[0].state == POSITION_LOOP
        pid_output = PID_Calculate(&wheels[0].position_loop, wheels[0].motor.real_round);
      Motor_SetSpeed(&wheels[0].motor, pid_output);
    }
    if (wheels[1].enable)
    {
      if (wheels[1].state == SPEED_LOOP)
        pid_output = PID_Calculate(&wheels[1].speed_loop, wheels[1].motor.real_speed);
      else // wheels[1].state == POSITION_LOOP
        pid_output = PID_Calculate(&wheels[1].position_loop, wheels[1].motor.real_round);
      Motor_SetSpeed(&wheels[1].motor, pid_output);
    }

  3. 然后编写一个控制状态切换函数

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    void switchWheelState(Wheel_t* wheel, WheelState_t newState)
    {
        if (wheel->state == newState) return;
        wheel->state = newState;
        switch (newState)
        {
        case SPEED_LOOP:
            wheel->speed_loop.output = wheel->position_loop.output;
            break;
        case POSITION_LOOP:
            wheel->position_loop.output = wheel->speed_loop.output;
            break;
        }
    }

  4. 如果你在切到位置环的时候不知道速度环到底把电机转到哪了

    你还可以使用之前在 pid.h 中挖下的一个坑来归零

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    /**
     * @brief 重置PID过程量
     * @note added 0.3.2
     * @param __PID_HANDLE__
     */
    #define __PID_RESET(__PID_HANDLE__) \
        do { \
            (__PID_HANDLE__)->error = 0; \
            (__PID_HANDLE__)->error_last1 = 0; \
            (__PID_HANDLE__)->error_last2 = 0; \
            (__PID_HANDLE__)->output = 0; \
        } while(0)

你可以随时切换两种控制方式，获得更自由的控制体验

使用 VOFA+ 提供舒适的 PID 调参体验

VOFA+：插件驱动的高自由度上位机

显然，在代码里修改 PID 参数再重新烧录的调参方式是非常繁琐且低效的，我们需要一个更妙的调参方式，所以想到了 VOFA+ 辅助调参，调参过程中我们默认只对一个电机调参。

代码配置

  1. 对 prinft 输出进行重定向

    1
    2
    3
    4
    5
    6
    /* printf retarget */
    int __io_putchar(int ch)
    {
      HAL_UART_Transmit(&huart1, (uint8_t*)&ch, 1, 0xFFFF);
      return ch;
    }

  2. 向 VOFA+ 发送当前系统的状态

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
      // delay_counter 的作用是在 PID 禁用后仍发送一些数据，带来更好的 VOFA+ 前端体验
      if (PIDs[0].enable || delay_counter)
      {
        printf("%f,%f,%f,%f\n", motors[0].real_round, motors[0].real_speed, PIDs[0].output, PIDs[0].target);
        delay_counter--;
      }
      HAL_Delay(20);
      /* USER CODE END WHILE */
      /* USER CODE BEGIN 3 */
    }
      /* USER CODE END 3 */

  3. 能够接受 VOFA+ 发送的指令

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef* huart)
    {
      if (huart == &huart1)
      {
        /* 校验协议头和协议尾 */
        if (rb[0] == 0xAA && rb[6] == 0xBB) 
        {
          switch (rb[1])
          {
          case 0x11: // 设置目标值
            PIDs[0].target = *(float*)(rb + 2);
            break;
          case 0x22: // 启动电机
            Motor_Start(motors);
            Encoder_Start(motors);
            PIDs[0].enable = 1;
            break;
          case 0x33: // 关闭电机，注意此时我们保证编码器开启，以持续记录电机位置
            Motor_Stop(motors);
            PIDs[0].enable = 0;
            PIDs[0].output = 0;
            delay_counter = 10;
            break;
          case 0x44: // 修改比例系数
            PIDs[0].KpS = *(float*)(rb + 2)
    #ifdef SPEED_LOOP
              / MAX_SPEED
    #endif
              ;
            break;
          case 0x55: // 修改积分系数
            PIDs[0].KiS = *(float*)(rb + 2)
    #ifdef SPEED_LOOP
              / MAX_SPEED
    #endif
              ;
            break;
          case 0x66: // 修改微分系数
            PIDs[0].KdS = *(float*)(rb + 2)
    #ifdef SPEED_LOOP
              / MAX_SPEED
    #endif
              ;
            break;
          case 0x77: // 修改微分低通滤波系数
            PIDs[0].dfilterS = *(float*)(rb + 2);
            break;
          case 0x99: // 修改输出值低通滤波系数
            PIDs[0].ufilterS = *(float*)(rb + 2);
            break;
          default: ;
          }
          printf("ACK: %f,%f,%f,%f,%f,%f\n",
                 PIDs[0].target, PIDs[0].KpS, PIDs[0].KiS, PIDs[0].KdS, PIDs[0].dfilterS, PIDs[0].ufilterS);
        }
        UART_Start_Receive_IT(&huart1, rb, 7);
      }
    }

    注意

    1. 这里调试的都是开始的 PID 参数，如果想要调试末尾的 PID 参数，请将后缀 S 改为 E

    2. 在位置环调试模式下之所以 / MAX_SPEED 是因为电机的速度范围为 $[-366, 366]$，而 PID 输出范围为 $[-1, 1]$，需要对齐阶数

  4. 记得在 main 函数里开启第一次接收

VOFA+ 配置

  1. 在 VOFA+ 里新建一个命令组，添加一些命令

    • Start：Hex AA 22 FF FF FF FF BB
    • Stop：Hex AA 33 FF FF FF FF BB
    • SetTarget：Hex AA 11 %% BB
    • ...

    image-20241202151624845

  2. 先进行一次烧录，让 VOFA+ 收到第一组数据（FireWater），并配置四个通道

    image-20241202152117108

    在左侧拖出波形图

    image-20241202152157385

    和一堆滑动条

    image-20241202152246823

    右键滑动条并绑定命令

    image-20241202152325780

    将事件触发改为

    image-20241202152356340

  3. 其实就差不多了，剩下的还是得自己摸索

项目地址

调参项目发到了 GitHub，不完善，需要者自取