PID学习笔记6-倒立摆的实现

main.c:

cpp 复制代码

#include "stm32f10x.h"                  // Device header
#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之间
										//中心角度值不准确会导致摆杆总是有往一个方向跑的趋势
										//中心角度值不准确会导致位置环PD控制器有稳态误差
										//中心角度值不准确会导致最终按键控制横杆正转一圈和反转一圈的速度不同

#define CENTER_RANGE		500			//中心区间范围
										//规定中心区间的角度值在CENTER_ANGLE - CENTER_RANGE至CENTER_ANGLE + CENTER_RANGE之间

/*定义全局变量*/
uint8_t KeyNum;				//按键键码
uint8_t RunState;			//运行状态，规定0为停止状态，1为运行状态

uint16_t Angle;				//摆杆角度值
int16_t Speed, Location;	//电机的速度和位置

/*定义PID结构体变量*/
PID_t AnglePID = {			//内环角度环PID结构体变量，定义的时候同时给部分成员赋初值
	.Target = CENTER_ANGLE,	//角度环目标值初值设定为中心角度值
	
	.Kp = 0.3,				//比例项权重
	.Ki = 0.01,				//积分项权重
	.Kd = 0.4,				//微分项权重
	
	.OutMax = 100,			//输出限幅的最大值
	.OutMin = -100,			//输出限幅的最小值
};

PID_t LocationPID = {		//外环位置环PID结构体变量，定义的时候同时给部分成员赋初值
	.Target = 0,			//位置环目标值初值设定为0
	
	.Kp = 0.4,				//比例项权重
	.Ki = 0,				//积分项权重
	.Kd = 4,				//微分项权重
	
	.OutMax = 100,			//输出限幅的最大值
	.OutMin = -100,			//输出限幅的最小值
};

int main(void)
{
	/*模块初始化*/
	OLED_Init();		//OLED初始化
	LED_Init();			//LED初始化
	Key_Init();			//非阻塞式按键初始化
	RP_Init();			//电位器旋钮初始化
	Motor_Init();		//电机初始化
	Encoder_Init();		//编码器初始化
	Serial_Init();		//串口初始化，波特率9600，当前程序暂未使用串口
	AD_Init();			//角度传感器初始化
	
	Timer_Init();		//定时器初始化，定时中断时间1ms
	
	while (1)
	{
		/*按键控制*/
		KeyNum = Key_GetNum();		//获取键码
		if (KeyNum == 1)			//如果K1按下
		{
			RunState = !RunState;	//运行状态取非，用于控制程序启动和停止
		}
		if (KeyNum == 2)			//如果K2按下
		{
			LocationPID.Target += 408;		//位置环目标位置加408，控制横杆正转一圈
			if (LocationPID.Target > 4080)	//如果正转了10圈
			{
				LocationPID.Target = 4080;	//不再允许正转，避免位置越界
			}
		}
		if (KeyNum == 3)			//如果K3按下
		{
			LocationPID.Target -= 408;		//位置环目标位置减408，控制横杆反转一圈
			if (LocationPID.Target < -4080)	//如果反转了10圈
			{
				LocationPID.Target = -4080;	//不再允许反转，避免位置越界
			}
		}
		
		/*LED指示程序运行状态*/
		if (RunState)		//如果运行状态非0
		{
			LED_ON();		//点亮LED，指示程序正在运行
		}
		else				//否则
		{
			LED_OFF();		//熄灭LED，指示程序停止运行
		}
		
		/*电位器旋钮调节角度环和位置环PID参数*/
		/*RP_GetValue函数返回电位器旋钮的AD值，范围：0~4095*/
		/* 除4095.0可以把AD值归一化，再乘上一个系数，可以调整到一个合适的范围*/
//		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(0, 0, OLED_6X8, "Angle");							//OLED左侧显示静态字符串Angel
		OLED_Printf(0, 12, OLED_6X8, "Kp:%05.3f", AnglePID.Kp);			//显示Kp
		OLED_Printf(0, 20, OLED_6X8, "Ki:%05.3f", AnglePID.Ki);			//显示Ki
		OLED_Printf(0, 28, OLED_6X8, "Kd:%05.3f", AnglePID.Kd);			//显示Kd
		OLED_Printf(0, 40, OLED_6X8, "Tar:%04.0f", AnglePID.Target);	//显示目标值
		OLED_Printf(0, 48, OLED_6X8, "Act:%04d", Angle);	//显示实际值，使用Angle而不是AnglePID.Actual，可以实现PID程序停止运行时仍然刷新实际值
		OLED_Printf(0, 56, OLED_6X8, "Out:%+04.0f", AnglePID.Out);		//显示输出值
		
		OLED_Printf(64, 0, OLED_6X8, "Location");						//OLED右侧显示静态字符串Location
		OLED_Printf(64, 12, OLED_6X8, "Kp:%05.3f", LocationPID.Kp);		//显示Kp
		OLED_Printf(64, 20, OLED_6X8, "Ki:%05.3f", LocationPID.Ki);		//显示Ki
		OLED_Printf(64, 28, OLED_6X8, "Kd:%05.3f", LocationPID.Kd);		//显示Kd
		OLED_Printf(64, 40, OLED_6X8, "Tar:%+05.0f", LocationPID.Target);	//显示目标值
		OLED_Printf(64, 48, OLED_6X8, "Act:%+05d", Location);	//显示实际值，使用Location而不是LocationPID.Actual，可以实现PID程序停止运行时仍然刷新实际值
		OLED_Printf(64, 56, OLED_6X8, "Out:%+04.0f", LocationPID.Out);	//显示输出值
		
		OLED_Update();	//OLED更新，调用显示函数后必须调用此函数更新，否则显示的内容不会更新到OLED上
	}
}

