解码PID控制（含编码器与电机控制）

编码器接口与编码电机工作原理

编码器接口

• 功能：接收增量（正交）编码器的A/B相信号，通过检测信号脉冲的边沿和相位关系，自动控制计数器（CNT）自增/自减，从而获取编码器的位置、旋转方向、旋转速度。
• 硬件关联：每个高级定时器和通用定时器都自带1个编码器接口，占用输入捕获的通道1（TI1）和通道2（TI2）。

• 核心部件：GPIO引脚（接收A/B相信号）、滤波器（滤除毛刺）、边沿检测、极性选择、时基单元（ARR自动重装器、CNT计数器、PSC预分频器）。
• 计数模式：

• 示例：（反相即在极性选择时加一个非门，反转极性）

  • 均不反相

  

  • TI1反相

  

正交编码器方向判断（A/B相逻辑）

正交编码器通过A相和B相的相位差判断旋转方向，核心依据是“边沿触发时另一相的电平状态”：

正转时的边沿-电平关系

触发边沿	另一相状态
A相上升沿（↑）	B相低电平
A相下降沿（↓）	B相高电平
B相上升沿（↑）	A相高电平
B相下降沿（↓）	A相低电平

反转时的边沿-电平关系

触发边沿	另一相状态
A相上升沿（↑）	B相高电平
A相下降沿（↓）	B相低电平
B相上升沿（↑）	A相低电平
B相下降沿（↓）	A相高电平

编码电机工作原理

• 核心组件：多级径向磁铁（如22极，11对极）、双极锁存型霍尔传感器（SENSOR1输出A相，SENSOR2输出B相）、减速箱齿轮。

• 霍尔传感器特性：磁场强度B>0mT时输出低电平（遇到S输出低电平），B<0mT时输出高电平（遇到N输出高电平），满足一个霍尔传感器正对N或S、另一个处于NS交界处，此处取90°，产生A/B相正交波形。（转一圈对应CNT变化）

• 减速比计算：

• 控制原理图

• 注：
  • 电机驱动方向与编码测速方向同为顺/逆时针为正（极性一致）（负反馈（实际值大于目标值就调小、反之调大）使得误差减小）
  • 若PWM输出值增大CNT值反而减小（极性相反）（正反馈（实际值大于目标值就调大、反之调小）使得误差增大），可以通过将A/B相位反相，使得电机驱动与编码测速极性一致
  • 编码器的读取周期和PID调控周期，最好保持一致

PID控制基础

PID定义与核心目标

• PID：比例（Proportional）、积分（Integral）、微分（Differential）的缩写，是一种闭环控制算法。
• 核心逻辑：通过计算“目标值（Target）-实际值（Actual）=误差（Error）”，动态调整输出值（Out），使实际值快速、准确、稳定地跟踪目标值。
• 核心优势：对被控对象模型要求低，无需精确建模，即使内部规律不明确也能有效调控。

开环控制与闭环控制的区别

开环控制

• 结构：

• 特点：控制器不知道被控对象的执行状态，无法修正误差，精度低、抗干扰能力差。
• 示例：直接给电机输出固定PWM，不检测转速/位置。

闭环控制

• 结构：

• 特点：控制器通过反馈获取实际值，根据误差调整输出，精度高、抗干扰能力强。
• 示例：PID控制器通过编码器反馈电机位置，调整PWM输出。

PID系统框图

PID各环节作用与原理

比例项（P）

• 核心公式：

• 作用：根据误差大小直接输出调控力，误差越大，调控力越强，快速缩小误差。（快）
• 实际效果：SerialPlot波形中，P控制能快速让实际值接近目标值，但存在稳态误差（实际值与目标值的稳定差值）。

稳态误差解析

• 产生原因：纯P控制时，误差为0则P输出为0；被控对象输入为0时会自发偏移（如电机摩擦力），产生误差后P输出调控力，与偏移力平衡时达到稳态，误差持续存在。
• 判断方法：给被控对象输入0，观察是否自发偏移（偏移则有稳态误差）；偏移方向即为稳态误差方向。

积分项（I）

• 核心公式：

• 作用：累积历史误差，即使误差很小，积分项也会逐渐增大，最终消除稳态误差。（稳）
• 实际效果：在P控制基础上加入I，SerialPlot波形中实际值会最终逼近目标值，稳态误差消失。

• 𝐾_𝑖越大，积分项权重越大，稳态误差消失越快，但系统滞后性也会随之增加

微分项（D）

• 核心公式：

• 作用：反映误差的变化率，误差变化快时输出较大调控力，抑制超调（实际值超过目标值），加快系统响应速度。
• 实际效果：加入D后，SerialPlot波形中实际值跟踪目标值时更平稳，无明显超调，震荡减少。

• 𝐾_d越大，微分项权重越大，系统阻尼越大，但系统卡顿现象也会随之增加

PID的离散化与两种实现形式

连续与离散PID

• 连续PID：适用于模拟系统，公式为

