PID 平衡小车：直立环 + 速度环 + 转向环，从倒立摆仿真调参到能站住

.gitignore
Makefile
README.md
include/
  balancer.h
  car_sim.h
  pid.h
platformio.ini
src/
  balancer.c
  car_sim.c
  main.cpp
  pid.c
test/
  test_balance.c

                  ┌─────────────┐
  速度目标 0 ────> │  速度环 (PI) │──> 目标倾角 ──┐
                  └─────────────┘                │
                                                 v
                  ┌─────────────┐        ┌─────────────┐
  角度目标 0 ────> │  直立环 (PD) │<───────│  实际倾角    │
                  └─────────────┘        └─────────────┘
                         │                      ^
                         v                      │ MPU6050
                   基础 PWM ──┬──> 左电机        │
                              │                 │
                              └──> 右电机 <─── 转向环 (PD) <── 目标偏航角速度

误差 = 实测倾角 - 目标倾角     <-- 注意是"实测减目标"
PWM  = Kp·误差 + Kd·角速度

/* 常规负反馈：误差 = 目标 - 测量，内部对测量求导 */
float pid_update(pid_t *p, float setpoint, float meas, float dt);

/* 显式版：误差和微分项都由调用方给，输出里加上 +kd * d_input */
float pid_update_ex(pid_t *p, float error, float d_input, float dt);

θ¨ = ( g·sinθ + cosθ·( −F − m·l·θ˙²·sinθ ) / (M+m) )
     / ( l·(4/3 − m·cos²θ/(M+m)) )

ẍ  = ( F + m·l·(θ˙²·sinθ − θ¨·cosθ) ) / (M+m)

CC      ?= gcc
CFLAGS  ?= -std=c99 -Wall -Wextra -O2 -Iinclude
LDLIBS  ?= -lm
SRC      = src/pid.c src/car_sim.c src/balancer.c
TEST     = test/test_balance.c

ifeq ($(OS),Windows_NT)
EXT = .exe
endif
BIN = build/test$(EXT)

all: run

$(BIN): $(SRC) $(TEST)
	@mkdir -p build
	$(CC) $(CFLAGS) $(SRC) $(TEST) -o $(BIN) $(LDLIBS)

run: $(BIN)
	@$(BIN)

clean:
	rm -rf build

.PHONY: all run clean

/**
 * balancer.h - 两轮自平衡小车的三环串级控制器
 *
 *   直立环（内，PD）  : 倾角/角速度 -> 基础 PWM
 *   速度环（外，PI）  : 编码器速度 -> 目标倾角
 *   转向环（PD）      : 偏航角速度 -> 左右轮差速
 */
#ifndef BALANCER_H
#define BALANCER_H

#include <stdint.h>

#include "pid.h"

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
    /* 直立环（内环） */
    float angle_kp;
    float angle_kd;

    /* 速度环（外环）：输出是"目标倾角"（度） */
    float speed_kp;
    float speed_ki;
    float speed_lpf_alpha;  /* 测速一阶低通 0~1，越小越平滑 */

    /* 转向环 */
    float turn_kp;
    float turn_kd;

    float max_lean_deg;     /* 速度环允许给出的最大目标倾角 */
    float pwm_max;          /* 输出 PWM 上限 */
    float pwm_deadzone;     /* 电机死区补偿 */
    float speed_setpoint;   /* 目标速度 m/s，0 = 停在原地 */
} balance_cfg_t;

typedef struct {
    balance_cfg_t cfg;
    pid_t angle_pid;
    pid_t speed_pid;
    pid_t turn_pid;

    float angle_trim_deg;   /* 机械零位补偿 */
    float speed_filt;       /* 测速低通状态 */
    float speed_valid;      /* 估出来的车体速度 */
    int32_t enc_prev;       /* M 法测速用的上一次计数值 */
    int enc_prev_valid;     /* 第一次调用时没有参考值，跳过一帧 */
    float yaw_prev;         /* 转向环微分用 */

    /* 输出 */
    float pwm_left;
    float pwm_right;

    /* 调试量 */
    float dbg_angle_err;
    float dbg_lean_target;
    float dbg_angle_out;
    float dbg_speed_out;
    float dbg_turn_out;
} balancer_t;

void balancer_init(balancer_t *b);
void balancer_reset(balancer_t *b, float angle_deg, float speed_mps);
void balancer_set_trim(balancer_t *b, float trim_deg);

/**
 * 执行一次三环计算。
 * @param angle_deg  融合后的倾角（度），前倾为正
 * @param gyro_dps   融合后的角速度（度/秒）
 * @param enc_count  编码器累计计数（由调用方差分得到速度）
 * @param yaw_dps    偏航角速度（度/秒）
 * @param dt         控制周期（秒）
 */
void balancer_step(balancer_t *b, float angle_deg, float gyro_dps,
                   int32_t enc_count, float yaw_dps, float dt);

#ifdef __cplusplus
}
#endif

#endif /* BALANCER_H */

/**
 * car_sim.h - 两轮自平衡小车的倒立摆物理仿真（主机端验证用）
 *
 * 不含任何硬件依赖，纯 C + math。用来在没有真车的情况下验证控制逻辑与调参。
 */
#ifndef CAR_SIM_H
#define CAR_SIM_H

#include <stdint.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
    /* ---- 状态 ---- */
    float theta;      /* 车体倾角，弧度，正值表示朝 +x 方向倾倒 */
    float theta_dot;  /* 角速度，rad/s */
    float x;          /* 位置，米 */
    float x_dot;      /* 速度，m/s */
    float yaw_rate;   /* 偏航角速度，rad/s */

    /* ---- 器件输出 ---- */
    int32_t enc_count; /* 量化后的编码器计数 */

    /* ---- 物理参数 ---- */
    float m_body;     /* 车体质量 kg */
    float m_wheel;    /* 等效轮子质量 kg */
    float l;          /* 质心到轮轴距离 m */
    float g;          /* 重力加速度 */
    float force_per_pwm; /* PWM=1.0 时输出的驱动力 N */
    float yaw_gain;   /* 差速指令 -> 稳态偏航角速度 */
    float yaw_tau;    /* 偏航一阶惯性时间常数 s */

    /* ---- 器件误差 ---- */
    float counts_per_m;   /* 每米多少编码器计数（含 4 倍频与减速比） */
    float acc_noise_deg;  /* 角度噪声幅值（度） */
    float gyro_noise_dps; /* 角速度噪声幅值（度/秒） */
    float enc_noise;      /* 编码器计数噪声（counts） */
    float angle_bias_deg; /* 平衡角零位偏差：装配重心偏移 / 陀螺仪零偏 */

    unsigned rng;
} car_sim_t;

void car_sim_init(car_sim_t *c);
/** 推进一个仿真步：pwm ∈ [-1,1]，turn ∈ [-1,1] */
void car_sim_step(car_sim_t *c, float pwm, float turn, float dt);

