【速度环+位置环串级PID(大疆M3508减速电机套装】_电机的位置是速度的累加?

1.PID介绍

        PID（Proportional-Integral-Derivative）是一种常用的反馈控制算法，广泛应用于自动控制系统中。它是根据目标值和实际值之间的差异（偏差）来调整输出控制信号，使系统能够稳定地接近期望的状态。

        PID控制器由三个部分组成：

1. 比例（Proportional，P）：比例控制作用于偏差的大小，输出信号与偏差成正比。P部分对应于偏差的大小来产生一个对应的控制量，但无法消除系统的稳态误差。

2. 积分（Integral，I）：积分控制用于累积偏差，并且与时间成正比。I部分解决了由于比例控制无法消除系统稳态误差而产生的问题。它能够持续地累积偏差，并产生一个反向控制信号，以逐渐消除系统的稳态误差。

3. 微分（Derivative，D）：微分控制响应偏差的变化率，通过检测偏差的变化趋势来预测未来的偏差。D部分有助于控制系统对于快速变化的偏差作出快速的响应，避免过冲和震荡。

        综合这三个部分，PID的数学如下图所示，$e(t)$表示当前时刻的误差，PID控制器的输出信号可以表示为以下形式：

        输出信号 = Kp * 偏差 + Ki * ∫(偏差 dt) + Kd * d(偏差)/dt

        其中，Kp、Ki和Kd是称为控制增益的参数，用来调整PID控制器的响应特性。不同的系统需要不同的参数配置，因此，在实际应用中，通常需要进行系统参数调整和优化。

        当使用蓄水池来解释PID控制器时，我们可以想象蓄水池的水位需要保持在一个特定的目标水位上。

        蓄水池的水位就相当于控制系统中的状态，而目标水位则是我们希望系统达到的目标状态。PID控制器就像一个智能调节阀门，根据当前水位和目标水位之间的差距，调整阀门的开关程度，以保持水位稳定在目标水位上。

1. 比例（Proportional）：比例部分会根据当前水位与目标水位之间的差距，决定调整阀门的开关程度。如果水位偏离目标水位很远，比例控制器会适当地打开或关闭阀门，使水位更接近目标水位。

2. 积分（Integral）：积分部分考虑过去一段时间内水位的累积偏差。如果蓄水池一直没有达到目标水位，积分控制器会累积这些小的水位偏差，并逐渐调整阀门，帮助池水稳步地接近目标水位。

3. 微分（Derivative）：微分部分关注水位变化的趋势。如果水位接近目标水位时迅速变化，微分控制器会预测这种趋势，并适当调整阀门，以防止水位过冲或下降过快。

        通过比例、积分和微分三个部分的协同作用，PID控制器能够帮助蓄水池维持稳定的水位。无论面对池水剧烈波动或外部影响，PID控制器都会及时作出调整，使蓄水池始终保持在理想的目标水位上。这样，我们可以确保蓄水池在不同条件下都能正常运行，提供稳定的水源供应。
        PID控制器适用于许多自动控制问题，如温度控制、速度控制、位置控制等。它简单、易于实现，是许多工业和工程领域中最常用的控制算法之一。然而，对于某些复杂的系统，PID控制可能不足以满足要求，因此在实际应用中，可能需要使用更高级的控制算法。

1.1位置式PID

        位置式PID（Proportional-Integral-Derivative）是一种常见的反馈控制算法，用于控制系统中。它通过监测系统当前的位置（或状态）与目标位置之间的差异，来调整输出控制信号，使系统尽可能稳定地接近目标位置。让我们详细解释每个组成部分：

1. 比例（Proportional）部分：比例控制器根据当前位置与目标位置的差异（称为误差），以一定比例来调整输出控制信号。比例增益（通常用字母Kp表示）决定了误差与输出之间的线性关系。如果误差较大，比例控制器会产生更大的调整，以更快地减小误差，但也可能引起过冲现象。

2. 积分（Integral）部分：积分控制器考虑的是误差随时间的积累。它根据过去的误差累积（称为积分项），以一定比例（积分增益，通常用字母Ki表示）来调整输出控制信号。积分控制器可以消除系统存在的稳态误差（即目标位置与实际位置之间的持续偏差），因为它考虑了误差累积。

3. 微分（Derivative）部分：微分控制器关注的是误差变化的趋势。它根据当前误差与上次误差的差值，以一定比例（微分增益，通常用字母Kd表示）来调整输出控制信号。微分控制器可以预测误差变化的趋势，从而避免过冲和振荡，提高系统的稳定性。

        位置式PID控制器的输出控制信号可以用以下公式表示：

$\text{Output} = Kp \times \text{误差} + Ki \times \text{误差累积} + Kd \times \text{误差变化率}$

        在实际应用中，通过调整比例增益（Kp）、积分增益（Ki）和微分增益（Kd），可以使PID控制器达到所需的控制效果。但要注意，过大或过小的增益值可能导致控制系统不稳定或响应过慢。因此，在实际应用中，需要根据系统特性和需求来仔细调整PID参数，以获得最佳的控制效果。

1.2增量式PID

        当解释增量式PID时，我们需要理解其计算方式和与传统位置式PID的区别。下面详细描述增量式PID的工作原理：

1. 初始设定：

   • 设定目标状态：我们需要指定系统的目标状态，例如目标温度、目标位置等。
   • 初始化增量式PID参数：设定比例增益（Kp）、积分增益（Ki）和微分增益（Kd）。这些参数会影响PID控制器的响应速度、稳定性和精确性。

2. 系统反馈与误差计算：

   • 测量当前状态：通过传感器或测量装置，获取系统当前的状态值（位置、温度、速度等）。
   • 计算误差：将目标状态减去当前状态，得到当前误差值。

3. 增量式PID计算：

   • 计算增量比例：将当前误差与上一时刻误差的差值（即增量）乘以比例增益（Kp），得到增量输出中比例部分的贡献。
   • 计算增量积分：将当前误差与上一时刻误差的差值乘以积分增益（Ki），得到增量输出中积分部分的贡献。
   • 计算增量输出：将增量比例和增量积分相加，得到本次的增量输出。

4. 更新控制信号：

   • 将增量输出与上一时刻的控制信号相加，得到本次的控制信号，用于控制系统。这个控制信号会调整执行器或驱动器，例如改变阀门开度或电机转速，以使系统趋近目标状态。

5. 循环控制：

   • 控制系统会不断地执行上述步骤，进行反复的测量、误差计算和控制信号更新，以持续调节系统状态，使其稳定在目标状态附近。

        与传统位置式PID相比，增量式PID的主要优势在于计算复杂性的降低和存储资源的节省。因为它只关注误差的变化情况，而不需要保存大量的历史误差数据。这使得增量式PID在资源有限的嵌入式系统或实时控制系统中应用更为方便。然而，由于增量式PID涉及误差的差值，它可能对测量噪声更为敏感，因此在某些情况下可能需要更仔细地调整参数，以获得最佳控制效果。

1.3对比

        位置式PID和增量式PID是两种不同的PID控制算法，它们在计算控制量的方式和特性上有一些区别。以下是它们之间的对比：

1. 控制量计算方式：

   • 位置式PID：控制量是直接基于当前偏差（目标值与实际输出之间的差异）进行计算，通过比例、积分和微分部分的加权求和来得到输出信号。
   • 增量式PID：控制量是计算当前偏差与过去几个控制周期的偏差之差，通过比例、积分和微分部分的加权求和来得到控制量的增量（变化量），然后将增量累加到输出信号得到最终的控制量。

2. 积分项处理：

   • 位置式PID：位置式PID中的积分项直接与偏差累积，并可能出现积分饱和问题，导致系统响应不稳定。
   • 增量式PID：增量式PID通过控制量的增量计算，避免了积分饱和问题，因为控制量的增量通常不会累积到较大的值。

3. 响应特性：

   • 位置式PID：位置式PID可以实现相对较平滑的控制器响应，对于某些系统，可能会有较低的峰值和较小的振荡。
   • 增量式PID：增量式PID对于某些系统可能表现出更为快速的响应特性，因为它直接计算控制量的变化量，可以更快地调整控制器输出。

4. 控制器集成：

   • 位置式PID：位置式PID相对简单，易于理解和实现。它对于一些简单的控制系统来说已经足够满足要求。
   • 增量式PID：增量式PID更容易与现有的控制器集成，因为它只需要控制量的增量而不需要绝对值。

        在实际应用中，选择使用哪种形式的PID控制算法取决于具体的控制系统要求和性能目标。对于一些简单的系统，位置式PID可能已经足够满足控制要求。而对于一些复杂的系统或需要更高性能控制的应用，增量式PID可能更具优势，因为它可以避免一些位置式PID可能存在的问题，提供更好的控制性能。

2.串级PID

        串级PID（Cascade PID）是一种高级的PID控制结构，用于解决复杂系统的控制问题。它通过将两个或多个PID控制器串联起来，使得一个PID控制器的输出作为另一个PID控制器的输入，从而实现更精确、稳定的控制效果。

        在串级PID控制结构中，通常有两个层级：

1. 主控制器（外层控制器）：主控制器负责控制系统的主要过程变量，如位置、速度或其他一般的控制目标。它通常使用较慢的控制动作，例如较大的时间常数，来控制整个过程。

2. 辅助控制器（内层控制器）：辅助控制器负责控制主控制器所需的参考变量。它根据主控制器的输出和实际过程变量之间的差异来计算控制量，然后作为主控制器的设定点（setpoint）。辅助控制器通常具有较快的响应速度，以便更精确地跟踪主控制器的设定点。

        串级PID的工作原理如下：

1. 主控制器：根据目标设定点和实际过程变量之间的偏差计算主控制器的输出，该输出作为辅助控制器的设定点。

2. 辅助控制器：根据主控制器的设定点和实际过程变量之间的偏差计算辅助控制器的输出，该输出作为最终的控制信号，用于直接控制系统。

        串级PID控制的优点包括：

• 提高系统响应速度：由于辅助控制器具有较快的响应特性，能够更快地跟踪主控制器的设定点，从而提高整个系统的响应速度。

• 减小稳态误差：主控制器负责控制整个过程的稳态误差，而辅助控制器负责校正主控制器的设定点，使得整个系统的稳态误差减小。

• 改善稳定性：通过两个控制层级的串联，可以在不同时间尺度上进行控制，更好地适应系统的动态特性，从而改善系统的稳定性。

        串级PID控制适用于复杂的控制系统，特别是那些具有多个耦合的过程变量和需要高精度控制的应用。然而，串级PID的设计和调试可能较为复杂，需要深入理解系统的动态特性和控制要求。在实际应用中，需要根据具体的控制需求来选择合适的控制结构和参数配置。

2.1直流无刷电机

        此处使用大疆M3508电机套装（P19直流无刷减速电机+C620无刷电机调速器）
相关资料参考：https://www.robomaster.com/zh-CN/products/components/general/M3508
        此处仅列举电机的部分重要参数：
        P19直流无刷减速电机：M3508 直流无刷减速电机是专为中小型移动平台和机器人等量身打造的高性能伺服电机，可搭配 RoboMaster C620 电调实现正弦驱动，相比传统方波驱动具有更高的效率、机动性和稳定性。本产品减速箱减速比约为 19:1。
位置反馈:电机自带位置传感器，可提供位置的反馈
温度检测:电机自带温度检测传感器，可有效防止电机因温度异常被损坏。
信息存储:存储电机校准参数，支持电机的快速更换。
        C620无刷电机调速器：C620 电调采用 32 位定制电机驱动芯片，使用磁场定向控制(FOC) 技术，实现对电机转知的精确控制，与 M3508 直流无刷减速电机搭配，组成强大的动力套件。可配合 RoboMasterAssistant 调参软件进行参数设置并升级固件。
        产品特性：
        支持两种可选控制方式50-500Hz 的 PWM( 脉宽调制）信号控制CAN 总线指令控制
        最高支持 20A 的持续电流支持对 CAN 总线上的电调快速设置 ID支持通过 CAN 总线获取电机温度、转子位置和转了转速等信息
        切换电机时可无需进行位置传感器参数校准
CAN通讯协议
        电调接收报文格式
        用于向电调发送控制指令控制电调的电流输出，两个标识符(0x200 和 0x1FF) 各自对应控制4个ID的电调。控制电流值范围-16384 ~ 0 ~16384，对应电调输出的转矩电流范围-20 ~ 0 ~20A。
标识符: 0x200 帧类型:标准帧 帧格式:DATA DLC:8字节

标识符: 0x1FF 帧类型:标准帧 帧格式:DATA DLC:8字节

电调反馈报文格式

电调向总线上发送的反馈数据。
标识符: 0x200 + 电调ID
(如: ID 为 1，该标识符为 0x201 )帧类型:标准帧帧格式:DATADLC: 8字节

发送频率: 1KHz ( 默认值，可在 RoboMasterAssistant 软件中修改发送频率)

转子机械角度值范围: 0 ~8191

对应转子机械角度为 0~360°

转子转速值的单位为: RPM

电机温度的单位为:°C

2.2电机PID闭环控制

此处的电机PID控制代码参考大疆提供的官方例程，可在网址处下载：M3508减速电机套装 (robomaster.com)

主要用到的文件包括：

pid.h

#ifndef __pid_H
#define __pid_H
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
 
enum{
    LLAST	= 0,
    LAST 	= 1,
    NOW 	= 2,
    
    POSITION_PID,
    DELTA_PID,//PID相关枚举类型
};
typedef struct __pid_t
{
    float p;
    float i;
    float d;
    
    float set[3];				//目标值,包含NOW， LAST， LLAST上上次
    float get[3];				//测量值
    float err[3];				//误差
	
    
    float pout;					//p输出
    float iout;					//i输出
    float dout;					//d输出
    
    float pos_out;				//本次位置式输出
    float last_pos_out;			//上次输出
    float delta_u;				//本次增量值
    float delta_out;			//本次增量式输出 = last_delta_out + delta_u
    float last_delta_out;
    
	float max_err;
	float deadband;				//err < deadband return
    uint32_t pid_mode;
    uint32_t MaxOutput;			//输出限幅
    uint32_t IntegralLimit;		//积分限幅
    
    void (*f_param_init)(struct __pid_t *pid,  //PID参数初始化
                        uint32_t pid_mode,
                        uint32_t maxOutput,
                        uint32_t integralLimit,
                        float p,
                        float i,
                        float d);
    void (*f_pid_reset)(struct __pid_t *pid, float p, float i, float d);//pid三个参数修改
}pid_t;//PID相关结构体定义
 
void PID_struct_init(
    pid_t* pid,
    uint32_t mode,
    uint32_t maxout,
    uint32_t intergral_limit,
    float 	kp, 
    float 	ki, 
    float 	kd);//初始化函数申明
    
float pid_calc(pid_t* pid, float fdb, float ref);//PID输出值计算
    
extern pid_t pid_rol;
extern pid_t pid_pit;
extern pid_t pid_yaw;
extern pid_t pid_pit_omg;
extern pid_t pid_yaw_omg;	
extern pid_t pid_spd[4];
extern pid_t pid_yaw_alfa;
extern pid_t pid_chassis_angle;
extern pid_t pid_poke;
extern pid_t pid_poke_omg;
extern pid_t pid_imu_tmp;		//imu_temperature
extern pid_t pid_cali_bby;	//big buff yaw
extern pid_t pid_cali_bbp;
extern pid_t pid_omg;
extern pid_t pid_pos;
#endif

头文件中包括PID相关结构体的定义以及用到的函数、变量声明。

pid.c

/* Includes ------------------------------------------------------------------*/
#include "pid.h"
#include "mytype.h"
#include <math.h>
 
#define ABS(x)		((x>0)? (x): (-x)) 
 
void abs_limit(float *a, float ABS_MAX)
{
    if(*a > ABS_MAX)
        *a = ABS_MAX;
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
}
/*参数初始化--------------------------------------------------------------*/
static void pid_param_init
(   pid_t *pid, 
    uint32_t mode,
    uint32_t maxout,
    uint32_t intergral_limit,
    float 	kp, 
    float 	ki, 
    float 	kd)
{
    
    pid->IntegralLimit = intergral_limit;
    pid->MaxOutput = maxout;
    pid->pid_mode = mode;
    
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
    
}
/*中途更改参数设定(调试)------------------------------------------------------------*/
static void pid_reset(pid_t	*pid, float kp, float ki, float kd)
{
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
}
/*PID计算（可选择位置式和增量式）----------------------------------------------------*/
float pid_calc(pid_t* pid, float get, float set){
    pid->get[NOW] = get;
    pid->set[NOW] = set;
    pid->err[NOW] = set - get;	//set - measure
    if (pid->max_err != 0 && ABS(pid->err[NOW]) >  pid->max_err  )
		return 0;
	if (pid->deadband != 0 && ABS(pid->err[NOW]) < pid->deadband)
		return 0;
    
    if(pid->pid_mode == POSITION_PID) //位置式p
    {
        pid->pout = pid->p * pid->err[NOW];
        pid->iout += pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST] );
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->pos_out = pid->pout + pid->iout + pid->dout;
        abs_limit(&(pid->pos_out), pid->MaxOutput);
        pid->last_pos_out = pid->pos_out;	//update last time 
    }
    else if(pid->pid_mode == DELTA_PID)//增量式P
    {
        pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
        pid->iout = pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
        
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->delta_u = pid->pout + pid->iout + pid->dout;
        pid->delta_out = pid->last_delta_out + pid->delta_u;
        abs_limit(&(pid->delta_out), pid->MaxOutput);
        pid->last_delta_out = pid->delta_out;	//update last time
    }
    
    pid->err[LLAST] = pid->err[LAST];
    pid->err[LAST] = pid->err[NOW];
    pid->get[LLAST] = pid->get[LAST];
    pid->get[LAST] = pid->get[NOW];
    pid->set[LLAST] = pid->set[LAST];
    pid->set[LAST] = pid->set[NOW];
    return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
}
 