• 离散PID：嵌入式系统（如STM32）是数字系统，需将连续公式离散化（按固定周期T采样计算），核心是用“求和”代替“积分”，用“差值”代替“微分”。

• 将𝑇并入𝐾_𝑖 和𝐾_d

位置式PID与增量式PID

位置式PID程序实现（STM32中断版）

• 确定一个调控周期T，每隔时间T，程序执行一次PID调控
  • 变化快的被控对象，如倒立摆、平衡车、四轴飞行器 T = 20ms/10ms/5ms/1ms
  • 变化慢的被控对象，如温控类 T=几百毫秒/几秒/几十秒

#include "stm32f10x.h"
#include "Delay.h"
#include "Timer.h"

// 全局变量定义
float Target = 0.0f;    // PID目标值（如电机目标位置/速度）
float Actual = 0.0f;    // PID实际值（从编码器读取）
float Out = 0.0f;       // PID输出值（如PWM占空比）
float Kp = 2.0f;        // 比例系数（需根据实际系统调试）
float Ki = 0.5f;        // 积分系数
float Kd = 0.1f;        // 微分系数
float Error0 = 0.0f;    // 当前误差
float Error1 = 0.0f;    // 上一次误差
float ErrorInt = 0.0f;  // 误差累积和（积分项）
uint8_t Flag = 0;       // PID调度标志位

int main(void) {
    Timer_Init();  // 初始化定时器（如TIM2，设置中断周期T=10ms）
    while(1) {
        // 用户可在此处修改目标值（如通过按键、串口设置）
        Target = 100.0f;  // 示例：目标位置100个脉冲
        if(Flag == 1) {
            Flag = 0;  // 清除标志位，避免重复执行
            // 1. 读取实际值（编码器反馈）
            Actual = Read_Encoder();  // 自定义函数：读取编码器计数，转换为实际值

            // 2. 更新误差（当前误差=目标值-实际值）
            Error1 = Error0;  // 保存上一次误差
            Error0 = Target - Actual;  // 计算当前误差

            // 3. 积分项累积（误差求和）
            ErrorInt += Error0;  // 累积历史误差

            // 4. 位置式PID计算
            Out = Kp * Error0 + Ki * ErrorInt + Kd * (Error0 - Error1);

            // 5. 输出限幅（防止输出超出执行器能力，如PWM占空比0~100）
            if(Out > 100.0f) Out = 100.0f;
            if(Out < -100.0f) Out = -100.0f;  // 正负表示方向（如电机正反转）

            // 6. 输出至被控对象（如电机PWM控制）
            Motor_Output_PWM(Out);  // 自定义函数：根据Out输出PWM
        }
    }
}

// 定时器2中断服务函数（PID调度周期T）
void TIM2_IRQHandler(void) {
    if(TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) {
        Flag = 1;  // 置位标志位，触发PID计算
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update);  // 清除中断标志
    }
}

/**
 * @brief 读取编码器实际值（示例函数）
 * @return float 编码器转换后的实际值（如位置脉冲数、速度r/min）
 * @note 需根据编码器接口配置，读取TIM计数器值，结合减速比转换
 */
float Read_Encoder(void) {
    int16_t cnt = TIM_GetCounter(TIM3);  // 假设编码器接TIM3
    TIM_SetCounter(TIM3, 0);  // 清零计数器，下次重新计数
    return (float)cnt;  // 简化示例，实际需乘系数转换
}

/**
 * @brief 电机PWM输出函数（示例函数）
 * @param pwm_val PWM占空比（-100~100，正负表示方向）
 * @note 需配置电机驱动芯片（如L298N），控制IN1/IN2方向，PWM引脚输出占空比
 */
void Motor_Output_PWM(float pwm_val) {
    // 方向控制：pwm_val>0正转，pwm_val<0反转
    if(pwm_val > 0) {
        GPIO_SetBits(GPIOA, GPIO_Pin_0);  // IN1=1
        GPIO_ResetBits(GPIOA, GPIO_Pin_1);  // IN2=0
    } else if(pwm_val < 0) {
        GPIO_ResetBits(GPIOA, GPIO_Pin_0);  // IN1=0
        GPIO_SetBits(GPIOA, GPIO_Pin_1);  // IN2=1
        pwm_val = -pwm_val;  // 取绝对值作为占空比
    } else {
        GPIO_ResetBits(GPIOA, GPIO_Pin_0 | GPIO_Pin_1);  // 停止
    }
    // 占空比输出（假设PWM接TIM1_CH1，ARR=999，预分频=71，频率1kHz）
    TIM_SetCompare1(TIM1, (uint16_t)pwm_val * 10);  // 0~100 → 0~1000
}

特点

• 输出为全量值，可直接控制执行器（如PWM占空比）。
• 需累积误差积分项，对积分饱和敏感（需配合积分限幅）。
• 适用于执行器需要全量输出的场景（如电机位置控制）。

增量式PID程序实现（控制器内积分，输出全量）

#include "stm32f10x.h"
#include "Delay.h"
#include "Timer.h"