void TIM1_UP_IRQHandler(void)
{
	/*定义静态变量（默认初值为0，函数退出后保留值和存储空间）*/
	static uint16_t Count1, Count2;		//分别用于角度环和位置环的计次分频
	
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
		/*每隔1ms，程序执行到这里一次*/
		
		Key_Tick();			//调用按键的Tick函数
		
		/*每隔1ms，统一获取角度传感器值*/
		Angle = AD_GetValue();		//获取角度传感器的角度值
		Speed = Encoder_Get();		//获取电机的速度值
		Location += Speed;			//获取电机的位置值
		
		/*摆杆倒下自动停止PID程序*/
		if (! (Angle > CENTER_ANGLE - CENTER_RANGE
			&& Angle < CENTER_ANGLE + CENTER_RANGE))	//如果角度值超过了规定的中心区间
		{
			RunState = 0;			//运行状态变量置0，自动停止PID程序
		}
		
		/*根据运行状态执行PID程序或者停止*/
		if (RunState)				//如果运行状态不为0
		{
			/*角度环计次分频*/
			Count1 ++;				//计次自增
			if (Count1 >= 5)		//如果计次5次，则if成立，即if每隔5ms进一次
			{
				Count1 = 0;			//计次清零，便于下次计次
				
				/*以下进行角度环PID控制*/
				AnglePID.Actual = Angle;		//内环为角度环，实际值为角度值
				PID_Update(&AnglePID);			//调用封装好的函数，一步完成PID计算和更新
				Motor_SetPWM(AnglePID.Out);		//角度环的输出值给到电机PWM
			}
			
			/*位置环计次分频*/
			Count2 ++;				//计次自增
			if (Count2 >= 50)		//如果计次50次，则if成立，即if每隔50ms进一次
			{
				Count2 = 0;			//计次清零，便于下次计次
				
				/*以下进行位置环PID控制*/
				LocationPID.Actual = Location;	//外环为位置环，实际值为位置值
				PID_Update(&LocationPID);		//调用封装好的函数，一步完成PID计算和更新
				AnglePID.Target = CENTER_ANGLE - LocationPID.Out;	//外环的输出值作用于内环的目标值，组成串级PID结构
			}
		}
		else						//如果运行状态为0
		{
			Motor_SetPWM(0);		//不执行PID程序且电机PWM直接设置为0，电机停止
		}
		
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
	}
}

PID.c:

cpp 复制代码

#include "stm32f10x.h"                  // Device header
#include "PID.h"

/**
  * 函    数：PID计算及结构体变量值更新
  * 参    数：PID_t * 指定结构体的地址
  * 返 回 值：无
  */
void PID_Update(PID_t *p)
{
	/*获取本次误差和上次误差*/
	p->Error1 = p->Error0;					//获取上次误差
	p->Error0 = p->Target - p->Actual;		//获取本次误差，目标值减实际值，即为误差值
	
	/*外环误差积分（累加）*/
	/*如果Ki不为0，才进行误差积分，这样做的目的是便于调试*/
	/*因为在调试时，我们可能先把Ki设置为0，这时积分项无作用，误差消除不了，误差积分会积累到很大的值*/
	/*后续一旦Ki不为0，那么因为误差积分已经积累到很大的值了，这就导致积分项疯狂输出，不利于调试*/
	if (p->Ki != 0)					//如果Ki不为0
	{
		p->ErrorInt += p->Error0;	//进行误差积分
	}
	else							//否则
	{
		p->ErrorInt = 0;			//误差积分直接归0
	}
	
	/*PID计算*/
	/*使用位置式PID公式，计算得到输出值*/
	p->Out = p->Kp * p->Error0
		   + p->Ki * p->ErrorInt
		   + p->Kd * (p->Error0 - p->Error1);
	
	/*输出限幅*/
	if (p->Out > p->OutMax) {p->Out = p->OutMax;}	//限制输出值最大为结构体指定的OutMax
	if (p->Out < p->OutMin) {p->Out = p->OutMin;}	//限制输出值最小为结构体指定的OutMin
}