/**
    *@bref. special calculate position PID @attention @use @gyro data!!
    *@param[in] set： target
    *@param[in] real	measure
    */
float pid_sp_calc(pid_t* pid, float get, float set, float gyro){
    pid->get[NOW] = get;
    pid->set[NOW] = set;
    pid->err[NOW] = set - get;	//set - measure
    
    
    if(pid->pid_mode == POSITION_PID) //位置式p
    {
        pid->pout = pid->p * pid->err[NOW];
				if(fabs(pid->i) >= 0.001f)
					pid->iout += pid->i * pid->err[NOW];
				else
					pid->iout = 0;
        pid->dout = -pid->d * gyro/100.0f;	
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->pos_out = pid->pout + pid->iout + pid->dout;
        abs_limit(&(pid->pos_out), pid->MaxOutput);
        pid->last_pos_out = pid->pos_out;	//update last time 
    }
    else if(pid->pid_mode == DELTA_PID)//增量式P
    {
        pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
        pid->iout = pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
        
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->delta_u = pid->pout + pid->iout + pid->dout;
        pid->delta_out = pid->last_delta_out + pid->delta_u;
        abs_limit(&(pid->delta_out), pid->MaxOutput);
        pid->last_delta_out = pid->delta_out;	//update last time
    }
    
    pid->err[LLAST] = pid->err[LAST];
    pid->err[LAST] = pid->err[NOW];
    pid->get[LLAST] = pid->get[LAST];
    pid->get[LAST] = pid->get[NOW];
    pid->set[LLAST] = pid->set[LAST];
    pid->set[LAST] = pid->set[NOW];
    return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;	
}
/*pid总体初始化-----------------------------------------------------------------*/
void PID_struct_init(
    pid_t* pid,
    uint32_t mode,
    uint32_t maxout,
    uint32_t intergral_limit,
    
    float 	kp, 
    float 	ki, 
    float 	kd)
{
    /*init function pointer*/
    pid->f_param_init = pid_param_init;
    pid->f_pid_reset = pid_reset;
	pid->f_cal_pid = pid_calc;	
	pid->f_cal_sp_pid = pid_sp_calc;	//addition
		
    /*init pid param */
    pid->f_param_init(pid, mode, maxout, intergral_limit, kp, ki, kd);
	
}
 