// 全局变量定义
float Target = 0.0f;    // 目标值
float Actual = 0.0f;    // 实际值
float Out = 0.0f;       // 输出值（累加增量后）
float Kp = 2.0f;        // 比例系数
float Ki = 0.5f;        // 积分系数
float Kd = 0.1f;        // 微分系数
float Error0 = 0.0f;    // 当前误差
float Error1 = 0.0f;    // 上一次误差
float Error2 = 0.0f;    // 上上次误差
uint8_t Flag = 0;       // PID调度标志位

int main(void) {
    Timer_Init();  // 初始化定时器（调度周期T=10ms）
    while(1) {
        Target = 100.0f;  // 示例目标值
        if(Flag == 1) {
            Flag = 0;
            // 1. 读取实际值
            Actual = Read_Encoder();

            // 2. 更新误差（保存历史误差）
            Error2 = Error1;  // 上上次误差 = 上一次误差
            Error1 = Error0;  // 上一次误差 = 当前误差
            Error0 = Target - Actual;  // 计算当前误差

            // 3. 增量式PID计算（求输出增量）
            float delta_out = Kp*(Error0 - Error1) + Ki*Error0 + Kd*(Error0 - 2*Error1 + Error2);

            // 4. 累加增量得到当前输出
            Out += delta_out;

            // 5. 输出限幅
            if(Out > 100.0f) Out = 100.0f;
            if(Out < -100.0f) Out = -100.0f;

            // 6. 控制电机
            Motor_Output_PWM(Out);
        }
    }
}

// 定时器2中断服务函数（同位置式）
void TIM2_IRQHandler(void) {
    if(TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) {
        Flag = 1;
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
    }
}

// Read_Encoder()和Motor_Output_PWM()函数同位置式

注：增量式PID𝐾_𝑖一般不要给0

位置式与增量式PID对比

特性	位置式PID	增量式PID
输出形式	全量输出（直接控制）	增量输出（需累加）
积分项处理	直接累积误差	间接通过增量累加实现积分
对积分饱和的敏感性	敏感（需积分限幅）	较不敏感
执行器要求	无需内部积分功能	若直接输出增量，需执行器带积分
Kp、Ki、Kd全为0	out立即为0	只会导致增量为0
适用场景	位置控制、输出需全量的场景	速度控制、执行器带积分的场景

定速控制与定位置控制

• 定速控制：Actual给的是实际速度（Actual = Encoder_Get();）
• 定位置控制：Actual给的是实际位置（Actual += Encoder_Get();）
  • 增量式定位置PID,纯P调节，会依赖上次调控，若上次调控受到干扰，本次调控将难以调控正确，调节过快，可能会导致实际值与目标值存在一些偏移
  • 调控力度过小，不足以驱动执行器产生动作，差生调控误差

PID算法改进

积分限幅（解决积分饱和）

• 问题：执行器故障（卡住、断电）时，误差持续存在，积分项无限制增大（深度饱和）；即使执行器恢复，PID仍输出最大调控力，导致系统震荡。
• 实现思路：限制误差积分项或积分项输出的幅值，超过阈值则钳位。

程序实现（位置式PID中添加）

// 积分限幅参数（需调试，如±500）
#define INT_MAX 500.0f
#define INT_MIN -500.0f

// 误差积分累积后添加限幅
ErrorInt += Error0;
if(ErrorInt > INT_MAX) ErrorInt = INT_MAX;  // 积分上限钳位
if(ErrorInt < INT_MIN) ErrorInt = INT_MIN;  // 积分下限钳位

积分分离（解决大误差时积分超调）

• 问题：调控前期误差大，积分项累积过快，导致后期超调严重（实际值远超目标值）。
• 实现思路：误差绝对值小于阈值时，加入积分项；误差大于阈值时，清零积分或关闭积分作用。
• 积分分离阈值可以取误差波动范围，再留些余量。

程序实现（两种方式）

// 方式1：误差小时积分，大时清零积分
#define SEPARATE_THRESHOLD 10.0f  // 积分分离阈值（需调试）

Error1 = Error0;
Error0 = Target - Actual;
if(fabs(Error0) < SEPARATE_THRESHOLD) {
    ErrorInt += Error0;  // 误差小，累积积分
} else {
    ErrorInt = 0;  // 误差大，清零积分
}

// 方式2：用系数C控制是否启用积分
float C = 0.0f;
if(fabs(Error0) < SEPARATE_THRESHOLD) {
    C = 1.0f;  // 启用积分
} else {
    C = 0.0f;  // 关闭积分
}
Out = Kp*Error0 + C*Ki*ErrorInt + Kd*(Error0 - Error1);

变速积分（积分分离升级版）

• 问题：积分分离阈值设置不当，被控对象在阈值外稳定时，无积分作用，误差无法消除。
• 实现思路：设计调整系数C（随误差绝对值增大而减小），动态调整积分速度或积分强度，避免积分完全关闭。

程序实现（两种方式）