/* 器件读数（含噪声；会推进内部随机数，所以不是 const） */
float car_sim_angle_deg(car_sim_t *c);   /* 来自 MPU6050 的角度（融合后） */
float car_sim_gyro_dps(car_sim_t *c);    /* 来自 MPU6050 的 Y 轴角速度 */

/* 真值（只用于评估控制效果，控制算法不能看） */
float car_sim_true_angle_deg(const car_sim_t *c);
float car_sim_true_speed_mps(const car_sim_t *c);
float car_sim_true_x(const car_sim_t *c);
float car_sim_yaw_dps(const car_sim_t *c);
int32_t car_sim_enc(const car_sim_t *c);

#ifdef __cplusplus
}
#endif

#endif /* CAR_SIM_H */

/**
 * pid.h - 通用位置式 PID（抗积分饱和 + 微分先行 + 微分低通 + 死区）
 *
 * 提供两个入口：
 *   pid_update()     常规负反馈，误差 = setpoint - meas，内部对测量求导
 *   pid_update_ex()  误差和微分输入都由调用方给，用于正反馈结构
 *
 * 平衡车的直立环是正反馈（倾角越大越要往前冲），
 * 而且它的"微分项"本来就是陀螺仪读数，不需要再求导，
 * 所以直立环用 pid_update_ex()：
 *
 *   pid_update_ex(&angle_pid, angle_deg - target_deg, gyro_dps, dt);
 */
#ifndef PID_H
#define PID_H

#include <stdbool.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
    float kp;
    float ki;
    float kd;

    float out_min;      /* 输出下限 */
    float out_max;      /* 输出上限 */
    float i_min;        /* 积分项下限（抗饱和第二道防线） */
    float i_max;        /* 积分项上限 */
    float deadband;     /* 误差死区，|误差|<死区 时按 0 处理 */
    float d_lpf_alpha;  /* 微分低通系数 0~1，0 表示不滤波 */
    bool anti_windup;   /* 是否启用条件积分抗饱和（默认开） */

    /* --- 内部状态，调用方不要直接改 --- */
    float integ;
    float prev_meas;
    float prev_d;
    float out;
    bool saturated;
} pid_t;

void pid_init(pid_t *p, float kp, float ki, float kd, float out_min, float out_max);
void pid_set_integ_limits(pid_t *p, float lo, float hi);
void pid_set_d_lpf(pid_t *p, float alpha);
void pid_set_deadband(pid_t *p, float db);
void pid_set_antiwindup(pid_t *p, bool enable);
void pid_reset(pid_t *p, float meas);

/**
 * 常规负反馈：error = setpoint - meas。
 * 微分项用"微分先行"（对测量求导并取负），设定值跳变时不会产生微分尖峰。
 */
float pid_update(pid_t *p, float setpoint, float meas, float dt);

/**
 * 显式版：
 * @param error   已经算好的误差（方向由调用方决定）
 * @param d_input 微分项的输入，输出里会加上 +kd * d_input
 *                （正反馈结构就靠这里翻符号，或者直接把陀螺仪读数塞进来）
 * @param dt      采样周期（秒）
 * @return 限幅后的输出
 */
float pid_update_ex(pid_t *p, float error, float d_input, float dt);

#ifdef __cplusplus
}
#endif

#endif /* PID_H */

[platformio]
default_envs = bluepill

[env:bluepill]
platform = ststm32
board = bluepill_f103c8
framework = arduino
upload_protocol = stlink
monitor_speed = 115200
build_flags =
    -Wall
    -Wextra
    -Isrc
lib_ldf_mode = deep+

#include "balancer.h"

#include <math.h>
#include <string.h>

#define DEFAULT_COUNTS_PER_M 7640.0f

void balancer_init(balancer_t *b)
{
    memset(b, 0, sizeof(*b));

    /* 直立环：PD，输出单位是"千分之一 PWM" */
    b->cfg.angle_kp = 25.0f;
    b->cfg.angle_kd = 1.35f;

    /*
     * 速度环：PI，输出单位是"度"。
     *
     * 注意增益为什么这么小：速度环输出的是目标倾角，而倾角 θ 直接对应
     * 加速度 a ≈ g·θ。所以速度环的等效增益被重力放大了 9.81 倍 ——
     * Kp=9 时开环带宽约 9.81×9 ≈ 88 rad/s，比直立环（约 7 rad/s）快了十几倍，
     * 串级结构立刻失稳。经验值：Kp 取 0.2~0.5，让外环比内环慢 3~5 倍。
     */
    b->cfg.speed_kp = 0.25f;
    b->cfg.speed_ki = 2.0f;
    b->cfg.speed_lpf_alpha = 0.15f;

    /* 转向环 */
    b->cfg.turn_kp = 12.0f;
    b->cfg.turn_kd = 0.15f;

    b->cfg.max_lean_deg = 8.0f;
    b->cfg.pwm_max = 1000.0f;
    /*
     * 电机死区补偿的幅度。真实电机在 |PWM| < 8%~12% 时不转，
     * 这里默认给 0（纯线性执行器），这样仿真里"Kp 扫参数"的结论才干净；
     * 实车调试时按自己的电机填，常见值 60~120（即 6%~12%）。
     */
    b->cfg.pwm_deadzone = 0.0f;
    b->cfg.speed_setpoint = 0.0f;

    pid_init(&b->angle_pid, b->cfg.angle_kp, 0.0f, b->cfg.angle_kd,
             -b->cfg.pwm_max, b->cfg.pwm_max);
    /* 角度误差不会很大，积分限幅给窄一点 */
    pid_set_integ_limits(&b->angle_pid, -150.0f, 150.0f);

    pid_init(&b->speed_pid, b->cfg.speed_kp, b->cfg.speed_ki, 0.0f,
             -b->cfg.max_lean_deg, b->cfg.max_lean_deg);
    pid_set_integ_limits(&b->speed_pid, -b->cfg.max_lean_deg, b->cfg.max_lean_deg);

    pid_init(&b->turn_pid, b->cfg.turn_kp, 0.0f, b->cfg.turn_kd,
             -b->cfg.pwm_max, b->cfg.pwm_max);
    pid_set_integ_limits(&b->turn_pid, -200.0f, 200.0f);

    b->angle_trim_deg = 0.0f;
    b->speed_filt = 0.0f;
}

void balancer_reset(balancer_t *b, float angle_deg, float speed_mps)
{
    pid_reset(&b->angle_pid, angle_deg);
    pid_reset(&b->speed_pid, speed_mps);
    pid_reset(&b->turn_pid, 0.0f);
    b->speed_filt = speed_mps;
    b->speed_valid = speed_mps;
    b->enc_prev_valid = 0;   /* 丢掉上一帧的计数，避免解算出假的巨大速度 */
    b->yaw_prev = 0.0f;
    b->pwm_left = 0.0f;
    b->pwm_right = 0.0f;
}

void balancer_set_trim(balancer_t *b, float trim_deg)
{
    b->angle_trim_deg = trim_deg;
}