void pid_test_init(){
	
 
	//为了解决上位机调参的时候第一次赋值的时候清零其他参数， 应该提前把参数表填充一下！
 
}

上述文件是PID相关的功能函数，代码中已加入各函数的注释。

pid_t motor_pid_speed[2];				//PID结构体变量定义
PID_struct_init(motor_pid_speed,POSITION_PID,8000,8000,1.5f,0.0f,0.3f);//PID初始化
motor_pid_speed[0].deadband=200;//设置死区
motor_control_val[id]=motor_pid_speed[id].f_cal_pid((motor_pid_speed+id),moto_chassis[id].speed_rpm,motor_target.speed_target[id]);//PID计算
set_moto_current(&hcan,motor_control_val[0],motor_control_val[1],0x0000,0x0000);//发送计算后的电流值

mian函数PID的主要内容包括上述部分：PID结构体变量定义、PID结构体初始化、PID计算、PID结果输出。

2.3速度环+位置环串级PID原理

        因在项目中需要实现速度和位置双环的电机控制，故使用速度环+位置环串级PID控制，下图是控制流程图，参考文章下文已列出：

PID串级控制的典型结构为位置环+速度环+电流环，如下图。

PID串级控制中，最外环是输入是整个控制系统的期望值，外环PID的输出值是内环PID的期望值。