#define K_SPEED 0.1f  // 变速衰减系数（需调试）

Error1 = Error0;
Error0 = Target - Actual;

// 方式1：调整积分速度（C×Error0）
float C = 1.0f / (K_SPEED * fabs(Error0) + 1.0f);  // 误差越大，C越小
ErrorInt += C * Error0;

// 方式2：调整积分强度（C×Ki×ErrorInt）
float C = 1.0f / (K_SPEED * fabs(Error0) + 1.0f);
Out = Kp*Error0 + C*Ki*ErrorInt + Kd*(Error0 - Error1);

微分先行（解决目标值突变导致的微分冲击）

• 问题：目标值突然变化时，误差突变，微分项输出巨大冲击，导致系统震荡。

• 实现思路：将“对误差的微分”改为“对实际值的微分”，避免目标值突变影响微分项。

程序实现

float Actual1 = 0.0f;  // 上一次实际值
float DifOut = 0.0f;   // 微分先行输出

// PID计算部分修改
Actual1 = Actual;  // 保存上一次实际值
Actual = Read_Encoder();  // 读取当前实际值
Error0 = Target - Actual;  // 计算误差
ErrorInt += Error0;

// 微分先行：DifOut = -Kd×(当前实际值-上一次实际值)
DifOut = -Kd * (Actual - Actual1);

// PID总输出
Out = Kp*Error0 + Ki*ErrorInt + DifOut;

不完全微分（解决微分项对噪声敏感）

• 问题：微分项对信号噪声（如编码器毛刺）敏感，导致输出波动。

• 实现思路：给微分项加入一阶低通滤波器（惯性单元），平滑微分输出。

程序实现

float DifOut = 0.0f;  // 滤波后的微分输出
#define ALPHA 0.8f  // 滤波强度（0~1，越接近1滤波越强）

Error1 = Error0;
Error0 = Target - Actual;
ErrorInt += Error0;

// 不完全微分公式：DifOut = (1-α)×Kd×(Error0-Error1) + α×DifOut
DifOut = (1 - ALPHA) * Kd * (Error0 - Error1) + ALPHA * DifOut;

Out = Kp*Error0 + Ki*ErrorInt + DifOut;

输出偏移（解决零漂问题）

• 问题：被控对象存在零漂（如电机无输入时轻微转动），导致小误差无法消除。
• 实现思路：非零输出时，给输出值加固定偏移，补偿零漂。

程序实现

#define OFFSET 5.0f  // 输出偏移值（需调试）

Out = Kp*Error0 + Ki*ErrorInt + Kd*(Error0 - Error1);

// 输出偏移处理
if(Out > 0) {
    Out += OFFSET;  // 正输出加偏移
} else if(Out < 0) {
    Out -= OFFSET;  // 负输出减偏移（增强反向力）
} else {
    Out = 0;
}

// 输出限幅（需包含偏移值，避免超限）
if(Out > 100.0f) Out = 100.0f;
if(Out < -100.0f) Out = -100.0f;

输入死区（解决小误差频繁调控）

• 问题：误差极小时（如接近目标值），PID仍频繁调整，导致系统抖动。
• 实现思路：误差绝对值小于死区阈值时，输出为0，不进行调控。

程序实现

#define DEAD_ZONE 2.0f  // 死区阈值（需调试）

Error1 = Error0;
Error0 = Target - Actual;

if(fabs(Error0) < DEAD_ZONE) {
    Out = 0;  // 小误差，不调控
} else {
    ErrorInt += Error0;
    Out = Kp*Error0 + Ki*ErrorInt + Kd*(Error0 - Error1);
}

// 输出限幅
if(Out > 100.0f) Out = 100.0f;
if(Out < -100.0f) Out = -100.0f;

多环串级PID（双环PID）

核心概念

• 定义：多个PID环路串级连接，外环输出作为内环目标值，实现对多个物理量的控制（如位置+速度、角度+位置）。

• 优势：相比单环PID，准确性更高、响应更快、稳定性更强。

• 典型结构（以电机定位置控制为例）：

  

  • 外环（位置环）：目标值=期望位置，实际值=编码器位置，输出=目标速度（给内环）。
  • 内环（速度环）：目标值=外环输出的速度，实际值=电机转速，输出=PWM（直接控制电机）。

  小解：

  • 假设位置环PID采用PD调控、速度环PID采用PI控制，由于速度环I项作用，即使速度很小也不存在输出值不足以驱动电机转动的情况，准确性更高。
  • 假设电机位置稳定后，外力干扰电机的位置。实际速度与目标速度存在误差，速度环PID会输出PWM调控误差，同时实际位置与目标位置也存在误差，位置环PID也会输出目标速度驱动速度环PID输出PWM调控误差，双环同时调控响应更快、稳定性更强。

单环与双环PID对比（电机位置控制）

控制方式	结构	优点	缺点
单环PID	位置环→电机	结构简单	响应慢、抗干扰弱
双环PID	位置环→速度环→电机	响应快、稳定性强、抗干扰好	结构复杂、需调试两个环参数