void balancer_step(balancer_t *b, float angle_deg, float gyro_dps,
                   int32_t enc_count, float yaw_dps, float dt)
{
    float speed_raw;
    float lean_target;
    float angle_err;
    float base;
    float turn;
    float trim = b->angle_trim_deg;

    if (dt <= 0.0f) {
        return;
    }

    /*
     * 1) 测速：M 法（固定窗口计数值差分）。
     *    低速时分辨率很差，必须低通。速度环最怕的就是把噪声当信号放大。
     */
    if (b->enc_prev_valid) {
        speed_raw = (float)(enc_count - b->enc_prev) /
                    DEFAULT_COUNTS_PER_M / dt;
    } else {
        speed_raw = 0.0f;
    }
    b->enc_prev = enc_count;
    b->enc_prev_valid = 1;

    {
        float a = b->cfg.speed_lpf_alpha;
        if (a <= 0.0f) {
            a = 1.0f;
        }
        b->speed_filt += a * (speed_raw - b->speed_filt);
        b->speed_valid = b->speed_filt;
    }

    /*
     * 2) 速度环（外环）：发现车在漂，就故意让它朝反方向倾一点。
     *    目标是速度，输出是"目标倾角"。标准负反馈，用 pid_update()。
     */
    b->dbg_speed_out = pid_update(&b->speed_pid, b->cfg.speed_setpoint,
                                  b->speed_valid, dt);

    lean_target = b->dbg_speed_out;
    if (lean_target > b->cfg.max_lean_deg) {
        lean_target = b->cfg.max_lean_deg;
    }
    if (lean_target < -b->cfg.max_lean_deg) {
        lean_target = -b->cfg.max_lean_deg;
    }
    b->dbg_lean_target = lean_target;

    /*
     * 3) 直立环（内环）：正反馈结构，所以用"实测 - 目标"作为误差。
     *    微分项直接用陀螺仪读数——它本来就是角速度，再求一次导只是放大噪声。
     *    前倾 + 正在往前倒  =>  两个方向都要往前冲，所以 Kd 也是正的。
     */
    angle_err = (angle_deg - trim) - lean_target;
    b->dbg_angle_err = angle_err;
    base = pid_update_ex(&b->angle_pid, angle_err, gyro_dps, dt);
    b->dbg_angle_out = base;

    /*
     * 4) 转向环：偏航角速度误差 -> 左右轮差速。
     *    车在往左转（yaw 为正）而目标是直行，就加大右轮、减小左轮。
     */
    {
        float d_yaw = -(yaw_dps - b->yaw_prev) / dt;
        b->yaw_prev = yaw_dps;
        b->dbg_turn_out = pid_update_ex(&b->turn_pid, 0.0f - yaw_dps, d_yaw, dt);
        turn = b->dbg_turn_out;
    }

    b->pwm_left = base - turn;
    b->pwm_right = base + turn;

    /* 5) 死区补偿：小 PWM 电机根本转不动，干脆补到死区上 */
    for (int i = 0; i < 2; i++) {
        float *p = (i == 0) ? &b->pwm_left : &b->pwm_right;
        if (*p > 0.0f && *p < b->cfg.pwm_deadzone) {
            *p = b->cfg.pwm_deadzone;
        } else if (*p < 0.0f && *p > -b->cfg.pwm_deadzone) {
            *p = -b->cfg.pwm_deadzone;
        }
        if (*p > b->cfg.pwm_max) {
            *p = b->cfg.pwm_max;
        }
        if (*p < -b->cfg.pwm_max) {
            *p = -b->cfg.pwm_max;
        }
    }
}

#include "car_sim.h"

#include <math.h>
#include <string.h>

#define RAD2DEG 57.2957795f

/* xorshift 太麻烦，用一个够用的线性同余 */
static float unit_noise(car_sim_t *c)
{
    c->rng = c->rng * 1103515245u + 12345u;
    return ((float)((c->rng >> 9) & 0x7FFFu) / 16384.0f) - 1.0f;  /* [-1,1) */
}

void car_sim_init(car_sim_t *c)
{
    memset(c, 0, sizeof(*c));
    c->m_body = 1.0f;
    c->m_wheel = 0.12f;
    c->l = 0.16f;
    c->g = 9.81f;
    c->force_per_pwm = 16.0f;
    c->yaw_gain = 4.0f;
    c->yaw_tau = 0.25f;

    /* 13 线编码器 × 4 倍频 × 30 减速比 / 轮周长 0.204m ≈ 7640 counts/m */
    c->counts_per_m = 7640.0f;
    c->acc_noise_deg = 0.06f;
    c->gyro_noise_dps = 0.35f;
    c->enc_noise = 0.4f;
    /*
     * 0.6° 的零位偏差听起来很小，却是"能站住却会跑"的真正原因：
     * 直立环会把真实的机械倾角稳在 −0.6°，车身因此带一个恒定小倾角，
     * 重力分量让它持续加速，10 秒就能跑出好几米。
     */
    c->angle_bias_deg = 0.6f;
    c->rng = 20260925u;
}

void car_sim_step(car_sim_t *c, float pwm, float turn, float dt)
{
    float f;
    float s;
    float co;
    float denom;
    float theta_ddot;
    float x_ddot;

    f = pwm * c->force_per_pwm;
    s = sinf(c->theta);
    co = cosf(c->theta);

    /* 标准小车-倒立摆方程 */
    denom = c->l * (4.0f / 3.0f -
                    (c->m_wheel * co * co) / (c->m_body + c->m_wheel));
    theta_ddot = (c->g * s +
                  co * (-f - c->m_wheel * c->l * c->theta_dot * c->theta_dot * s) /
                      (c->m_body + c->m_wheel)) / denom;
    x_ddot = (f + c->m_wheel * c->l *
                  (c->theta_dot * c->theta_dot * s - theta_ddot * co)) /
             (c->m_body + c->m_wheel);

    /* 半隐式欧拉，dt 足够小时够准 */
    c->theta_dot += theta_ddot * dt;
    c->theta += c->theta_dot * dt;
    c->x_dot += x_ddot * dt;
    c->x += c->x_dot * dt;

    /* 偏航：一阶惯性跟随差速指令 */
    c->yaw_rate += (turn * c->yaw_gain - c->yaw_rate) * (dt / c->yaw_tau);

    /* 编码器：位置量化成计数，再加一点抖动 */
    {
        float raw = c->x * c->counts_per_m + unit_noise(c) * c->enc_noise;
        int32_t q = (int32_t)floorf(raw + 0.5f);
        c->enc_count = q;
    }
}

float car_sim_angle_deg(car_sim_t *c)
{
    return c->theta * RAD2DEG + c->angle_bias_deg +
           unit_noise(c) * c->acc_noise_deg;
}

float car_sim_gyro_dps(car_sim_t *c)
{
    return c->theta_dot * RAD2DEG + unit_noise(c) * c->gyro_noise_dps;
}