PID.h:

cpp 复制代码

#ifndef __PID_H
#define __PID_H

typedef struct {
	float Target;
	float Actual;
	float Out;
	
	float Kp;
	float Ki;
	float Kd;
	
	float Error0;
	float Error1;
	float ErrorInt;
	
	float OutMax;
	float OutMin;
} PID_t;

void PID_Update(PID_t *p);

#endif

一、系统怎么让摆"立起来"（两阶段）

阶段 A：自动启摆（Swing-Up，给能量）

目标：让摆从自然下垂逐步"加能量"，越荡越高，最终接近竖直。
原理：像人荡秋千一样，通过小车（横杆）左右快速移动 在合适的时机注入动能 。

典型做法有两种：
1. 能量法：根据当前机械能（势能+动能）与目标能量的差，生成加速度指令给小车，把能量补足。
2. 脉冲/状态机法 （你图里画的）：根据摆角处于左/右半区及是否到达最高点 ，给出短时正/负向 PWM （RunState=21/23/31/33），再延时（RunState=22/24/32/34），周而复始，让幅度越来越大，直到进入中心区间（A<角度<B）。
你的两张图正是这种状态机启摆：
- 在右侧最高点 → 迅速向左给一个正 PWM；
- 在左侧最高点 → 迅速向右给一个负 PWM；
- 反向推进 + 延时，反复"打气"。
- 一旦摆进入中心区间 （A～B），就结束启摆，切换到 PID 稳定。

说明：你贴出的 main.c 这版还未集成 图中的启摆状态机（只有 RunState 开关、倒下自动停机），但图已清楚表达启摆思路；把状态机按图实现即可。

阶段 B：倒立稳定（Stabilization）

目标：摆靠近竖直后，用反馈控制把它锁在竖直附近并让小车位置受控。
结构：双环串级 PID
- 内环：角度环（AnglePID） ------ 高频、快环
  - 实际值：Angle = AD_GetValue()
  - 采样/计算：每 5 ms （见 Count1>=5）
  - 输出：AnglePID.Out 直接送 Motor_SetPWM()
- 外环：位置环（LocationPID） ------ 低频、慢环
  - 实际值：Location = ∑ Speed，Speed = Encoder_Get()
  - 采样/计算：每 50 ms （见 Count2>=50）
  - 输出：LocationPID.Out 作用到内环目标角：
    复制代码
```
AnglePID.Target = CENTER_ANGLE - LocationPID.Out; 
```
    意思是：位置误差会"偏置"角度目标，让内环把摆杆朝某一侧微微倾斜，从而产生水平力把小车拉回目标位置（0/所需圈数）。
进入/退出条件：
- CENTER_ANGLE 定位竖直角；CENTER_RANGE 定义"中心可控区"。
- 若摆离开中心区 （倒下）则 RunState=0，立刻停机，避免冲撞。

二、代码和流程关键点（对应`main.c`）

统一感知（每 1 ms 中断）

cpp 复制代码

Angle = AD_GetValue();     // 角度
Speed = Encoder_Get();     // 速度（增量）
Location += Speed;         // 位置（累加）

2.安全互锁

cpp 复制代码

if (!(Angle > CENTER_ANGLE - CENTER_RANGE && Angle < CENTER_ANGLE + CENTER_RANGE))
    RunState = 0; // 摆倒，自动停机

3.角度内环（5 ms）

cpp 复制代码

AnglePID.Actual = Angle;
PID_Update(&AnglePID);
Motor_SetPWM(AnglePID.Out);

这是快环，负责抑制摆角偏差（"不让它倒"）。

4.位置外环（50 ms）

cpp 复制代码

LocationPID.Actual = Location;
PID_Update(&LocationPID);
AnglePID.Target = CENTER_ANGLE - LocationPID.Out;

这是慢环，通过"偏置角度目标"让内环把小车缓慢回位，又不影响角度稳定。

5.人机交互

K1 切换 Run/Stop；K2/K3 给位置目标正/反一圈 （408 计数 ≈ 1 圈），并做位置限幅（±10 圈）。

6.时基与限幅