参数调控

• 先调控内环，再调控外环
• 一般参数越大，响应越快，抖动不可避免；参数越小，响应越慢，动作会更加平缓

双环PID程序实现

头文件（pid.h）

#ifndef __PID_H
#define __PID_H

// PID结构体定义：包含双环PID所需所有参数、变量，通用化设计支持速度环/位置环
typedef struct {
	float Target;    // 目标值（速度环=目标速度，位置环=目标位置）
	float Actual;    // 实际值（速度环=编码器速度，位置环=编码器累加位置）
	float Out;       // PID输出值（速度环=电机PWM，位置环=速度环目标值）
	
	float Kp;        // 比例系数
	float Ki;        // 积分系数
	float Kd;        // 微分系数
	
	float Error0;    // 当前误差
	float Error1;    // 上一次误差
	float ErrorInt;  // 误差积分累积值
	
	float OutMax;    // 输出最大值（限幅）
	float OutMin;    // 输出最小值（限幅）
} PID_t;

/**
 * @brief  PID计算及结构体变量更新通用函数
 * @param  p 指向PID_t结构体的指针，支持速度环/位置环复用
 * @retval 无
 * @note   内置积分防误累积（Ki=0时清零积分）、位置式PID计算、输出限幅，一步完成PID核心逻辑
 */
void PID_Update(PID_t *p);

#endif

PID 底层实现（pid.c）

#include "stm32f10x.h"
#include "PID.h"

/**
 * @brief  PID计算及结构体变量值更新（核心通用函数）
 * @param  p PID_t * 指向速度环/位置环PID结构体的地址
 * @retval 无
 * @note   1. 自动更新当前/上一次误差 2. Ki=0时积分清零，避免调试时积分误累积
 *         3. 标准位置式PID公式计算 4. 内置输出限幅，防止超出执行器能力
 */
void PID_Update(PID_t *p)
{
	// 1. 更新误差：上一次误差 = 本次误差，本次误差 = 目标值 - 实际值
	p->Error1 = p->Error0;					// 保存上一次误差，用于微分项计算
	p->Error0 = p->Target - p->Actual;		// 计算当前误差，目标值减实际值
	
	// 2. 误差积分累积：Ki=0时直接清零，避免调试时积分无意义累积导致的输出突变
	if (p->Ki != 0)			
	{
		p->ErrorInt += p->Error0;			// Ki非0时，正常累积积分项
	}
	else						
	{
		p->ErrorInt = 0;					// Ki为0时，积分项归0，消除积分影响
	}
	
	// 3. 标准位置式PID公式计算输出值
	p->Out = p->Kp * p->Error0                // 比例项：根据当前误差直接调控
		   + p->Ki * p->ErrorInt              // 积分项：累积历史误差，消除稳态误差
		   + p->Kd * (p->Error0 - p->Error1); // 微分项：根据误差变化率，抑制超调
	
	// 4. 输出限幅：限制PID输出在执行器可承受范围内
	if (p->Out > p->OutMax) {p->Out = p->OutMax;}
	if (p->Out < p->OutMin) {p->Out = p->OutMin;}
}

双环 PID 主程序（main.c）

#include "stm32f10x.h"                  // STM32F10x系列核心头文件
#include "PID.h"                        // 自定义PID头文件

/* 定义双环PID结构体变量，初始化核心参数（贴合实际工程调参范围）
 * 内环：速度环，直接控制电机PWM，输出限幅-100~100（对应PWM占空比）
 * 外环：位置环，输出作为速度环目标值，输出限幅-20~20（速度环合理目标范围）
 */
PID_t Inner = {                         // 内环-速度环（核心控制电机转速）
	.Kp = 0.3,        // 比例系数（实际调参初始值）
	.Ki = 0.3,        // 积分系数（实际调参初始值）
	.Kd = 0,          // 微分项（初始关闭，调参后开启）
	.OutMax = 100,    // PWM最大输出（正转满占空比）
	.OutMin = -100,   // PWM最小输出（反转满占空比）
};

PID_t Outer = {                         // 外环-位置环（控制电机位置，输出给速度环）
	.Kp = 0.3,        // 比例系数（实际调参初始值）
	.Ki = 0,          // 积分系数（初始关闭，调参后开启）
	.Kd = 0.4,        // 微分系数（实际调参初始值，抑制位置超调）
	.OutMax = 20,     // 速度环最大目标值
	.OutMin = -20,    // 速度环最小目标值
};

int main(void)
{
	/* 模块初始化：按实际工程优先级排序，外设逐一初始化 */
	Timer_Init();		// 定时器初始化（配置1ms定时中断，作为PID调度基准）
	
	while (1)
	{

	}
}

/**
 * @brief  定时器1更新中断服务函数（1ms中断一次，PID调度核心）
 * @note   1. 采用静态计数器分频，分别调度内环（速度环）和外环（位置环），均为40ms一次
 *         2. 内环：采集编码器速度→PID计算→输出PWM控制电机
 *         3. 外环：采集编码器位置→PID计算→输出作为内环速度环目标值（串级核心）
 *         4. 所有PID计算通过PID_Update通用函数完成，代码复用性强
 */