float car_sim_true_angle_deg(const car_sim_t *c)
{
    return c->theta * RAD2DEG;
}

float car_sim_true_speed_mps(const car_sim_t *c)
{
    return c->x_dot;
}

float car_sim_true_x(const car_sim_t *c)
{
    return c->x;
}

float car_sim_yaw_dps(const car_sim_t *c)
{
    return c->yaw_rate * RAD2DEG;
}

int32_t car_sim_enc(const car_sim_t *c)
{
    return c->enc_count;
}

/**
 * main.cpp - STM32F103 (Bluepill) + MPU6050 + TB6612 + 双编码器
 *            两轮自平衡小车固件骨架（PlatformIO / Arduino 框架）
 *
 * 这份代码负责"把控制算法接到硬件上"，核心算法在 pid.c / balancer.c 里，
 * 已经用主机端仿真验证过。
 */
#include <Arduino.h>
#include <Wire.h>

extern "C" {
#include "pid.h"
#include "balancer.h"
}

/* ---------------- 硬件引脚 ---------------- */
#define PIN_PWMA     PA8
#define PIN_PWMB     PA11
#define PIN_AIN1     PB12
#define PIN_AIN2     PB13
#define PIN_BIN1     PB14
#define PIN_BIN2     PB15

/* ---------------- 控制周期 ---------------- */
#define CTRL_HZ      200
#define CTRL_DT      (1.0f / (float)CTRL_HZ)
#define CONTROL_MS   (1000 / CTRL_HZ)   /* 5 ms */

static balancer_t g_bal;
static volatile bool g_imu_ready = false;

/* ---------------- MPU6050 最小驱动 ---------------- */
#define MPU_ADDR     0x68
#define REG_PWR      0x6B
#define REG_ACCEL    0x3B
#define REG_GYRO     0x43
#define REG_CONFIG   0x1A

static void mpu_write(uint8_t reg, uint8_t val)
{
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(reg);
    Wire.write(val);
    Wire.endTransmission();
}

static int16_t mpu_read16(uint8_t reg)
{
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom((uint8_t)MPU_ADDR, (uint8_t)2);
    return (int16_t)((Wire.read() << 8) | Wire.read());
}

static void mpu_init(void)
{
    Wire.begin();
    mpu_write(REG_PWR, 0x00);
    /* 陀螺仪 ±500dps，加速度 ±4g，DLPF 约 44Hz */
    mpu_write(0x1B, 0x08);
    mpu_write(0x1C, 0x08);
    mpu_write(REG_CONFIG, 0x03);
}

static void mpu_read(float *angle_deg, float *gyro_dps)
{
    static uint32_t t_prev = 0;
    int16_t ax = mpu_read16(REG_ACCEL);
    int16_t az = mpu_read16(REG_ACCEL + 4);
    int16_t gy = mpu_read16(REG_GYRO + 2);

    uint32_t now = micros();
    float dt = (now - t_prev) * 1e-6f;
    if (dt <= 0.0f || dt > 0.5f) { dt = 1e-3f; }
    t_prev = now;

    /*
     * 这里的倾角只用了最简单的互补滤波，正式做法见本站
     * 《MPU6050 姿态解算》一文（含卡尔曼）。
     */
    static float fused = 0.0f;
    float acc_angle = atan2f(-(float)ax, (float)az) * 57.29578f;
    float gyro_rate = (float)gy / 65.5f;          /* ±500dps -> 65.5 LSB/dps */

    fused = 0.98f * (fused + gyro_rate * dt) + 0.02f * acc_angle;
    *angle_deg = fused;
    *gyro_dps = gyro_rate;
}

/* ---------------- 电机与编码器 ---------------- */
static void motor_set(float left, float right)
{
    int l = (int)constrain(left, -1000.0f, 1000.0f);
    int r = (int)constrain(right, -1000.0f, 1000.0f);

    digitalWrite(PIN_AIN1, l >= 0 ? HIGH : LOW);
    digitalWrite(PIN_AIN2, l >= 0 ? LOW : HIGH);
    digitalWrite(PIN_BIN1, r >= 0 ? HIGH : LOW);
    digitalWrite(PIN_BIN2, r >= 0 ? LOW : HIGH);
    pwmWrite(PIN_PWMA, (uint32_t)abs(l));
    pwmWrite(PIN_PWMB, (uint32_t)abs(r));
}

extern "C" int32_t encoder_read(void);

void setup(void)
{
    Serial.begin(115200);
    pinMode(PIN_AIN1, OUTPUT);
    pinMode(PIN_AIN2, OUTPUT);
    pinMode(PIN_BIN1, OUTPUT);
    pinMode(PIN_BIN2, OUTPUT);
    pinMode(PIN_PWMA, PWM);
    pinMode(PIN_PWMB, PWM);

    mpu_init();
    balancer_init(&g_bal);

    /* 上电后先静止 1 秒，确认方向正确再放开输出 */
    delay(1000);
    Serial.println("balance-car ready, holding at 20% power limit");
    g_bal.cfg.pwm_max = 200.0f;
}

void loop(void)
{
    static uint32_t t_last = 0;
    float angle_deg;
    float gyro_dps;

    if (millis() - t_last < CONTROL_MS) {
        return;
    }
    t_last = millis();

    mpu_read(&angle_deg, &gyro_dps);

    int32_t enc = encoder_read();
    /* 舵机/陀螺仪 Z 轴未接时给 0；有遥控时换成陀螺仪 Z 轴读数 */
    float yaw_dps = 0.0f;

    balancer_step(&g_bal, angle_deg, gyro_dps, enc, yaw_dps, CTRL_DT);
    motor_set(g_bal.pwm_left, g_bal.pwm_right);

    /* 200Hz 跑完留点时间，调试时每 100ms 打一次状态 */
    static uint32_t t_print = 0;
    if (millis() - t_print > 100) {
        t_print = millis();
        Serial.print(angle_deg, 2);
        Serial.print(' ');
        Serial.print(g_bal.speed_valid, 3);
        Serial.print(' ');
        Serial.print(g_bal.dbg_lean_target, 2);
        Serial.print(' ');
        Serial.println(g_bal.pwm_left, 0);
    }
}

#include "pid.h"

#include <string.h>

void pid_init(pid_t *p, float kp, float ki, float kd, float out_min, float out_max)
{
    memset(p, 0, sizeof(*p));
    p->kp = kp;
    p->ki = ki;
    p->kd = kd;
    p->out_min = out_min;
    p->out_max = out_max;
    p->i_min = out_min;   /* 默认积分项不允许超过总输出的范围 */
    p->i_max = out_max;
    p->deadband = 0.0f;
    p->d_lpf_alpha = 0.0f;
    p->anti_windup = true;
}

void pid_set_integ_limits(pid_t *p, float lo, float hi)
{
    p->i_min = lo;
    p->i_max = hi;
}