能够使用三环控制的前提是要硬件支持，比如位置环和速度环需要实时的电机转动位置和转动速度作为反馈，这就需要电机需要配有编码器用于测速与测量转动的位置；电流环需要有电流采样电路来实时获取电机的电流作为反馈。

因上文中有单级速度PID的相关程序，串级PID可在其基础之上进行拓展。

pid_t motor_pid_speed[2];			//pid速度环结构体定义
pid_t motor_pid_location[2];		//pid位置环结构体定义
 
float motor_control_val[2]={0.0};//PID结果输出存储
PID_struct_init(motor_pid_speed,POSITION_PID,8000,8000,1.5f,0.0f,0.3f);//电机1速度环PID结构体初始化
PID_struct_init((motor_pid_speed+1),POSITION_PID,8000,8000,1.5f,0.0f,0.3f);//电机2速度环PID结构体初始化
PID_struct_init(motor_pid_location,POSITION_PID,8000,8000,1.5f,0.0f,0.3f);//电机1位置环PID结构体初始化
PID_struct_init((motor_pid_location+1),POSITION_PID,8000,8000,1.5f,0.0f,0.3f);//电机2位置环PID结构体初始化
 
motor_pid_speed[0].deadband=200;//电机1速度计算死区
motor_pid_speed[1].deadband=200;//电机2速度计算死区
motor_pid_location[0].deadband=5000;//电机1位置计算死区
motor_pid_location[1].deadband=5000;//电机2位置计算死区
 