void TIM1_UP_IRQHandler(void)
{
	// 静态分频计数器：函数退出后保留值，实现1ms→40ms分频，默认初值0
	static uint16_t Count1, Count2;	
	
	// 检查定时器1更新中断标志位
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
		
		/************************ 内环：速度环调度（40ms一次） ************************/
		Count1 ++;				// 1ms计数器自增
		if (Count1 >= 40)		// 累计40ms，执行速度环逻辑
		{
			Count1 = 0;			// 计数器清零，准备下次累计
			
			// 1. 采集编码器数据：获取40ms内编码器增量，正比于电机速度
			Speed = Read_Encoder();		// Read_Encoder()：返回编码器A/B相脉冲增量
			Location += Speed;			// 速度增量累加，得到电机实际位置（给外环使用）
			
			// 2. 给速度环赋值实际值：当前编码器增量即为实际速度
			Inner.Actual = Speed;
			
			// 3. 速度环PID计算：调用通用函数，一步完成误差更新、积分、计算、限幅
			PID_Update(&Inner);
			
			// 4. 速度环执行控制：将PID输出值给到电机，控制PWM占空比/方向
			Motor_Output_PWM(Inner.Out);
		}

		/************************ 外环：位置环调度（40ms一次） ************************/
		Count2 ++;				// 1ms计数器自增
		if (Count2 >= 40)		// 累计40ms，执行位置环逻辑
		{
			Count2 = 0;			// 计数器清零，准备下次累计
			
			// 1. 给位置环赋值实际值：累加后的编码器位置即为实际位置
			Outer.Actual = Location;
			
			// 2. 位置环PID计算：调用通用函数，一步完成所有逻辑
			PID_Update(&Outer);
			
			// 3. 串级PID核心：位置环输出作为速度环目标值，实现位置→速度的串级控制
			Inner.Target = Outer.Out;
		}
		
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);	// 清除中断标志位，避免重复中断
	}
}
// Read_Encoder()和Motor_Output_PWM()函数同上,建议也进行单独封装、同时建议定义两个flag将中断中的处理放到后台

倒立摆实验（双环PID应用）

倒立摆硬件结构

• 核心组件：STM32控制板、直流减速电机（带编码器）、摆杆（含角度传感器）、横杆（电机驱动）、霍尔传感器（检测摆杆角度）。
• 角度传感器：分辨率（0~4095），中心角度对应2048左右（易受干扰），用于检测摆杆倾斜角度。

倒立摆控制逻辑（双环PID）

• 内环（角度环）：目标值=摆杆中心角度（如2048），实际值=角度传感器读数，输出=电机PWM（控制横杆移动，保持摆杆倒立）。
• 外环（位置环）：目标值=横杆期望位置（如中心），实际值=电机编码器位置，输出=角度环的目标角度偏移（微调摆杆角度，让横杆回归中心）。

自动启摆流程（状态机）

倒立摆初始为下垂状态，需通过启摆流程让摆杆摆动至中心区间（A<角度值<B），再启动PID控制：

状态

核心控制代码框架

宏定义与全局变量（硬件适配核心）

#include "stm32f10x.h"
#include "Delay.h"
#include "OLED.h"
#include "LED.h"
#include "Timer.h"
#include "Key.h"
#include "RP.h"
#include "Motor.h"
#include "Encoder.h"
#include "Serial.h"
#include "AD.h"
#include "PID.h"

/************************ 硬件适配宏定义（需根据实际设备调整） ************************/
#define CENTER_ANGLE    2010    // 摆杆中心角度值（1900~2200，需实测：摆杆垂直时AD值）
                                 // 不准确会导致摆杆单向偏移、位置环有稳态误差
#define CENTER_RANGE    500     // 中心区间范围（CENTER_ANGLE±500，进入此区间启动PID）
#define START_PWM       35      // 启摆瞬时驱动力PWM（30~40，太小摆不上去，太大摆过头）
#define START_TIME      100     // 启摆驱动力持续时间（80~120ms，太小力度不足，太大不利于共振）

/************************ 全局变量定义 ************************/
uint8_t KeyNum;                 // 按键键码
uint8_t RunState = 0;          // 运行状态机：0=停止，1=判断，21-24=左启摆，31-34=右启摆，4=PID控制
uint16_t Angle;                 // 摆杆角度值（AD采集，0~4095）
int16_t Speed, Location;        // 电机速度（编码器增量）、位置（速度累加）

/************************ 双环PID结构体定义（复用之前的PID_t） ************************/
PID_t AnglePID = {              // 内环：角度环（保持摆杆倒立）
	.Target = CENTER_ANGLE,     // 目标角度=中心角度
	.Kp = 0.3, .Ki = 0.01, .Kd = 0.4,
	.OutMax = 100, .OutMin = -100
};