void pid_set_d_lpf(pid_t *p, float alpha)
{
    if (alpha < 0.0f) {
        alpha = 0.0f;
    }
    if (alpha > 1.0f) {
        alpha = 1.0f;
    }
    p->d_lpf_alpha = alpha;
}

void pid_set_deadband(pid_t *p, float db)
{
    p->deadband = (db < 0.0f) ? -db : db;
}

void pid_set_antiwindup(pid_t *p, bool enable)
{
    p->anti_windup = enable;
}

void pid_reset(pid_t *p, float meas)
{
    p->integ = 0.0f;
    p->prev_meas = meas;
    p->prev_d = 0.0f;
    p->out = 0.0f;
    p->saturated = false;
}

float pid_update_ex(pid_t *p, float error, float d_input, float dt)
{
    float err = error;
    float p_term;
    float d_term;
    float i_term;
    float out;
    int freeze;

    if (dt <= 0.0f) {
        return p->out;
    }

    /* 死区：小误差直接归零，抑制执行器抖动 */
    if (err < p->deadband && err > -p->deadband) {
        err = 0.0f;
    }

    p_term = p->kp * err;

    /*
     * 微分项。这里不自己求导——求导是"噪声放大器"，
     * 而且平衡车的角速度本来就由陀螺仪直接给出，再求一次导只会把噪声放大 200 倍。
     * 调用方决定 d_input 是什么、符号朝哪边。
     */
    d_term = p->kd * d_input;
    if (p->d_lpf_alpha > 0.0f) {
        d_term = p->prev_d + p->d_lpf_alpha * (d_term - p->prev_d);
        p->prev_d = d_term;
    }

    i_term = p->integ + p->ki * err * dt;
    if (i_term > p->i_max) {
        i_term = p->i_max;
    }
    if (i_term < p->i_min) {
        i_term = p->i_min;
    }

    out = p_term + i_term + d_term;

    /*
     * 条件积分（抗积分饱和）：
     * 输出已经顶到上限、误差还在要求更大的输出时，冻结积分；
     * 只要误差方向能把输出拉回来，就允许积分继续累加（甚至回退）。
     * anti_windup = false 时退化成"傻瓜积分"——用来在实验里做对照。
     */
    freeze = 0;
    if (out > p->out_max) {
        out = p->out_max;
        freeze = (err > 0.0f);
    } else if (out < p->out_min) {
        out = p->out_min;
        freeze = (err < 0.0f);
    }

    if (!p->anti_windup) {
        p->integ = i_term;
        p->saturated = (out >= p->out_max) || (out <= p->out_min);
    } else if (freeze) {
        p->saturated = true;
    } else {
        p->integ = i_term;
        p->saturated = false;
    }

    p->out = out;
    return out;
}

float pid_update(pid_t *p, float setpoint, float meas, float dt)
{
    float d_input;

    if (dt <= 0.0f) {
        return p->out;
    }
    /* 微分先行：对测量求导并取负 */
    d_input = -(meas - p->prev_meas) / dt;
    p->prev_meas = meas;
    return pid_update_ex(p, setpoint - meas, d_input, dt);
}

/**
 * 主机端测试：用倒立摆仿真验证三环串级 PID
 *
 * gcc -std=c99 -Wall -Wextra -Iinclude src/pid.c src/car_sim.c src/balancer.c \
 *     test/test_balance.c -o build/test -lm
 */
#include <math.h>
#include <stdio.h>
#include <string.h>

#include "balancer.h"
#include "car_sim.h"

#define DT_SIM    0.0002f            /* 物理积分步长 0.2ms */
#define DT_CTRL   0.005f             /* 控制周期 5ms (200Hz) */
#define SUBSTEPS  25                 /* DT_CTRL / DT_SIM */
#define RAD2DEG   57.2957795f

typedef struct {
    balancer_t bal;
    car_sim_t sim;
} rig_t;

/* 仿真选项：不想改的字段填默认值 */
typedef struct {
    float angle_kp;        /* 0 = 用默认 */
    float angle_kd;        /* 0 = 用默认 */
    float speed_ki;        /* <0 = 用默认 */
    float speed_i_limit;   /* 0 = 用默认（±max_lean） */
    int speed_loop;        /* 0 = 关掉速度环 */
} opt_t;

typedef struct {
    float recover_t;      /* 倾角回到 ±1° 且之后不再超出所需时间；-1 表示没恢复 */
    float max_abs_deg;
    float mean_deg;       /* 最后 3 秒的 |倾角| 均值（= 稳态倾角大小） */
    float rms_deg;        /* 最后 3 秒**去掉均值**后的 RMS，衡量"抖不抖" */
    float pos_rms;        /* 最后 5 秒的位置 RMS，衡量"定得稳不稳" */
    float final_x;        /* 结束时位置 m */
    float max_abs_x;
    int fallen;           /* |倾角| > 60° */
    float pwm_peak;
} result_t;

static void rig_init(rig_t *r, float init_deg, const opt_t *o)
{
    opt_t d;

    if (o == NULL) {
        memset(&d, 0, sizeof(d));
        d.speed_loop = 1;
        d.speed_ki = -1.0f;
        o = &d;
    }

    car_sim_init(&r->sim);
    r->sim.theta = init_deg / RAD2DEG;
    r->sim.theta_dot = 0.0f;

    balancer_init(&r->bal);
    if (o->angle_kp > 0.0f) {
        r->bal.cfg.angle_kp = o->angle_kp;
    }
    if (o->angle_kd > 0.0f) {
        r->bal.cfg.angle_kd = o->angle_kd;
    }
    if (o->speed_ki >= 0.0f) {
        r->bal.cfg.speed_ki = o->speed_ki;
    }
    if (!o->speed_loop) {
        r->bal.cfg.speed_kp = 0.0f;
        r->bal.cfg.speed_ki = 0.0f;
    }

    /* 按最终 cfg 重建（或重新限幅）三个 PID */
    pid_init(&r->bal.angle_pid, r->bal.cfg.angle_kp, 0.0f, r->bal.cfg.angle_kd,
             -r->bal.cfg.pwm_max, r->bal.cfg.pwm_max);
    pid_set_integ_limits(&r->bal.angle_pid, -150.0f, 150.0f);

    pid_init(&r->bal.speed_pid, r->bal.cfg.speed_kp, r->bal.cfg.speed_ki, 0.0f,
             -r->bal.cfg.max_lean_deg, r->bal.cfg.max_lean_deg);
    if (o->speed_i_limit > 0.0f) {
        pid_set_integ_limits(&r->bal.speed_pid, -o->speed_i_limit,
                             o->speed_i_limit);
    }

    balancer_reset(&r->bal, init_deg, 0.0f);
}