motor_control_val[id]=motor_pid_location[id].f_cal_pid((motor_pid_location+id),moto_chassis[id].total_angle,motor_target.location_target[id]);//根据设定值进行PID计算
if((motor_control_val[id] > motor_target.speed_target[id])&&(motor_target.speed_target[id]>=0))//速度阈值限制
    motor_control_val[id] = motor_target.speed_target[id];
else if((motor_control_val[id] < -motor_target.speed_target[id])&&(motor_target.speed_target[id]>=0))
    motor_control_val[id] = -motor_target.speed_target[id];
else if((motor_control_val[id] < motor_target.speed_target[id])&&(motor_target.speed_target[id]<0))
	motor_control_val[id] = motor_target.speed_target[id];
else if((motor_control_val[id] > -motor_target.speed_target[id])&&(motor_target.speed_target[id]<0))
	motor_control_val[id] = -motor_target.speed_target[id];
motor_control_val[id]=motor_pid_speed[id].f_cal_pid((motor_pid_speed+id),moto_chassis[id].speed_rpm,motor_control_val[id]);//位置环PID计算
 
set_moto_current(&hcan,motor_control_val[0],motor_control_val[1],0x0000,0x0000);//结果输出

上诉过程中，用到两个电机（项目需要），使用单个电机，自行修改代码即可，此处只做过程展示，在串级PID中，速度环作为内环，位置环作为外环，位置环的PID结果输出值作为内环PID的输入，在输入前，需与速度阈值进行比较，以确保速度环稳定。最后的速度环计算结果作为电流环的输入，由C620电调驱动自行实现。

具体代码可参考：【免费】【速度环+位置环串级PID(大疆M3508减速电机套装】资源-CSDN文库

参考文章网址如下：电机控制进阶3——PID串级控制（附全套代码） - 知乎 (zhihu.com)