PID_t LocationPID = {           // 外环：位置环（保持横杆位置）
	.Target = 0,                // 目标位置=初始中心
	.Kp = 0.4, .Ki = 0, .Kd = 4,
	.OutMax = 100, .OutMin = -100
};

主程序（按键控制 + LED 指示 + OLED 显示）

int main(void)
{
	/************************ 模块初始化 ************************/
	OLED_Init();		// OLED初始化（显示状态/参数）
	LED_Init();			// LED初始化（指示运行状态）
	Key_Init();			// 非阻塞式按键初始化
	RP_Init();			// 电位器初始化（调参用，暂注释）
	Motor_Init();		// 电机PWM初始化
	Encoder_Init();		// 编码器初始化（采集速度/位置）
	Serial_Init();		// 串口初始化（9600波特率，暂未用）
	AD_Init();			// 角度传感器AD初始化
	Timer_Init();		// 定时器初始化（1ms中断，调度核心）

	while (1)
	{
		/************************ 按键控制逻辑 ************************/
		KeyNum = Key_GetNum();
		if (KeyNum == 1) {			// K1：启动/停止
			if (RunState == 0) RunState = 21;	// 停止→左启摆（避开角度传感器盲区）
			else RunState = 0;						// 运行→停止
		}
		if (KeyNum == 2) {			// K2：位置+408（正转一圈，408=电机一周脉冲数）
			LocationPID.Target += 408;
			if (LocationPID.Target > 4080) LocationPID.Target = 4080;	// 限幅±10圈
		}
		if (KeyNum == 3) {			// K3：位置-408（反转一圈）
			LocationPID.Target -= 408;
			if (LocationPID.Target < -4080) LocationPID.Target = -4080;
		}

		/************************ LED状态指示 ************************/
		if (RunState) LED_ON();	// 非0状态：LED亮（运行中）
		else LED_OFF();				// 0状态：LED灭（停止）

		/************************ 电位器调参（暂注释，调参时解除） ************************/
		// AnglePID.Kp = RP_GetValue(1)/4095.0*1;    // 角度环Kp：0~1
		// AnglePID.Ki = RP_GetValue(2)/4095.0*1;    // 角度环Ki：0~1
		// AnglePID.Kd = RP_GetValue(3)/4095.0*1;    // 角度环Kd：0~1
		// LocationPID.Kp = RP_GetValue(1)/4095.0*1; // 位置环Kp：0~1
		// LocationPID.Ki = RP_GetValue(2)/4095.0*1; // 位置环Ki：0~1
		// LocationPID.Kd = RP_GetValue(3)/4095.0*9; // 位置环Kd：0~9

		/************************ OLED全参数显示 ************************/
		OLED_Printf(42, 0, OLED_6X8, "%02d", RunState);	// 显示当前状态（调试用）

		// 左侧：角度环参数
		OLED_Printf(0, 0, OLED_6X8, "Angle");
		OLED_Printf(0, 12, OLED_6X8, "Kp:%05.3f", AnglePID.Kp);
		OLED_Printf(0, 20, OLED_6X8, "Ki:%05.3f", AnglePID.Ki);
		OLED_Printf(0, 28, OLED_6X8, "Kd:%05.3f", AnglePID.Kd);
		OLED_Printf(0, 40, OLED_6X8, "Tar:%04.0f", AnglePID.Target);
		OLED_Printf(0, 48, OLED_6X8, "Act:%04d", Angle);	// 直接显示AD采集的角度，PID停止也能刷新
		OLED_Printf(0, 56, OLED_6X8, "Out:%+04.0f", AnglePID.Out);

		// 右侧：位置环参数
		OLED_Printf(64, 0, OLED_6X8, "Location");
		OLED_Printf(64, 12, OLED_6X8, "Kp:%05.3f", LocationPID.Kp);
		OLED_Printf(64, 20, OLED_6X8, "Ki:%05.3f", LocationPID.Ki);
		OLED_Printf(64, 28, OLED_6X8, "Kd:%05.3f", LocationPID.Kd);
		OLED_Printf(64, 40, OLED_6X8, "Tar:%+05.0f", LocationPID.Target);
		OLED_Printf(64, 48, OLED_6X8, "Act:%+05d", Location);	// 直接显示累加位置
		OLED_Printf(64, 56, OLED_6X8, "Out:%+04.0f", LocationPID.Out);

		OLED_Update();	// 强制刷新OLED
	}
}

定时器中断服务函数（启摆状态机 + 双环 PID 调度核心）