/* 跑一次仿真；push_n 为在 push_at 时刻施加的水平推力冲量(N·s) */
static result_t simulate(rig_t *r, float init_deg, float push_n, float push_at,
                         float t_end, const opt_t *o)
{
    result_t res;
    float t = 0.0f;
    float rms_acc = 0.0f;
    float rms_sum = 0.0f;
    float pos_acc = 0.0f;
    float recover_from;
    int rms_n = 0;
    int pos_n = 0;
    int recovered = 0;

    memset(&res, 0, sizeof(res));
    res.recover_t = -1.0f;

    /* 有扰动时，只统计扰动之后的恢复过程 */
    recover_from = (push_n > 0.0f) ? push_at : 0.0f;

    rig_init(r, init_deg, o);

    while (t < t_end) {
        float angle = car_sim_angle_deg(&r->sim);
        float gyro = car_sim_gyro_dps(&r->sim);
        float yaw = car_sim_yaw_dps(&r->sim);
        float pwm;
        int32_t enc = car_sim_enc(&r->sim);
        int i;

        balancer_step(&r->bal, angle, gyro, enc, yaw, DT_CTRL);
        pwm = r->bal.pwm_left / r->bal.cfg.pwm_max;

        /* 脉冲扰动：模拟被人从后面推一下 */
        if (push_n > 0.0f && t >= push_at && t < push_at + DT_CTRL) {
            float dv = push_n / (r->sim.m_body + r->sim.m_wheel);
            r->sim.x_dot += dv * 0.25f;
            r->sim.theta_dot += dv * 0.35f;
        }

        for (i = 0; i < SUBSTEPS; i++) {
            car_sim_step(&r->sim, pwm,
                         r->bal.dbg_turn_out / r->bal.cfg.pwm_max, DT_SIM);
        }
        t += DT_CTRL;

        {
            float ad = fabsf(car_sim_true_angle_deg(&r->sim));
            float ax = fabsf(car_sim_true_x(&r->sim));
            if (ad > res.max_abs_deg) {
                res.max_abs_deg = ad;
            }
            if (ax > res.max_abs_x) {
                res.max_abs_x = ax;
            }
            if (fabsf(r->bal.pwm_left) > res.pwm_peak) {
                res.pwm_peak = fabsf(r->bal.pwm_left);
            }
            if (ad > 60.0f) {
                res.fallen = 1;
            }
            if (t > recover_from + 0.1f) {
                if (ad > 1.0f) {
                    recovered = 0;
                } else if (!recovered) {
                    recovered = 1;
                    res.recover_t = t;
                }
            }
            if (t > t_end - 3.0f) {
                rms_acc += ad * ad;
                rms_sum += ad;
                rms_n++;
            }
            if (t > t_end - 5.0f) {
                float xx = car_sim_true_x(&r->sim);
                pos_acc += xx * xx;
                pos_n++;
            }
        }
        if (res.fallen) {
            break;
        }
    }

    if (rms_n > 0) {
        float mean = rms_sum / (float)rms_n;
        float var = rms_acc / (float)rms_n - mean * mean;
        res.mean_deg = mean;
        res.rms_deg = (var > 0.0f) ? sqrtf(var) : 0.0f;
    }
    if (pos_n > 0) {
        res.pos_rms = sqrtf(pos_acc / (float)pos_n);
    }
    res.final_x = car_sim_true_x(&r->sim);
    return res;
}