AnglePID：Out 限幅 ±100；LocationPID：Out 限幅 ±100。
双环限幅能抑制外环过猛，避免角度环来不及"托住"。

三、硬件设计要点

1.传感器

角度：用 绝对值角度传感器（ADC） 或 IMU，分辨率 & 零点 直接决定 CENTER_ANGLE 的准确性；噪声越低越好。
速度/位置：增量编码器安装同轴，接线方向要与"正方向"一致（否则极性反了会发散）。

2.执行器

电机/驱动器带宽要足够（内环 5 ms 步进）；PWM 频率适中（避免啸叫/过热）。
机械摩擦/间隙尽量小；滑轨要低阻尼但无明显回差。

3.电源/布线

充足的驱动电流，防止大口径 PWM 时电源塌陷。
ADC、编码器信号做好滤波与屏蔽，地线星形回路，避免大电流回流干扰。

四、软件调试步骤（按顺序来）

准备校准

标定 CENTER_ANGLE（使摆静止竖直时的 ADC 值），尽量把单边偏移调到最小。
检查电机方向、编码器方向：正角误差 应驱动电机产生正确的纠偏（否则 Kp 符号错）。

Step 1：先把角度环（内环）调稳

只跑角度环（先把位置环停掉或 Ki=Kd=0），目标设 CENTER_ANGLE。
提升 Kp 直到"快但不抖"，轻振就回退 10--20%。
加小 Kd 抑制超调（必要时用"微分先行 + 不完全微分"降噪）。
少量 Ki 只用来消除角度稳态偏差（要小，防止低频摆动）。

Step 2：再开位置环（外环）

外环先只用 Kp：把小车能慢慢回到原点为目标；
出现回位时角度扰动明显 → 适当加 Kd；
若位置稳态偏差可接受，可不加 Ki；必须归零才加很小的 Ki ，并做抗饱和；
把 AnglePID.Target = CENTER_ANGLE - LocationPID.Out 这条链路的量纲心里要有数（外环 Out 的限幅，决定了你允许角度目标被偏置的最大量；太大易把角度拉崩）。

Step 3：最后集成自动启摆

按你的流程图写状态机 （RunState=21/22/23/24/31/32/33/34...），以最高点检测 + 脉冲驱动 + 延时反复注能；
进入中心区（A～B）时，平滑切换到 Angle/Location 双环（可在切换前对速度做软钳位/低通，避免切换瞬间突跳）。

Step 4：加保护

角度越界立即停机（你已经做了）；
PWM 软限幅、加速度限幅、位置越界限位；
发生落摆后需按键复位（防止抖动误触发重启）。

五、为什么它能"立住"（控制学解释）

角度环把"倒立"点线性化为带阻尼的平衡点 （相当于给倒立点附近加等效刚度与阻尼），抵消"倒立不稳定平衡"。
位置环通过"角度目标偏置"给角度环一个偏心力 ，用来慢慢把小车拉回目标位置；因外环更慢，等效为在不打破角度稳定的前提下叠加低频位移。
两个环的带宽分离（5 ms vs 50 ms）避免了相互抢控制权：快环稳角度，慢环调位置。

六、常见坑 & 快速定位

CENTER_ANGLE 不准：会发现摆总"偏向一侧"，位置环也会出现稳态误差；重标定或在 AnglePID 前做零点补偿。
传感器噪声导致抖动 ：角度环 Kd 过大或传感器抖 → 用微分滤波 、ADC 滑动平均，或稍加死区/迟滞。
外环过猛 ：位置 Kp 大或限幅太宽 → 切换后角度大幅摆动；收小外环增益与限幅。
方向极性错：Kp 增大反而更不稳，立刻检查编码器计数方向、PWM 正负号。
积分风up ：若开 Ki，一定加抗饱和/条件积分（尤其位置环）。

总结

启摆：按"最高点脉冲 + 延时"的状态机不断注能/或能量法；进入中心区 后切换到角度内环 + 位置外环。
稳定：内环快、外环慢，外环通过"角度目标偏置"实现回位，双环限幅保证不过激。
落地经验 ：先硬件 （低摩擦、信号干净、极性对），再软件（先内环再外环），最后接入启摆状态机并加安全保护。调得好需要时间，但按照上面顺序，会快很多。

PID学习笔记6-倒立摆的实现

一、系统怎么让摆"立起来"（两阶段）

阶段 A：自动启摆（Swing-Up，给能量）

阶段 B：倒立稳定（Stabilization）

二、代码和流程关键点（对应main.c）

三、硬件设计要点

四、软件调试步骤（按顺序来）

五、为什么它能"立住"（控制学解释）

六、常见坑 & 快速定位

总结

二、代码和流程关键点（对应`main.c`）