void TIM1_UP_IRQHandler(void)
{
	/************************ 静态变量（函数退出后保留值） ************************/
	static uint16_t Count0, Count1, Count2, CountTime;	// 分频计数器：判断状态、角度环、位置环、启摆计时
	static uint16_t Angle0, Angle1, Angle2;				// 连续3次角度采样：本次、上次、上上次（判断最高点）

	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
		Key_Tick();			// 1ms调用一次按键Tick（非阻塞式按键核心）

		/************************ 统一1ms采集硬件数据 ************************/
		Angle = AD_GetValue();		// 采集摆杆角度（AD值0~4095）
		Speed = Read_Encoder();		// 采集电机速度（编码器脉冲增量）
		Location += Speed;			// 累加速度得到位置

		/************************ 状态机核心逻辑 ************************/
		if (RunState == 0) {			// 状态0：停止
			Motor_Output_PWM(0);			// 电机停止
		}
		else if (RunState == 1) {		// 状态1：判断（40ms一次）
			Count0++;
			if (Count0 >= 40) {
				Count0 = 0;

				// 1. 保存连续3次角度值（40ms间隔）
				Angle2 = Angle1;
				Angle1 = Angle0;
				Angle0 = Angle;

				// 2. 判断是否在右侧最高点：3次均在右侧区间，且中间角度最小
				if (Angle0 > CENTER_ANGLE + CENTER_RANGE
				 && Angle1 > CENTER_ANGLE + CENTER_RANGE
				 && Angle2 > CENTER_ANGLE + CENTER_RANGE
				 && Angle1 < Angle0 && Angle1 < Angle2) {
					RunState = 21;	// 转入左启摆
				}

				// 3. 判断是否在左侧最高点：3次均在左侧区间，且中间角度最大
				if (Angle0 < CENTER_ANGLE - CENTER_RANGE
				 && Angle1 < CENTER_ANGLE - CENTER_RANGE
				 && Angle2 < CENTER_ANGLE - CENTER_RANGE
				 && Angle1 > Angle0 && Angle1 > Angle2) {
					RunState = 31;	// 转入右启摆
				}

				// 4. 判断是否进入中心区间：连续2次在中心区间
				if (Angle0 > CENTER_ANGLE - CENTER_RANGE
				 && Angle0 < CENTER_ANGLE + CENTER_RANGE
				 && Angle1 > CENTER_ANGLE - CENTER_RANGE
				 && Angle1 < CENTER_ANGLE + CENTER_RANGE) {
					// PID启动前初始化：避免错误初值干扰
					Location = 0;
					AnglePID.ErrorInt = 0;
					LocationPID.ErrorInt = 0;
					RunState = 4;	// 转入PID控制
				}
			}
		}
		/************************ 左启摆流程（21→22→23→24→1） ************************/
		else if (RunState == 21) {	// 21：向左施加瞬时驱动力
			Motor_Output_PWM(START_PWM);
			CountTime = START_TIME;		// 初始化计时
			RunState = 22;
		}
		else if (RunState == 22) {	// 22：延时
			CountTime--;
			if (CountTime == 0) RunState = 23;
		}
		else if (RunState == 23) {	// 23：向右施加瞬时驱动力
			Motor_Output_PWM(-START_PWM);
			CountTime = START_TIME;
			RunState = 24;
		}
		else if (RunState == 24) {	// 24：延时
			CountTime--;
			if (CountTime == 0) {
				Motor_Output_PWM(0);		// 停止电机
				RunState = 1;			// 回到判断状态
			}
		}
		/************************ 右启摆流程（31→32→33→34→1） ************************/
		else if (RunState == 31) {	// 31：向右施加瞬时驱动力
			Motor_Output_PWM(-START_PWM);
			CountTime = START_TIME;
			RunState = 32;
		}
		else if (RunState == 32) {	// 32：延时
			CountTime--;
			if (CountTime == 0) RunState = 33;
		}
		else if (RunState == 33) {	// 33：向左施加瞬时驱动力
			Motor_Output_PWM(START_PWM);
			CountTime = START_TIME;
			RunState = 34;
		}
		else if (RunState == 34) {	// 34：延时
			CountTime--;
			if (CountTime == 0) {
				Motor_Output_PWM(0);
				RunState = 1;
			}
		}
		/************************ 状态4：双环PID控制 ************************/
		else if (RunState == 4) {
			// 1. 摆杆倒下自动停止：角度超出中心区间
			if (!(Angle > CENTER_ANGLE - CENTER_RANGE && Angle < CENTER_ANGLE + CENTER_RANGE)) {
				RunState = 0;
			}

			// 2. 内环：角度环（5ms一次，快速响应摆杆角度）
			Count1++;
			if (Count1 >= 5) {
				Count1 = 0;
				AnglePID.Actual = Angle;			// 实际值=AD采集的角度
				PID_Update(&AnglePID);				// 调用通用PID函数
				Motor_Output_PWM(AnglePID.Out);		// 输出PWM控制电机
			}

			// 3. 外环：位置环（50ms一次，慢速调整横杆位置）
			Count2++;
			if (Count2 >= 50) {
				Count2 = 0;
				LocationPID.Actual = Location;		// 实际值=累加的位置
				PID_Update(&LocationPID);			// 调用通用PID函数
				// 串级核心：位置环输出→角度环目标值（微调中心角度，控制横杆位置）
				AnglePID.Target = CENTER_ANGLE - LocationPID.Out;
			}
		}

		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);	// 清除中断标志
	}
}