int main(void)
{
    rig_t rig;
    opt_t o;
    result_t r_angle_only;
    result_t r_cascade;
    int failed = 0;

    printf("===== 两轮自平衡小车：三环串级 PID 仿真实测 =====\n");
    printf("物理步长 %.1fms，控制周期 %.0fHz，倒立摆模型 + IMU 噪声 + 编码器量化\n\n",
           DT_SIM * 1000.0f, 1.0f / DT_CTRL);

    /* ---------- [1] 只有直立环 ---------- */
    printf("[1] 只开直立环（速度环 Kp=Ki=0），初始倾角 +3°，跑 10 秒\n");
    memset(&o, 0, sizeof(o));
    o.speed_loop = 0;
    r_angle_only = simulate(&rig, 3.0f, 0.0f, 0.0f, 10.0f, &o);
    printf("    最终位置 %+.3f m，最大偏移 %.3f m，稳态倾角 %.3f°，抖动 RMS %.3f°，PWM 峰值 %.0f\n",
           r_angle_only.final_x, r_angle_only.max_abs_x, r_angle_only.mean_deg,
           r_angle_only.rms_deg, r_angle_only.pwm_peak);
    printf("    -> 角度确实稳住了（抖动 RMS 只有 %.3f°），但车以约 %.2f m/s 匀速跑掉\n",
           r_angle_only.rms_deg, fabsf(r_angle_only.final_x) / 10.0f);
    printf("       注意稳态倾角 %.2f°：零位偏差本身只有 0.6°，是有限 P 增益必然留下的\n",
           r_angle_only.mean_deg);
    printf("       稳态误差把它放大到了 %.2f°，这份额外倾角又变成了持续的加速度\n",
           r_angle_only.mean_deg);
    printf("       这就是「能站住却会跑」：直立环只约束角度，完全不约束位置\n");
    if (fabsf(r_angle_only.final_x) < 0.5f) {
        printf("    !! 预期应明显漂移，实际没有，增益或模型有问题\n");
        failed++;
    }

    /* ---------- [2] 加上速度环 ---------- */
    printf("\n[2] 加上速度环（串级），同样初始倾角 +3°\n");
    memset(&o, 0, sizeof(o));
    o.speed_loop = 1;
    o.speed_ki = -1.0f;
    r_cascade = simulate(&rig, 3.0f, 0.0f, 0.0f, 10.0f, &o);
    printf("    最终位置 %+.3f m，最大偏移 %.3f m，倾角 RMS %.3f°，PWM 峰值 %.0f\n",
           r_cascade.final_x, r_cascade.max_abs_x, r_cascade.rms_deg,
           r_cascade.pwm_peak);
    printf("    -> 位置被压回原点附近，|漂移| 从 %.2f m 降到 %.3f m（%.0f 倍）\n",
           fabsf(r_angle_only.final_x), fabsf(r_cascade.final_x),
           fabsf(r_angle_only.final_x) / (fabsf(r_cascade.final_x) + 1e-6f));
    if (fabsf(r_cascade.final_x) > 0.4f) {
        failed++;
    }

    /* ---------- [3] 抗扰 ---------- */
    printf("\n[3] 抗扰：第 4 秒从后面推一下（冲量 0.9 N·s）\n");
    {
        result_t r = simulate(&rig, 0.0f, 0.9f, 4.0f, 12.0f, &o);
        printf("    最大倾角 %.2f°，扰动后 %.2f s 回到 ±1° 内\n",
               r.max_abs_deg, r.recover_t - 4.0f);
        printf("    最终位置 %+.3f m，最后 5 秒位置 RMS %.3f m，抖动 RMS %.3f°\n",
               r.final_x, r.pos_rms, r.rms_deg);
        printf("    -> 被推之后车身先顺着倒让开，速度环再把位置拉回来，\n");
        printf("       这是平衡车抗扰的标准动作：内环保安全，外环管位置\n");
        if (r.fallen || r.recover_t < 0.0f || (r.recover_t - 4.0f) > 10.0f) {
            failed++;
        }
    }

    /* ---------- [4] 直立环先调 P、再加 D ---------- */
    printf("\n[4] 直立环整定：先 P 后 D（速度环关闭，初始倾角 +3°，跑 10 秒）\n");
    printf("    A) 只用 P（Kd = 0）\n");
    printf("       Kp   稳态倾角(°)  抖动RMS(°)  最大倾角(°)  PWM峰值  判定\n");
    {
        const float kps[] = {4.0f, 8.0f, 15.0f, 25.0f, 50.0f};
        for (unsigned i = 0; i < sizeof(kps) / sizeof(kps[0]); i++) {
            result_t r;
            memset(&o, 0, sizeof(o));
            o.speed_loop = 0;
            o.angle_kp = kps[i];
            o.angle_kd = 0.01f;          /* 0.01 表示"几乎就是 0" */
            r = simulate(&rig, 3.0f, 0.0f, 0.0f, 10.0f, &o);
            printf("    %5.0f  %10.3f  %10.3f  %11.2f  %8.0f  %s\n",
                   kps[i], r.mean_deg, r.rms_deg, r.max_abs_deg, r.pwm_peak,
                   r.fallen ? "倒下" : (r.rms_deg < 0.3f ? "稳定" : "等幅振荡"));
        }
        printf("    -> Kp 太小时回正力矩打不过重力分量，车身直接倒；\n");
        printf("       Kp 够大以后能「稳住」，但因为没有阻尼，会一直等幅振荡下去\n");
    }
    printf("    B) 固定 Kp = 25，加 D\n");
    printf("       Kd   稳态倾角(°)  抖动RMS(°)  最大倾角(°)  PWM峰值  判定\n");
    {
        const float kds[] = {0.01f, 0.3f, 0.8f, 1.35f, 3.0f, 6.0f};
        for (unsigned i = 0; i < sizeof(kds) / sizeof(kds[0]); i++) {
            result_t r;
            memset(&o, 0, sizeof(o));
            o.speed_loop = 0;
            o.angle_kp = 25.0f;
            o.angle_kd = kds[i];
            r = simulate(&rig, 3.0f, 0.0f, 0.0f, 10.0f, &o);
            printf("    %5.2f  %10.3f  %10.3f  %11.2f  %8.0f  %s\n",
                   kds[i], r.mean_deg, r.rms_deg, r.max_abs_deg, r.pwm_peak,
                   r.fallen ? "倒下" : (r.rms_deg < 0.3f ? "稳定" : "残余振荡"));
        }
        printf("    -> D 才是让车身真正「停下来」的那一项。Kd 不足就一直晃，\n");
        printf("       Kd 过大则把陀螺仪噪声放大成高频抖动（实测里能看到 PWM 峰值抬高）\n");
    }

    /* ---------- [5] 速度环 Ki 的取舍 ---------- */
    printf("\n[5] 速度环 Ki 的取舍（Kp 固定 0.25，初始倾角 +3°，跑 12 秒）\n");
    printf("       Ki  位置RMS(m)  抖动RMS(°)  PWM峰值  判定\n");
    {
        const float kis[] = {0.0f, 0.5f, 1.0f, 2.0f, 4.0f, 8.0f};
        float best = 1.0e9f;
        for (unsigned i = 0; i < sizeof(kis) / sizeof(kis[0]); i++) {
            result_t r;
            memset(&o, 0, sizeof(o));
            o.speed_loop = 1;
            o.speed_ki = kis[i];
            r = simulate(&rig, 3.0f, 0.0f, 0.0f, 12.0f, &o);
            printf("    %5.2f  %10.3f  %10.3f  %8.0f  %s\n",
                   kis[i], r.pos_rms, r.rms_deg, r.pwm_peak,
                   r.fallen ? "倒下"
                            : (r.rms_deg > 1.0f ? "车身开始晃"
                                                : (r.pos_rms < 0.5f ? "兼顾"
                                                                    : "位置没拉回来")));
            if (kis[i] > 0.0f && r.pos_rms < best && r.rms_deg < 1.0f) {
                best = r.pos_rms;
            }
        }
        printf("    -> Ki=0 时速度环只剩 P，位置压不回原点（P 必须靠剩余速度产生误差）；\n");
        printf("       Ki 越大位置回得越稳，但外环带宽逼近内环之后车身开始晃——\n");
        printf("       这就是「外环必须比内环慢 3~5 倍」的由来。\n");
        printf("       代价是被推之后要好几秒才把位置拉回来，这不是 bug，是取舍。\n");
        if (best > 1.0e8f) {
            printf("    !! 没有任何 Ki 能把位置压到 0.25 m 内\n");
            failed++;
        }
    }

    /* ---------- [6] 抗积分饱和到底在防什么 ---------- */
    printf("\n[6] 抗积分饱和：用一个「一阶惯性 + 限幅执行器」把现象放大出来\n");
    {
        /*
         * 场景：执行器输出被限死在 0~50，但一开始给的目标是 100（根本达不到）。
         * 误差会一直存在，积分项就一路涨。3 秒后把目标降到 30（可达），
         * 看两种实现谁能及时"松手"。
         * 用纯积分控制器（Kp=0）是为了让现象最干净：输出就等于积分项。
         */
        const float TAU = 1.0f;
        const float U_MAX = 50.0f;
        const float KI = 2.0f;
        const float DT = 0.002f;
        float t = 0.0f;
        float y_aw = 0.0f;
        float y_no = 0.0f;
        float y_aw_at3 = 0.0f;
        float y_no_at3 = 0.0f;
        float y_aw_end = 0.0f;
        float y_no_end = 0.0f;
        float t_aw = -1.0f;
        float t_no = -1.0f;
        pid_t p_aw;
        pid_t p_no;

        pid_init(&p_aw, 0.0f, KI, 0.0f, 0.0f, U_MAX);
        pid_set_integ_limits(&p_aw, 0.0f, U_MAX);        /* 积分项不得超过输出范围 */
        pid_set_antiwindup(&p_aw, true);
        pid_init(&p_no, 0.0f, KI, 0.0f, 0.0f, U_MAX);
        pid_set_integ_limits(&p_no, -1.0e9f, 1.0e9f);    /* 积分不限幅 */
        pid_set_antiwindup(&p_no, false);                /* 并关掉条件积分 */

        while (t < 12.0f) {
            float sp = (t < 3.0f) ? 100.0f : 30.0f;
            float u_aw = pid_update(&p_aw, sp, y_aw, DT);
            float u_no = pid_update(&p_no, sp, y_no, DT);

            y_aw += (u_aw - y_aw) * (DT / TAU);
            y_no += (u_no - y_no) * (DT / TAU);
            t += DT;

            if (t >= 3.0f && y_aw_at3 == 0.0f) {
                y_aw_at3 = y_aw;
                y_no_at3 = y_no;
            }
            if (t > 3.0f) {
                if (t_aw < 0.0f && y_aw < 31.5f) {
                    t_aw = t;
                }
                if (t_no < 0.0f && y_no < 31.5f) {
                    t_no = t;
                }
            }
            y_aw_end = y_aw;
            y_no_end = y_no;
        }

        printf("    对象 tau=1.0s，执行器限幅 0~%.0f，目标 100（不可达）3 秒后降到 30\n",
               U_MAX);
        printf("    有抗饱和：第 3 秒输出 %.1f，12 秒后 %.1f，降到 31.5 用了 %s\n",
               y_aw_at3, y_aw_end,
               (t_aw < 0.0f) ? "一直没降到" : "几秒");
        printf("    无抗饱和：第 3 秒输出 %.1f，12 秒后 %.1f，降到 31.5 用了 %s\n",
               y_no_at3, y_no_end,
               (t_no < 0.0f) ? "一直没降到" : "几秒");
        if (t_aw > 0.0f) {
            printf("    有抗饱和的具体时间：%.2f s（目标下调后 %.2f s 跟上）\n",
                   t_aw, t_aw - 3.0f);
        }
        if (t_no > 0.0f) {
            printf("    无抗饱和的具体时间：%.2f s（目标下调后 %.2f s 才跟上）\n",
                   t_no, t_no - 3.0f);
        }
        printf("    -> 输出顶到限幅时误差还在让积分继续涨，等目标降下来，\n");
        printf("       积分「欠的债」只能靠反向误差一点点吐出来——这就是积分饱和。\n");
        printf("       平衡车的速度环正是这个结构：车身被墙/坡顶住时误差不消失，\n");
        printf("       没有抗饱和，等你把车挪开的瞬间它就会猛冲出去\n");
        if (t_aw < 0.0f) {
            printf("    !! 有抗饱和也没能恢复，参数需要再看\n");
            failed++;
        }
    }

    printf("\n===== %s =====\n",
           failed == 0 ? "全部通过：以上数据由本机 gcc 实编译实运行（纯仿真，无需硬件）"
                       : "有失败项！");
    return failed == 0 ? 0 : 1;
}

===== 两轮自平衡小车：三环串级 PID 仿真实测 =====
物理步长 0.2ms，控制周期 200Hz，倒立摆模型 + IMU 噪声 + 编码器量化

[1] 只开直立环（速度环 Kp=Ki=0），初始倾角 +3°，跑 10 秒
    最终位置 -9.165 m，最大偏移 9.165 m，稳态倾角 1.151°，抖动 RMS 0.010°，PWM 峰值 90
    -> 角度确实稳住了（抖动 RMS 只有 0.010°），但车以约 0.92 m/s 匀速跑掉
       注意稳态倾角 1.15°：零位偏差本身只有 0.6°，是有限 P 增益必然留下的
       稳态误差把它放大到了 1.15°，这份额外倾角又变成了持续的加速度
       这就是「能站住却会跑」：直立环只约束角度，完全不约束位置

[2] 加上速度环（串级），同样初始倾角 +3°
    最终位置 -0.335 m，最大偏移 0.641 m，倾角 RMS 0.411°，PWM 峰值 90
    -> 位置被压回原点附近，|漂移| 从 9.17 m 降到 0.335 m（27 倍）

[3] 抗扰：第 4 秒从后面推一下（冲量 0.9 N·s）
    最大倾角 2.54°，扰动后 7.84 s 回到 ±1° 内
    最终位置 -0.484 m，最后 5 秒位置 RMS 0.569 m，抖动 RMS 0.417°
    -> 被推之后车身先顺着倒让开，速度环再把位置拉回来，
       这是平衡车抗扰的标准动作：内环保安全，外环管位置

[4] 直立环整定：先 P 后 D（速度环关闭，初始倾角 +3°，跑 10 秒）
    A) 只用 P（Kd = 0）
       Kp   稳态倾角(°)  抖动RMS(°)  最大倾角(°)  PWM峰值  判定
        4       0.000       0.000        60.03       239  倒下
        8       0.000       0.000        60.15       479  倒下
       15       6.516       3.827        13.56       195  等幅振荡
       25       6.733       3.455        12.97       310  等幅振荡
       50      18.909       9.981        39.85      1000  等幅振荡
    -> Kp 太小时回正力矩打不过重力分量，车身直接倒；
       Kp 够大以后能「稳住」，但因为没有阻尼，会一直等幅振荡下去
    B) 固定 Kp = 25，加 D
       Kd   稳态倾角(°)  抖动RMS(°)  最大倾角(°)  PWM峰值  判定
     0.01       6.733       3.455        12.97       310  残余振荡
     0.30       1.160       0.055         4.51        99  稳定
     0.80       1.152       0.013         3.26        90  稳定
     1.35       1.151       0.010         3.00        90  稳定
     3.00       1.150       0.007         3.00        89  稳定
     6.00       1.150       0.006         3.00        89  稳定
    -> D 才是让车身真正「停下来」的那一项。Kd 不足就一直晃，
       Kd 过大则把陀螺仪噪声放大成高频抖动（实测里能看到 PWM 峰值抬高）

[5] 速度环 Ki 的取舍（Kp 固定 0.25，初始倾角 +3°，跑 12 秒）
       Ki  位置RMS(m)  抖动RMS(°)  PWM峰值  判定
     0.00       6.924       0.037        90  位置没拉回来
     0.50       1.913       0.261        90  位置没拉回来
     1.00       0.520       0.190        90  位置没拉回来
     2.00       0.364       0.432        90  兼顾
     4.00       0.262       0.750        90  兼顾
     8.00       0.381       3.006       145  车身开始晃
    -> Ki=0 时速度环只剩 P，位置压不回原点（P 必须靠剩余速度产生误差）；
       Ki 越大位置回得越稳，但外环带宽逼近内环之后车身开始晃——
       这就是「外环必须比内环慢 3~5 倍」的由来。
       代价是被推之后要好几秒才把位置拉回来，这不是 bug，是取舍。

[6] 抗积分饱和：用一个「一阶惯性 + 限幅执行器」把现象放大出来
    对象 tau=1.0s，执行器限幅 0~50，目标 100（不可达）3 秒后降到 30
    有抗饱和：第 3 秒输出 47.2，12 秒后 30.1，降到 31.5 用了 几秒
    无抗饱和：第 3 秒输出 47.2，12 秒后 50.0，降到 31.5 用了 一直没降到
    有抗饱和的具体时间：4.42 s（目标下调后 1.42 s 跟上）
    -> 输出顶到限幅时误差还在让积分继续涨，等目标降下来，
       积分「欠的债」只能靠反向误差一点点吐出来——这就是积分饱和。
       平衡车的速度环正是这个结构：车身被墙/坡顶住时误差不消失，
       没有抗饱和，等你把车挪开的瞬间它就会猛冲出去

===== 全部通过：以上数据由本机 gcc 实编译实运行（纯仿真，无需硬件） =====